├── LICENSE ├── README.md ├── sm_nav2_test_1 ├── CMakeLists.txt ├── README.md ├── config │ ├── mapper_params_online_sync.yaml │ ├── navigation_tree.xml │ ├── params_isaac_sim.yaml │ ├── rtx_carter_navigation_params.yaml │ ├── rtx_rviz_conf.rviz │ ├── sm_nav2_test_1_config.yaml │ └── waypoints_warehouse_1.yaml ├── docs │ ├── SmNav2Test1_2024-9-7_222547.svg │ └── SmNav2Test1_2024-9-7_222555.pdf ├── include │ └── sm_nav2_test_1 │ │ ├── modestates │ │ ├── ms_nav2_test_1_recovery_mode.hpp │ │ └── ms_nav2_test_1_run_mode.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_1.hpp │ │ ├── states │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_back_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_right_1.hpp │ │ └── st_switch_yard.hpp │ │ └── superstates │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── __pycache__ │ │ └── isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc │ ├── bringup_launch.py │ ├── carter_navigation_rtx.launch.py │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── navigation_launch.py │ ├── online_sync_launch.py │ ├── rviz_launch.py │ ├── slam_launch.py │ └── sm_nav2_test_1_launch.py ├── maps │ ├── carter_warehouse_navigation.png │ ├── carter_warehouse_navigation.yaml │ ├── improved_carter_warehouse_navigation.png │ └── improved_carter_warehouse_navigation.yaml ├── package.xml ├── scripts │ ├── __pycache__ │ │ └── isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc │ └── points_publisher.sh └── src │ └── sm_nav2_test_1 │ └── sm_nav2_test_1.cpp ├── sm_nav2_test_2 ├── CMakeLists.txt ├── README.md ├── config │ ├── mapper_params_online_sync.yaml │ ├── navigation_tree.xml │ ├── params_isaac_sim.yaml │ ├── rtx_carter_navigation_params.yaml │ ├── rtx_rviz_conf.rviz │ ├── sm_nav2_test_2_config.yaml │ └── waypoints_warehouse_1.yaml ├── docs │ ├── SmNav2Test2_2024-11-11_231713.pdf │ └── SmNav2Test2_2024-11-11_231727.svg ├── include │ └── sm_nav2_test_2 │ │ ├── modestates │ │ ├── ms_nav2_test_2_recovery_mode.hpp │ │ └── ms_nav2_test_2_run_mode.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_2.hpp │ │ ├── states │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── bringup_launch.py │ ├── carter_navigation_rtx.launch.py │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── navigation_launch.py │ ├── online_sync_launch.py │ ├── rviz_launch.py │ ├── slam_launch.py │ └── sm_nav2_test_2_launch.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ └── lidar_completion.py └── src │ └── sm_nav2_test_2 │ └── sm_nav2_test_2.cpp ├── sm_nav2_test_3 ├── CMakeLists.txt ├── README.md ├── config │ ├── mapper_params_online_sync.yaml │ ├── navigation_tree.xml │ ├── params_isaac_sim.yaml │ ├── rtx_carter_navigation_params.yaml │ ├── rtx_rviz_conf.rviz │ ├── sm_nav2_test_3_config.yaml │ └── waypoints_warehouse_1.yaml ├── docs │ ├── SmNav2Test3_2024-9-7_222147.pdf │ └── SmNav2Test3_2024-9-7_222225.svg ├── include │ └── sm_nav2_test_3 │ │ ├── modestates │ │ ├── ms_nav2_test_3_recovery_mode.hpp │ │ └── ms_nav2_test_3_run_mode.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_3.hpp │ │ ├── states │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── bringup_launch.py │ ├── carter_navigation_rtx.launch.py │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── navigation_launch.py │ ├── online_sync_launch.py │ ├── rviz_launch.py │ ├── slam_launch.py │ └── sm_nav2_test_3_launch.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ └── lidar_completion.py └── src │ └── sm_nav2_test_3 │ └── sm_nav2_test_3.cpp ├── sm_nav2_test_4 ├── CMakeLists.txt ├── README.md ├── config │ ├── foundationpose_isaac_sim.rviz │ ├── mapper_params_online_sync.yaml │ ├── navigation_tree.xml │ ├── params_isaac_sim.yaml │ ├── rtx_carter_navigation_params.yaml │ ├── rtx_rviz_conf.rviz │ ├── rtx_rviz_conf2.rviz │ ├── sm_nav2_test_4_config.yaml │ └── waypoints_warehouse_1.yaml ├── docs │ ├── SmNav2Test4_2024-9-7_222147.pdf │ └── SmNav2Test4_2024-9-7_222225.svg ├── include │ └── sm_nav2_test_4 │ │ ├── clients │ │ └── cl_april_tag_detector │ │ │ ├── cl_april_tag_detector.hpp │ │ │ └── client_behaviors │ │ │ └── cb_detect_apriltag.hpp │ │ ├── modestates │ │ ├── ms_nav2_test_4_recovery_mode.hpp │ │ ├── ms_nav2_test_4_run_mode.hpp │ │ └── ms_recover.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_4.hpp │ │ ├── states │ │ ├── docking_sequence_states │ │ │ ├── st_recover_step_1.hpp │ │ │ ├── st_recover_step_2.hpp │ │ │ ├── st_recover_step_3.hpp │ │ │ ├── st_recover_step_4.hpp │ │ │ ├── st_recover_step_5.hpp │ │ │ ├── st_recover_step_6.hpp │ │ │ ├── st_recover_step_7.hpp │ │ │ └── st_recover_step_8.hpp │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_launch_vision_pipeline.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_docking_sequence_1.hpp │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── bringup_launch.py │ ├── carter_navigation_rtx.launch.py │ ├── extras │ │ ├── fp_sim.launch.py │ │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ │ └── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ ├── isaac_ros_foundationpose_isaac_sim1.launch.py │ ├── isaac_ros_foundationpose_isaac_sim2.launch.py │ ├── isaac_ros_foundationpose_isaac_sim_tracking.launch.py │ ├── navigation_launch.py │ ├── online_sync_launch.py │ ├── rviz_launch.py │ ├── slam_launch.py │ ├── sm_nav2_test_4_launch - original.py │ └── sm_nav2_test_4_launch.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ ├── fp_dock_pose_publisher.py │ └── lidar_completion.py └── src │ └── sm_nav2_test_4 │ └── sm_nav2_test_4.cpp ├── sm_nav2_test_5 ├── CMakeLists.txt ├── README.md ├── config │ ├── mapper_params_online_sync.yaml │ ├── navigation_tree.xml │ ├── params_isaac_sim.yaml │ ├── rtx_carter_navigation_params.yaml │ ├── rtx_rviz_conf.rviz │ ├── sm_nav2_test_5_config.yaml │ └── waypoints_warehouse_1.yaml ├── docs │ ├── SmNav2Test5_2024-9-7_222147.pdf │ └── SmNav2Test5_2024-9-7_222225.svg ├── include │ └── sm_nav2_test_5 │ │ ├── modestates │ │ ├── ms_nav2_test_5_recovery_mode.hpp │ │ └── ms_nav2_test_5_run_mode.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_5.hpp │ │ ├── states │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── bringup_launch.py │ ├── carter_navigation_rtx.launch.py │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── navigation_launch.py │ ├── online_sync_launch.py │ ├── rviz_launch.py │ ├── slam_launch.py │ └── sm_nav2_test_5_launch.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ └── lidar_completion.py └── src │ └── sm_nav2_test_5 │ └── sm_nav2_test_5.cpp ├── sm_nav2_test_6 ├── CMakeLists.txt ├── README.md ├── config │ ├── default_nav_to_pose_bt.xml │ ├── extras │ │ ├── foundationpose_isaac_sim.rviz │ │ ├── params_isaac_sim.yaml │ │ ├── rtx_rviz_conf copy 2.rviz │ │ ├── rtx_rviz_conf copy.rviz │ │ └── rtx_rviz_conf2.rviz │ ├── nav2_config.yaml │ ├── navigation_waypoints_1.yaml │ ├── rviz_config.rviz │ ├── slam_toolbox_config.yaml │ └── sm_nav2_test_6_config.yaml ├── docs │ ├── SmNav2Test6_2024-9-7_222147.pdf │ └── SmNav2Test6_2024-9-7_222225.svg ├── include │ └── sm_nav2_test_6 │ │ ├── clients │ │ ├── cl_april_tag_detector │ │ │ ├── cl_april_tag_detector.hpp │ │ │ └── client_behaviors │ │ │ │ └── cb_detect_apriltag.hpp │ │ └── cl_foundationpose │ │ │ ├── cl_foundationpose.hpp │ │ │ └── components │ │ │ └── cp_object_tracker_1.hpp │ │ ├── modestates │ │ ├── ms_nav2_test_4_recovery_mode.hpp │ │ ├── ms_nav2_test_4_run_mode.hpp │ │ └── ms_recover.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_navigation.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_6.hpp │ │ ├── states │ │ ├── docking_sequence_states │ │ │ ├── st_recover_step_1.hpp │ │ │ ├── st_recover_step_2.hpp │ │ │ ├── st_recover_step_3.hpp │ │ │ ├── st_recover_step_4.hpp │ │ │ ├── st_recover_step_5.hpp │ │ │ ├── st_recover_step_6.hpp │ │ │ ├── st_recover_step_7.hpp │ │ │ └── st_recover_step_8.hpp │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_acquire_sensors.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_launch_vision_pipeline.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_docking_sequence_1.hpp │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── extras │ │ ├── carter_navigation_rtx.launch original.py │ │ ├── carter_navigation_rtx.launch.py │ │ ├── fp_sim.launch.py │ │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim1.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim2.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim_tracking.launch.py │ │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ │ └── sm_nav2_test_6_launch - original.py │ ├── nav2_bringup_launch.py │ ├── nav2_launch.py │ ├── nav2_stack_launch.py │ ├── nav2_stack_launch2.py │ ├── rviz_launch.py │ ├── slam_stack_launch.py │ ├── slam_toolbox_launch.py │ ├── sm_nav2_test_6_launch.py │ ├── vision_pipeline_launch.py │ └── vision_pipeline_launch2.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ ├── extras │ │ └── fp_dock_pose_publisher.py │ └── lidar_completion.py └── src │ └── sm_nav2_test_6 │ └── sm_nav2_test_6.cpp ├── sm_nav2_test_7 ├── CMakeLists.txt ├── README.md ├── config │ ├── default_nav_to_pose_bt.xml │ ├── extras │ │ ├── foundationpose_isaac_sim.rviz │ │ ├── params_isaac_sim.yaml │ │ ├── rtx_rviz_conf copy 2.rviz │ │ ├── rtx_rviz_conf copy.rviz │ │ └── rtx_rviz_conf2.rviz │ ├── nav2_config.yaml │ ├── navigation_waypoints_1.yaml │ ├── rviz_config.rviz │ ├── slam_toolbox_config.yaml │ └── sm_nav2_test_7_config.yaml ├── docs │ ├── SmNav2Test7_2025-4-11_224818.svg │ └── SmNav2Test7_2025-4-11_22484.pdf ├── include │ └── sm_nav2_test_7 │ │ ├── clients │ │ ├── cl_april_tag_detector │ │ │ ├── cl_april_tag_detector.hpp │ │ │ ├── client_behaviors │ │ │ │ └── cb_detect_apriltag.hpp │ │ │ └── components │ │ │ │ └── cp_april_visualization.hpp │ │ ├── cl_foundationpose │ │ │ ├── cl_foundationpose.hpp │ │ │ ├── client_behaviors │ │ │ │ ├── cb_pause_object_tracking.hpp │ │ │ │ └── cb_track_object_pose.hpp │ │ │ └── components │ │ │ │ ├── cp_object_tracker_1.hpp │ │ │ │ ├── cp_object_tracker_tf.hpp │ │ │ │ └── tracker_utils.hpp │ │ ├── cl_lidar │ │ │ ├── cl_lidar.hpp │ │ │ ├── client_behaviors │ │ │ │ └── cb_lidar_sensor.hpp │ │ │ └── components │ │ │ │ └── cp_forward_obstacle_detector.hpp │ │ ├── cl_mission_tracker │ │ │ ├── cl_mission_tracker.hpp │ │ │ └── client_behaviors │ │ │ │ └── cb_battery_decission.hpp │ │ └── cl_nav2z │ │ │ └── client_behaviors │ │ │ ├── cb_position_control_free_space.hpp │ │ │ ├── cb_spiral_motion.hpp │ │ │ ├── cb_track_path_odometry.hpp │ │ │ └── cb_track_path_slam.hpp │ │ ├── modestates │ │ ├── ms_nav2_test_4_recovery_mode.hpp │ │ ├── ms_nav2_test_4_run_mode.hpp │ │ └── ms_recover.hpp │ │ ├── orthogonals │ │ ├── or_keyboard.hpp │ │ ├── or_lifecyclenode.hpp │ │ ├── or_localization.hpp │ │ ├── or_mission_tracker.hpp │ │ ├── or_navigation.hpp │ │ ├── or_obstacle_perception.hpp │ │ ├── or_perception.hpp │ │ ├── or_slam.hpp │ │ └── or_timer.hpp │ │ ├── sm_nav2_test_7.hpp │ │ ├── states │ │ ├── docking_sequence_states │ │ │ ├── st_recover_step_1.hpp │ │ │ ├── st_recover_step_10.hpp │ │ │ ├── st_recover_step_2.hpp │ │ │ ├── st_recover_step_3.hpp │ │ │ ├── st_recover_step_4.hpp │ │ │ ├── st_recover_step_5.hpp │ │ │ ├── st_recover_step_6.hpp │ │ │ ├── st_recover_step_7_a.hpp │ │ │ ├── st_recover_step_7_b.hpp │ │ │ ├── st_recover_step_8.hpp │ │ │ └── st_recover_step_9.hpp │ │ ├── f_pattern_states │ │ │ ├── sti_fpattern_forward_1.hpp │ │ │ ├── sti_fpattern_forward_2.hpp │ │ │ ├── sti_fpattern_loop_start.hpp │ │ │ ├── sti_fpattern_return_1.hpp │ │ │ ├── sti_fpattern_rotate_1.hpp │ │ │ └── sti_fpattern_rotate_2.hpp │ │ ├── radial_motion_states │ │ │ ├── sti_radial_end_point.hpp │ │ │ ├── sti_radial_loop_start.hpp │ │ │ ├── sti_radial_return.hpp │ │ │ └── sti_radial_rotate.hpp │ │ ├── s_pattern_states │ │ │ ├── sti_spattern_forward_1.hpp │ │ │ ├── sti_spattern_forward_2.hpp │ │ │ ├── sti_spattern_forward_3.hpp │ │ │ ├── sti_spattern_forward_4.hpp │ │ │ ├── sti_spattern_loop_start.hpp │ │ │ ├── sti_spattern_rotate_1.hpp │ │ │ ├── sti_spattern_rotate_2.hpp │ │ │ ├── sti_spattern_rotate_3.hpp │ │ │ └── sti_spattern_rotate_4.hpp │ │ ├── st_backup_1.hpp │ │ ├── st_battery_check.hpp │ │ ├── st_final_return_to_origin.hpp │ │ ├── st_final_state.hpp │ │ ├── st_initial_move.hpp │ │ ├── st_initial_move_stop.hpp │ │ ├── st_initial_return_to_origin.hpp │ │ ├── st_launch_nav_stack.hpp │ │ ├── st_launch_vision_pipeline.hpp │ │ ├── st_loading_waypoints_file.hpp │ │ ├── st_navigate_to_waypoint_1.hpp │ │ ├── st_navigate_to_waypoint_2.hpp │ │ ├── st_navigate_to_waypoint_3.hpp │ │ ├── st_navigate_to_waypoint_4.hpp │ │ ├── st_navigate_to_waypoint_5.hpp │ │ ├── st_navigate_warehouse_waypoints.x.hpp │ │ ├── st_pause_slam.hpp │ │ ├── st_pause_to_acquire_fp.hpp │ │ ├── st_pause_to_setup_video.hpp │ │ ├── st_pre_spiral_pattern.hpp │ │ ├── st_recovery_nav2.hpp │ │ ├── st_spin_left_1.hpp │ │ ├── st_spin_left_2.hpp │ │ ├── st_spin_right_1.hpp │ │ ├── st_spiral_pattern_1.hpp │ │ ├── st_switch_yard.hpp │ │ ├── st_waypoint_spin_left.hpp │ │ └── st_waypoint_spin_right.hpp │ │ └── superstates │ │ ├── ss_docking_sequence_1.hpp │ │ ├── ss_f_pattern_1.hpp │ │ ├── ss_radial_pattern_1.hpp │ │ └── ss_s_pattern_1.hpp ├── launch │ ├── extras │ │ ├── carter_navigation_rtx.launch original.py │ │ ├── carter_navigation_rtx.launch.py │ │ ├── fp_sim.launch.py │ │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim1.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim2.launch.py │ │ ├── isaac_ros_foundationpose_isaac_sim_tracking.launch.py │ │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ │ └── sm_nav2_test_6_launch - original.py │ ├── nav2_bringup_launch.py │ ├── nav2_launch.py │ ├── nav2_stack_launch.py │ ├── nav2_stack_launch2.py │ ├── rviz_launch.py │ ├── slam_stack_launch.py │ ├── slam_toolbox_launch.py │ ├── sm_nav2_test_7_launch.py │ ├── vision_pipeline_launch.py │ └── vision_pipeline_launch2.py ├── maps │ ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png │ └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts │ ├── extras │ │ └── fp_dock_pose_publisher.py │ └── lidar_completion.py └── src │ ├── clients │ └── nav2z_client │ │ └── client_behaviors │ │ ├── cb_position_control_free_space.cpp │ │ ├── cb_spiral_motion.cpp │ │ ├── cb_track_path_odometry.cpp │ │ └── cb_track_path_slam.cpp │ └── sm_nav2_test_7 │ └── sm_nav2_test_7.cpp └── sm_nav2_test_8 ├── CMakeLists.txt ├── README.md ├── config ├── default_nav_to_pose_bt.xml ├── extras │ ├── foundationpose_isaac_sim.rviz │ ├── params_isaac_sim.yaml │ ├── rtx_rviz_conf copy 2.rviz │ ├── rtx_rviz_conf copy.rviz │ └── rtx_rviz_conf2.rviz ├── nav2_config.yaml ├── navigation_waypoints_1.yaml ├── rviz_config.rviz ├── slam_toolbox_config.yaml └── sm_nav2_test_8_config.yaml ├── docs ├── SmNav2Test8_2025-4-16_05430.svg └── SmNav2Test8_2025-4-16_05444.pdf ├── include └── sm_nav2_test_8 │ ├── clients │ ├── cl_april_tag_detector │ │ ├── cl_april_tag_detector.hpp │ │ ├── client_behaviors │ │ │ └── cb_detect_apriltag.hpp │ │ └── components │ │ │ └── cp_april_visualization.hpp │ ├── cl_foundationpose │ │ ├── cl_foundationpose.hpp │ │ ├── client_behaviors │ │ │ ├── cb_pause_object_tracking.hpp │ │ │ └── cb_track_object_pose.hpp │ │ └── components │ │ │ ├── cp_object_tracker_1.hpp │ │ │ ├── cp_object_tracker_tf.hpp │ │ │ └── tracker_utils.hpp │ ├── cl_lidar │ │ ├── cl_lidar.hpp │ │ ├── client_behaviors │ │ │ └── cb_lidar_sensor.hpp │ │ └── components │ │ │ └── cp_forward_obstacle_detector.hpp │ └── cl_nav2z │ │ └── client_behaviors │ │ ├── cb_position_control_free_space.hpp │ │ ├── cb_spiral_motion.hpp │ │ ├── cb_track_path_odometry.hpp │ │ └── cb_track_path_slam.hpp │ ├── modestates │ ├── ms_nav2_test_4_recovery_mode.hpp │ └── ms_nav2_test_4_run_mode.hpp │ ├── orthogonals │ ├── or_keyboard.hpp │ ├── or_lifecyclenode.hpp │ ├── or_localization.hpp │ ├── or_navigation.hpp │ ├── or_obstacle_perception.hpp │ ├── or_perception.hpp │ ├── or_slam.hpp │ └── or_timer.hpp │ ├── sm_nav2_test_8.hpp │ ├── states │ ├── f_pattern_states │ │ ├── sti_fpattern_forward_1.hpp │ │ ├── sti_fpattern_forward_2.hpp │ │ ├── sti_fpattern_loop_start.hpp │ │ ├── sti_fpattern_return_1.hpp │ │ ├── sti_fpattern_rotate_1.hpp │ │ └── sti_fpattern_rotate_2.hpp │ ├── radial_motion_states │ │ ├── sti_radial_end_point.hpp │ │ ├── sti_radial_loop_start.hpp │ │ ├── sti_radial_return.hpp │ │ └── sti_radial_rotate.hpp │ ├── st_final_return_to_origin.hpp │ ├── st_final_state.hpp │ ├── st_initial_move.hpp │ ├── st_initial_move_stop.hpp │ ├── st_launch_nav_stack.hpp │ ├── st_loading_waypoints_file.hpp │ ├── st_navigate_to_waypoint_1.hpp │ ├── st_navigate_to_waypoint_2.hpp │ ├── st_navigate_to_waypoint_3.hpp │ ├── st_navigate_to_waypoint_4.hpp │ ├── st_navigate_warehouse_waypoints.x.hpp │ ├── st_pause_to_setup_video.hpp │ ├── st_spin_left_1.hpp │ ├── st_spin_right_1.hpp │ ├── st_spiral_pattern_1.hpp │ ├── st_switch_yard.hpp │ ├── st_waypoint_spin_left.hpp │ └── st_waypoint_spin_right.hpp │ └── superstates │ ├── ss_f_pattern_1.hpp │ └── ss_radial_pattern_1.hpp ├── launch ├── extras │ ├── carter_navigation_rtx.launch original.py │ ├── carter_navigation_rtx.launch.py │ ├── fp_sim.launch.py │ ├── isaac_ros_apriltag_isaac_sim_pipeline.launch.py │ ├── isaac_ros_detectnet_isaac_sim.launch.py │ ├── isaac_ros_foundationpose_isaac_sim1.launch.py │ ├── isaac_ros_foundationpose_isaac_sim2.launch.py │ ├── isaac_ros_foundationpose_isaac_sim_tracking.launch.py │ ├── isaac_ros_occupancy_grid_localizer_nav2.launch.py │ ├── isaac_ros_occupancy_grid_localizer_no_nav2.launch.py │ ├── sm_nav2_test_6_launch - original.py │ └── vision_pipeline_launch2.py ├── nav2_bringup_launch.py ├── nav2_launch.py ├── nav2_stack_launch.py ├── nav2_stack_launch2.py ├── rviz_launch.py ├── slam_stack_launch.py ├── slam_toolbox_launch.py ├── sm_nav2_test_8_launch.py └── vision_pipeline_launch2.py ├── maps ├── placeholder_for_when_isaacsim_gets_mapcreator_act_together.png └── placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml ├── package.xml ├── scripts ├── extras │ └── fp_dock_pose_publisher.py └── lidar_completion.py └── src ├── clients └── nav2z_client │ └── client_behaviors │ ├── cb_position_control_free_space.cpp │ ├── cb_spiral_motion.cpp │ ├── cb_track_path_odometry.cpp │ └── cb_track_path_slam.cpp └── sm_nav2_test_8 └── sm_nav2_test_8.cpp /README.md: -------------------------------------------------------------------------------- 1 | If you're looking for source code from an example you saw on linkedin, you're in the right place. 2 | -------------------------------------------------------------------------------- /sm_nav2_test_1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(sm_nav2_test_1) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(Boost COMPONENTS thread REQUIRED) 14 | 15 | find_package(ament_cmake_auto REQUIRED) 16 | ament_auto_find_build_dependencies() 17 | 18 | include_directories(include) 19 | 20 | ament_export_include_directories(include) 21 | 22 | ament_auto_add_executable(${PROJECT_NAME}_node 23 | src/sm_nav2_test_1/sm_nav2_test_1.cpp 24 | ) 25 | 26 | 27 | 28 | target_link_libraries(${PROJECT_NAME}_node 29 | ${Boost_LIBRARIES} 30 | ) 31 | if(BUILD_TESTING) 32 | find_package(ament_lint_auto REQUIRED) 33 | # the following line skips the linter which checks for copyrights 34 | # comment the line when a copyright and license is added to all source files 35 | set(ament_cmake_copyright_FOUND TRUE) 36 | # the following line skips cpplint (only works in a git repo) 37 | # comment the line when this package is in a git repo and when 38 | # a copyright and license is added to all source files 39 | set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | install(DIRECTORY 44 | launch 45 | config 46 | maps 47 | DESTINATION share/${PROJECT_NAME}) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include 52 | ) 53 | 54 | install(TARGETS 55 | ${PROJECT_NAME}_node 56 | 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | 62 | 63 | ament_package() 64 | -------------------------------------------------------------------------------- /sm_nav2_test_1/config/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_1/config/sm_nav2_test_1_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test1: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/waypoints_warehouse_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_1/docs/SmNav2Test1_2024-9-7_222555.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_1/docs/SmNav2Test1_2024-9-7_222555.pdf -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/modestates/ms_nav2_test_1_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_1 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_1 42 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/modestates/ms_nav2_test_1_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_1 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_1 39 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_1 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_1 36 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_1 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_1 40 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_1 { 26 | 27 | class OrPerception : public smacc2::Orthogonal { 28 | public: 29 | void onInitialize() override { 30 | 31 | } 32 | }; 33 | } // namespace sm_nav2_test_1 34 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_1 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_1 37 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_1 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_1 46 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_1 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_1/include/sm_nav2_test_1/states/st_switch_yard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_1 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StSwitchYard 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSpinRight1, SUCCESS>, 27 | //Keyboard events 28 | Transition, StNavigateToWaypoint2, NEXT>, 29 | Transition, StNavigateToWaypoint3, NEXT>, 30 | Transition, StNavigateToWaypoint4, NEXT>, 31 | Transition, StBackup1, NEXT>, 32 | 33 | Transition, StSpinRight1, NEXT>, 34 | Transition, StInitialMoveStop, PREVIOUS> 35 | > reactions; 36 | 37 | // CpTopicPublisher *pub; 38 | 39 | // STATE FUNCTIONS 40 | static void staticConfigure() { 41 | configure_orthogonal(15s); 42 | configure_orthogonal(); 43 | } 44 | 45 | void runtimeConfigure() {} 46 | 47 | void onEntry() {} 48 | }; 49 | } 50 | -------------------------------------------------------------------------------- /sm_nav2_test_1/launch/__pycache__/isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_1/launch/__pycache__/isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc -------------------------------------------------------------------------------- /sm_nav2_test_1/launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_1"), 22 | "config", 23 | "mapper_params_online_sync.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_1/maps/carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_1/maps/carter_warehouse_navigation.png -------------------------------------------------------------------------------- /sm_nav2_test_1/maps/carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_1/maps/improved_carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_1/maps/improved_carter_warehouse_navigation.png -------------------------------------------------------------------------------- /sm_nav2_test_1/maps/improved_carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: improved_carter_warehouse_navigation.png 2 | resolution: 0.05 3 | origin: [-11.975, -17.975, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /sm_nav2_test_1/scripts/__pycache__/isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_1/scripts/__pycache__/isaac_ros_detectnet_isaac_sim.launch.cpython-310.pyc -------------------------------------------------------------------------------- /sm_nav2_test_1/scripts/points_publisher.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source /opt/ros/humble/setup.bash 4 | 5 | ros2 topic pub /clicked_point geometry_msgs/msg/PointStamped " 6 | header: 7 | stamp: 8 | sec: 1696417411 9 | nanosec: 172037944 10 | frame_id: map 11 | point: 12 | x: -30.2908992767334 13 | y: 44.03512191772461 14 | z: 0.006378173828125 15 | " --once 16 | 17 | ros2 topic pub /clicked_point geometry_msgs/msg/PointStamped " 18 | header: 19 | stamp: 20 | sec: 1696417892 21 | nanosec: 837120351 22 | frame_id: map 23 | point: 24 | x: 29.334697723388672 25 | y: 43.96137237548828 26 | z: 0.002471923828125 27 | " 28 | 29 | ros2 topic pub /clicked_point geometry_msgs/msg/PointStamped " 30 | header: 31 | stamp: 32 | sec: 1696418056 33 | nanosec: 487895629 34 | frame_id: map 35 | point: 36 | x: 28.936553955078125 37 | y: -30.039058685302734 38 | z: -0.001434326171875 39 | " --once 40 | 41 | ros2 topic pub /clicked_point geometry_msgs/msg/PointStamped " 42 | header: 43 | stamp: 44 | sec: 1696418385 45 | nanosec: 67372988 46 | frame_id: map 47 | point: 48 | x: -29.617624282836914 49 | y: -29.65144920349121 50 | z: -0.001434326171875 51 | " --once 52 | 53 | ros2 topic pub /clicked_point geometry_msgs/msg/PointStamped " 54 | header: 55 | stamp: 56 | sec: 1696418581 57 | nanosec: 701335763 58 | frame_id: map 59 | point: 60 | x: -0.2395467907190323 61 | y: 0.038018785417079926 62 | z: 0.002471923828125 63 | " --once 64 | -------------------------------------------------------------------------------- /sm_nav2_test_1/src/sm_nav2_test_1/sm_nav2_test_1.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | rclcpp::init(argc, argv); 25 | smacc2::run(); 26 | } 27 | 28 | //int StExploreNextPoint::count_visited_states = 0; 29 | -------------------------------------------------------------------------------- /sm_nav2_test_2/config/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_2/config/sm_nav2_test_2_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test2: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/waypoints_warehouse_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_2/docs/SmNav2Test2_2024-11-11_231713.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_2/docs/SmNav2Test2_2024-11-11_231713.pdf -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/modestates/ms_nav2_test_2_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_2 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_2 42 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/modestates/ms_nav2_test_2_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_2 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_2 39 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_2 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_2 36 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_2 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_2 40 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_2 { 26 | 27 | class OrPerception : public smacc2::Orthogonal { 28 | public: 29 | void onInitialize() override { 30 | 31 | } 32 | }; 33 | } // namespace sm_nav2_test_2 34 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_2 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_2 37 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_2 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_2 46 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_2 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_2 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_2 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern, SUCCESS>, 29 | 30 | Transition, StSpiralPattern, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_2/include/sm_nav2_test_2/states/st_switch_yard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_2 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StSwitchYard 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSpinRight1, SUCCESS>, 27 | //Keyboard events 28 | Transition, StNavigateToWaypoint2, NEXT>, 29 | Transition, StNavigateToWaypoint3, NEXT>, 30 | Transition, StNavigateToWaypoint4, NEXT>, 31 | Transition, StBackup1, NEXT>, 32 | 33 | Transition, StSpinRight1, NEXT>, 34 | Transition, StInitialMoveStop, PREVIOUS> 35 | > reactions; 36 | 37 | // CpTopicPublisher *pub; 38 | 39 | // STATE FUNCTIONS 40 | static void staticConfigure() { 41 | configure_orthogonal(15s); 42 | configure_orthogonal(); 43 | } 44 | 45 | void runtimeConfigure() {} 46 | 47 | void onEntry() {} 48 | }; 49 | } 50 | -------------------------------------------------------------------------------- /sm_nav2_test_2/launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_2"), 22 | "config", 23 | "mapper_params_online_sync.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_2/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_2/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_2/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_2/src/sm_nav2_test_2/sm_nav2_test_2.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | rclcpp::init(argc, argv); 25 | smacc2::run(); 26 | } 27 | 28 | //int StExploreNextPoint::count_visited_states = 0; 29 | -------------------------------------------------------------------------------- /sm_nav2_test_3/config/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_3/config/sm_nav2_test_3_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test3: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/waypoints_warehouse_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_3/docs/SmNav2Test3_2024-9-7_222147.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_3/docs/SmNav2Test3_2024-9-7_222147.pdf -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/modestates/ms_nav2_test_3_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_3 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_3 42 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/modestates/ms_nav2_test_3_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_3 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_3 39 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_3 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_3 36 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_3 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_3 40 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_3 { 26 | 27 | class OrPerception : public smacc2::Orthogonal { 28 | public: 29 | void onInitialize() override { 30 | 31 | } 32 | }; 33 | } // namespace sm_nav2_test_3 34 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_3 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_3 37 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_3 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_3 46 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_3 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_3 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_3 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern, SUCCESS>, 29 | 30 | Transition, StSpiralPattern, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_3/include/sm_nav2_test_3/states/st_switch_yard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_3 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StSwitchYard 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSpinRight1, SUCCESS>, 27 | //Keyboard events 28 | Transition, StNavigateToWaypoint2, NEXT>, 29 | Transition, StNavigateToWaypoint3, NEXT>, 30 | Transition, StNavigateToWaypoint4, NEXT>, 31 | Transition, StBackup1, NEXT>, 32 | 33 | Transition, StSpinRight1, NEXT>, 34 | Transition, StInitialMoveStop, PREVIOUS> 35 | > reactions; 36 | 37 | // CpTopicPublisher *pub; 38 | 39 | // STATE FUNCTIONS 40 | static void staticConfigure() { 41 | configure_orthogonal(15s); 42 | configure_orthogonal(); 43 | } 44 | 45 | void runtimeConfigure() {} 46 | 47 | void onEntry() {} 48 | }; 49 | } 50 | -------------------------------------------------------------------------------- /sm_nav2_test_3/launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_3"), 22 | "config", 23 | "mapper_params_online_sync.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_3/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_3/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_3/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_3/src/sm_nav2_test_3/sm_nav2_test_3.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | rclcpp::init(argc, argv); 25 | smacc2::run(); 26 | } 27 | 28 | //int StExploreNextPoint::count_visited_states = 0; 29 | -------------------------------------------------------------------------------- /sm_nav2_test_4/config/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_4/config/sm_nav2_test_4_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test4: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/waypoints_warehouse_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_4/docs/SmNav2Test4_2024-9-7_222147.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_4/docs/SmNav2Test4_2024-9-7_222147.pdf -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/modestates/ms_nav2_test_4_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_4 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_4 42 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/modestates/ms_nav2_test_4_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_4 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_4 39 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/modestates/ms_recover.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | namespace sm_nav2_test_4 17 | { 18 | // STATE DECLARATION 19 | class MsRecover : public smacc2::SmaccState 20 | { 21 | public: 22 | using SmaccState::SmaccState; 23 | }; 24 | } // namespace sm_nav2_test_4 25 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_4 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_4 36 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_4 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_4 40 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_4 { 26 | 27 | class OrPerception : public smacc2::Orthogonal { 28 | public: 29 | void onInitialize() override { 30 | 31 | } 32 | }; 33 | } // namespace sm_nav2_test_4 34 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_4 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_4 37 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/docking_sequence_states/st_recover_step_2.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_4 16 | { 17 | // STATE DECLARATION - Calculate Final Pose from Apriltags 18 | struct StRecoverStep2 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep3, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | // configure_orthogonal(50); 38 | // configure_orthogonal(); 39 | // configure_orthogonal(); 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | 45 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 46 | 47 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 48 | }; 49 | } // namespace sm_nav2_test_4 50 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/docking_sequence_states/st_recover_step_3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_4 16 | { 17 | // STATE DECLARATION - Dock 18 | struct StRecoverStep3 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep4, SUCCESS> 31 | >reactions; 32 | 33 | // STATE FUNCTIONS 34 | static void staticConfigure() 35 | { 36 | // configure_orthogonal(50); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 43 | 44 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 45 | }; 46 | } // namespace sm_nav2_test_4 47 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/docking_sequence_states/st_recover_step_4.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_4 16 | { 17 | // STATE DECLARATION - Docked 18 | struct StRecoverStep4 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep5, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | // configure_orthogonal(50); 38 | configure_orthogonal(); 39 | } 40 | 41 | void runtimeConfigure() {} 42 | 43 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 44 | 45 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 46 | }; 47 | } // namespace sm_nav2_test_4 48 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/docking_sequence_states/st_recover_step_5.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_4 16 | { 17 | // STATE DECLARATION - Prepare to Undock 18 | struct StRecoverStep5 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep6, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 43 | 44 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 45 | }; 46 | } // namespace sm_nav2_test_4 47 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_4 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_4 46 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_4 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_4 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_4/include/sm_nav2_test_4/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_4 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern, SUCCESS>, 29 | 30 | Transition, StSpiralPattern, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_4/launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_4"), 22 | "config", 23 | "mapper_params_online_sync.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_4/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_4/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_4/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_4/src/sm_nav2_test_4/sm_nav2_test_4.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | rclcpp::init(argc, argv); 25 | smacc2::run(); 26 | } 27 | 28 | //int StExploreNextPoint::count_visited_states = 0; 29 | -------------------------------------------------------------------------------- /sm_nav2_test_5/config/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_5/config/sm_nav2_test_5_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test5: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/waypoints_warehouse_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_5/docs/SmNav2Test5_2024-9-7_222147.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_5/docs/SmNav2Test5_2024-9-7_222147.pdf -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/modestates/ms_nav2_test_5_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_5 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_5 42 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/modestates/ms_nav2_test_5_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_5 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_5 39 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_5 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_5 36 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_5 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_5 40 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_5 { 26 | 27 | class OrPerception : public smacc2::Orthogonal { 28 | public: 29 | void onInitialize() override { 30 | 31 | } 32 | }; 33 | } // namespace sm_nav2_test_5 34 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_5 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_5 37 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_5 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_5 46 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_5 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_5 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_5 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern, SUCCESS>, 29 | 30 | Transition, StSpiralPattern, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_5/include/sm_nav2_test_5/states/st_switch_yard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_5 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StSwitchYard 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSpinRight1, SUCCESS>, 27 | //Keyboard events 28 | Transition, StNavigateToWaypoint2, NEXT>, 29 | Transition, StNavigateToWaypoint3, NEXT>, 30 | Transition, StNavigateToWaypoint4, NEXT>, 31 | Transition, StBackup1, NEXT>, 32 | 33 | Transition, StSpinRight1, NEXT>, 34 | Transition, StInitialMoveStop, PREVIOUS> 35 | > reactions; 36 | 37 | // CpTopicPublisher *pub; 38 | 39 | // STATE FUNCTIONS 40 | static void staticConfigure() { 41 | configure_orthogonal(15s); 42 | configure_orthogonal(); 43 | } 44 | 45 | void runtimeConfigure() {} 46 | 47 | void onEntry() {} 48 | }; 49 | } 50 | -------------------------------------------------------------------------------- /sm_nav2_test_5/launch/online_sync_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_5"), 22 | "config", 23 | "mapper_params_online_sync.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_5/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_5/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_5/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_5/src/sm_nav2_test_5/sm_nav2_test_5.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | rclcpp::init(argc, argv); 25 | smacc2::run(); 26 | } 27 | 28 | //int StExploreNextPoint::count_visited_states = 0; 29 | -------------------------------------------------------------------------------- /sm_nav2_test_6/config/extras/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_6/config/sm_nav2_test_6_config.yaml: -------------------------------------------------------------------------------- 1 | SmNav2Test6: 2 | ros__parameters: 3 | use_sim_time: true 4 | signal_detector_loop_freq: 20.0 5 | clear_angular_distance_threshold: 0.1 # 0.05 6 | clear_point_distance_threshold: 0.4 #0.1 7 | record_angular_distance_threshold: 0.005 8 | record_point_distance_threshold: 0.1 9 | max_visited_points: 15 #50 10 | 11 | waypoints_file: $(pkg_share)/config/navigation_waypoints_1.yaml 12 | -------------------------------------------------------------------------------- /sm_nav2_test_6/docs/SmNav2Test6_2024-9-7_222147.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_6/docs/SmNav2Test6_2024-9-7_222147.pdf -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/clients/cl_foundationpose/cl_foundationpose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace cl_foundationpose 28 | { 29 | using namespace smacc2::components; 30 | 31 | class ClFoundationPose : public smacc2::ISmaccClient 32 | { 33 | 34 | public: 35 | ClFoundationPose() {} 36 | 37 | void onInitialize() override 38 | { 39 | //auto subcomponent = this->createComponent>("/detection3d_array"); 40 | } 41 | }; 42 | 43 | } // namespace cl_apriltag_detector 44 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/modestates/ms_nav2_test_4_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_6 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_6 42 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/modestates/ms_nav2_test_4_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_6 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_6 39 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/modestates/ms_recover.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | namespace sm_nav2_test_6 17 | { 18 | // STATE DECLARATION 19 | class MsRecover : public smacc2::SmaccState 20 | { 21 | public: 22 | using SmaccState::SmaccState; 23 | }; 24 | } // namespace sm_nav2_test_6 25 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_6 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_6 36 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_6 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_6 40 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/orthogonals/or_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | namespace sm_nav2_test_6 { 29 | 30 | using namespace cl_foundationpose; 31 | 32 | class OrPerception : public smacc2::Orthogonal { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient(); 37 | 38 | //configure the client 39 | auto subcomponent = client->createComponent>("/detection3d_array"); 40 | 41 | client->createComponent(); 42 | 43 | } 44 | }; 45 | } // namespace sm_nav2_test_6 46 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_6 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_6 37 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/docking_sequence_states/st_recover_step_2.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_6 16 | { 17 | // STATE DECLARATION - Calculate Final Pose from Apriltags 18 | struct StRecoverStep2 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep3, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | // configure_orthogonal(50); 38 | // configure_orthogonal(); 39 | // configure_orthogonal(); 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | 45 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 46 | 47 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 48 | }; 49 | } // namespace sm_nav2_test_6 50 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/docking_sequence_states/st_recover_step_3.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_6 16 | { 17 | // STATE DECLARATION - Dock 18 | struct StRecoverStep3 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep4, SUCCESS> 31 | >reactions; 32 | 33 | // STATE FUNCTIONS 34 | static void staticConfigure() 35 | { 36 | // configure_orthogonal(50); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 43 | 44 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 45 | }; 46 | } // namespace sm_nav2_test_6 47 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/docking_sequence_states/st_recover_step_4.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_6 16 | { 17 | // STATE DECLARATION - Docked 18 | struct StRecoverStep4 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep5, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | // configure_orthogonal(50); 38 | configure_orthogonal(); 39 | } 40 | 41 | void runtimeConfigure() {} 42 | 43 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 44 | 45 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 46 | }; 47 | } // namespace sm_nav2_test_6 48 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/docking_sequence_states/st_recover_step_5.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace sm_nav2_test_6 16 | { 17 | // STATE DECLARATION - Prepare to Undock 18 | struct StRecoverStep5 : smacc2::SmaccState 19 | { 20 | using SmaccState::SmaccState; 21 | 22 | // DECLARE CUSTOM OBJECT TAGS 23 | struct TIMEOUT : ABORT{}; 24 | struct NEXT : SUCCESS{}; 25 | struct PREVIOUS : ABORT{}; 26 | 27 | // TRANSITION TABLE 28 | typedef mpl::list< 29 | 30 | Transition, StRecoverStep6, SUCCESS> 31 | 32 | >reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() 36 | { 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } 43 | 44 | void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } 45 | }; 46 | } // namespace sm_nav2_test_6 47 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_6 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_6 46 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_6 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_6 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_6/include/sm_nav2_test_6/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_6 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern, SUCCESS>, 29 | 30 | Transition, StSpiralPattern, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_6/launch/slam_toolbox_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_6"), 22 | "config", 23 | "slam_toolbox_config.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_6/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_6/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_6/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_6/src/sm_nav2_test_6/sm_nav2_test_6.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Robosoft Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // Main entry point of the application 18 | int main(int argc, char **argv) { 19 | rclcpp::init(argc, argv); 20 | smacc2::run(); 21 | } 22 | 23 | -------------------------------------------------------------------------------- /sm_nav2_test_7/config/extras/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_7/docs/SmNav2Test7_2025-4-11_22484.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_7/docs/SmNav2Test7_2025-4-11_22484.pdf -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_foundationpose/cl_foundationpose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace cl_foundationpose 28 | { 29 | using namespace smacc2::components; 30 | 31 | class ClFoundationPose : public smacc2::ISmaccClient 32 | { 33 | 34 | public: 35 | ClFoundationPose() {} 36 | 37 | void onInitialize() override 38 | { 39 | //auto subcomponent = this->createComponent>("/detection3d_array"); 40 | } 41 | }; 42 | 43 | } // namespace cl_apriltag_detector 44 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_foundationpose/components/tracker_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | #include 17 | #include 18 | 19 | namespace cl_foundationpose 20 | { 21 | 22 | struct DetectedObject 23 | { 24 | vision_msgs::msg::Detection3D msg; 25 | 26 | std::optional filtered_pose; 27 | 28 | std::vector historicalPoses_; 29 | 30 | DetectedObject() 31 | { 32 | historicalPoses_.clear(); 33 | } 34 | }; 35 | 36 | 37 | struct EvObjectDetected : sc::event {}; 38 | 39 | } // namespace cl_apriltag_detector 40 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_lidar/cl_lidar.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace sm_nav2_test_7 29 | { 30 | namespace cl_lidar 31 | { 32 | class ClLidarSensor : public cl_multirole_sensor::ClMultiroleSensor 33 | { 34 | public: 35 | ClLidarSensor() {} 36 | }; 37 | } // namespace cl_lidar 38 | } // namespace sm_dance_bot_warehouse_3 39 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace sm_nav2_test_7 28 | { 29 | namespace cl_lidar 30 | { 31 | struct CbLidarSensor : cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior 32 | { 33 | public: 34 | CbLidarSensor() {} 35 | 36 | void onEntry() override 37 | { 38 | RCLCPP_INFO(getLogger(), "[CbLidarSensor] onEntry"); 39 | cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); 40 | } 41 | 42 | virtual void onMessageCallback(const sensor_msgs::msg::LaserScan & /*msg*/) override {} 43 | }; 44 | } // namespace cl_lidar 45 | } // namespace sm_dance_bot_warehouse_3 46 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_nav2z/client_behaviors/cb_track_path_odometry.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_7 27 | { 28 | struct CbTrackPathOdometry : public smacc2::SmaccAsyncClientBehavior 29 | { 30 | private: 31 | 32 | 33 | public: 34 | 35 | CbTrackPathOdometry(); 36 | 37 | void onEntry() override; 38 | 39 | void onExit() override; 40 | }; 41 | } // namespace sm_nav2_test_7 -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/clients/cl_nav2z/client_behaviors/cb_track_path_slam.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_7 27 | { 28 | struct CbTrackPathSLAM : public smacc2::SmaccAsyncClientBehavior 29 | { 30 | private: 31 | 32 | 33 | public: 34 | 35 | CbTrackPathSLAM(); 36 | 37 | void onEntry() override; 38 | 39 | void onExit() override; 40 | }; 41 | } // namespace sm_nav2_test_7 -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/modestates/ms_nav2_test_4_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_7 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_7 42 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/modestates/ms_nav2_test_4_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_7 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_7 39 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/modestates/ms_recover.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | namespace sm_nav2_test_7 17 | { 18 | // STATE DECLARATION 19 | class MsRecover : public smacc2::SmaccState 20 | { 21 | public: 22 | using SmaccState::SmaccState; 23 | }; 24 | } // namespace sm_nav2_test_7 25 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_7 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_7 36 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_7 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_7 40 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/orthogonals/or_mission_tracker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace sm_nav2_test_7 { 30 | using namespace cl_nav2z; 31 | using namespace smacc2; 32 | using namespace client_bases; 33 | using namespace std::chrono_literals; 34 | 35 | class OrMissionTracker : public smacc2::Orthogonal { 36 | public: 37 | void onInitialize() override { 38 | 39 | this->createClient(); 40 | } 41 | }; 42 | } // namespace sm_nav2_test_7 43 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/orthogonals/or_obstacle_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace sm_nav2_test_7 29 | { 30 | using sm_nav2_test_7::cl_lidar::CpForwardObstacleDetector; 31 | using sm_nav2_test_7::cl_lidar::ClLidarSensor; 32 | 33 | class OrObstaclePerception : public smacc2::Orthogonal 34 | { 35 | public: 36 | void onInitialize() override 37 | { 38 | auto lidarClient = this->createClient(); 39 | lidarClient->createComponent(); 40 | 41 | lidarClient->topicName = "/scan2"; 42 | lidarClient->timeout_ = rclcpp::Duration(std::chrono::seconds(10)); 43 | } 44 | }; 45 | } // namespace sm_dance_bot_warehouse 46 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_7 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_7 37 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_7 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_7 46 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_7 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // CpTopicPublisher *pub; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | configure_orthogonal(2s); 37 | configure_orthogonal(); 38 | } 39 | 40 | void runtimeConfigure() {} 41 | 42 | void onEntry() { 43 | cl_nav2z::ClNav2Z* clNav; 44 | this->requiresClient(clNav); 45 | auto pub = clNav->getComponent>(); 46 | auto twist_msg = std::make_shared(); 47 | twist_msg->linear.x = 0.0; 48 | twist_msg->angular.z = 0.0; 49 | pub->publish(*twist_msg); 50 | } 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/states/st_pause_to_acquire_fp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_7 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToAcquireFp 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StLoadingWaypointsFile, SUCCESS>, 27 | //Keyboard events 28 | Transition, StLoadingWaypointsFile, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(20s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_7 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StBatteryCheck, SUCCESS>, 29 | Transition, StInitialMove, NEXT> 30 | // Transition, MsRecover, NEXT> 31 | // Transition, StNavigateToWaypoint2, NEXT> 32 | > reactions; 33 | 34 | // STATE FUNCTIONS 35 | static void staticConfigure() { 36 | // -#configure_orthogonal(20s); 37 | configure_orthogonal(10s); 38 | configure_orthogonal(); 39 | } 40 | 41 | void runtimeConfigure() {} 42 | 43 | void onEntry() {} 44 | }; 45 | } 46 | -------------------------------------------------------------------------------- /sm_nav2_test_7/include/sm_nav2_test_7/states/st_pre_spiral_pattern.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_7 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | using namespace cl_ros_timer; 14 | 15 | 16 | // STATE DECLARATION 17 | struct StPreSpiralPattern 18 | : smacc2::SmaccState { 19 | using SmaccState::SmaccState; 20 | 21 | // DECLARE CUSTOM OBJECT TAGS 22 | struct NEXT : SUCCESS{}; 23 | struct PREVIOUS : ABORT{}; 24 | 25 | 26 | // TRANSITION TABLE 27 | typedef mpl::list< 28 | Transition, StSpiralPattern1, SUCCESS>, 29 | 30 | Transition, StSpiralPattern1, NEXT> 31 | > reactions; 32 | 33 | // CpTopicPublisher *pub; 34 | 35 | // STATE FUNCTIONS 36 | static void staticConfigure() { 37 | //configure_orthogonal(10s); I would like to use the CLtimer instead 38 | configure_orthogonal(); 39 | configure_orthogonal(); 40 | } 41 | 42 | void runtimeConfigure() {} 43 | 44 | void onEntry() {} 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /sm_nav2_test_7/launch/slam_toolbox_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_7"), 22 | "config", 23 | "slam_toolbox_config.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_7/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_7/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_7/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_7/src/sm_nav2_test_7/sm_nav2_test_7.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Robosoft Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // Main entry point of the application 18 | int main(int argc, char **argv) { 19 | rclcpp::init(argc, argv); 20 | smacc2::run(); 21 | } 22 | 23 | -------------------------------------------------------------------------------- /sm_nav2_test_8/config/extras/params_isaac_sim.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | model_name: 'detectnet' 20 | bounding_box_scale: 35.0 21 | bounding_box_offset: 0.0 22 | 23 | # post filtering parameters 24 | enable_confidence_threshold: true 25 | enable_bbox_area_threshold: true 26 | enable_dbscan_clustering: true 27 | confidence_threshold: 0.35 28 | min_bbox_area: 10000.0 29 | dbscan_confidence_threshold: 0.35 30 | dbscan_eps: 0.7 31 | dbscan_min_boxes: 1 32 | dbscan_enable_athr_filter: 0 33 | dbscan_threshold_athr: 0.0 34 | dbscan_clustering_algorithm: 1 35 | -------------------------------------------------------------------------------- /sm_nav2_test_8/docs/SmNav2Test8_2025-4-16_05444.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_8/docs/SmNav2Test8_2025-4-16_05444.pdf -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_foundationpose/cl_foundationpose.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | namespace cl_foundationpose 28 | { 29 | using namespace smacc2::components; 30 | 31 | class ClFoundationPose : public smacc2::ISmaccClient 32 | { 33 | 34 | public: 35 | ClFoundationPose() {} 36 | 37 | void onInitialize() override 38 | { 39 | //auto subcomponent = this->createComponent>("/detection3d_array"); 40 | } 41 | }; 42 | 43 | } // namespace cl_apriltag_detector 44 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_foundationpose/components/tracker_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | #include 17 | #include 18 | 19 | namespace cl_foundationpose 20 | { 21 | 22 | struct DetectedObject 23 | { 24 | vision_msgs::msg::Detection3D msg; 25 | 26 | std::optional filtered_pose; 27 | 28 | std::vector historicalPoses_; 29 | 30 | DetectedObject() 31 | { 32 | historicalPoses_.clear(); 33 | } 34 | }; 35 | 36 | 37 | struct EvObjectDetected : sc::event {}; 38 | 39 | } // namespace cl_apriltag_detector 40 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_lidar/cl_lidar.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace sm_nav2_test_8 29 | { 30 | namespace cl_lidar 31 | { 32 | class ClLidarSensor : public cl_multirole_sensor::ClMultiroleSensor 33 | { 34 | public: 35 | ClLidarSensor() {} 36 | }; 37 | } // namespace cl_lidar 38 | } // namespace sm_dance_bot_warehouse_3 39 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_lidar/client_behaviors/cb_lidar_sensor.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace sm_nav2_test_8 28 | { 29 | namespace cl_lidar 30 | { 31 | struct CbLidarSensor : cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior 32 | { 33 | public: 34 | CbLidarSensor() {} 35 | 36 | void onEntry() override 37 | { 38 | RCLCPP_INFO(getLogger(), "[CbLidarSensor] onEntry"); 39 | cl_multirole_sensor::CbDefaultMultiRoleSensorBehavior::onEntry(); 40 | } 41 | 42 | virtual void onMessageCallback(const sensor_msgs::msg::LaserScan & /*msg*/) override {} 43 | }; 44 | } // namespace cl_lidar 45 | } // namespace sm_dance_bot_warehouse_3 46 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_nav2z/client_behaviors/cb_track_path_odometry.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_8 27 | { 28 | struct CbTrackPathOdometry : public smacc2::SmaccAsyncClientBehavior 29 | { 30 | private: 31 | 32 | 33 | public: 34 | 35 | CbTrackPathOdometry(); 36 | 37 | void onEntry() override; 38 | 39 | void onExit() override; 40 | }; 41 | } // namespace sm_nav2_test_8 -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/clients/cl_nav2z/client_behaviors/cb_track_path_slam.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_8 27 | { 28 | struct CbTrackPathSLAM : public smacc2::SmaccAsyncClientBehavior 29 | { 30 | private: 31 | 32 | 33 | public: 34 | 35 | CbTrackPathSLAM(); 36 | 37 | void onEntry() override; 38 | 39 | void onExit() override; 40 | }; 41 | } // namespace sm_nav2_test_8 -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/modestates/ms_nav2_test_4_recovery_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_8 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RecoveryMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition> 36 | 37 | > 38 | reactions; 39 | // typedef Transition reactions; 40 | }; 41 | } // namespace sm_nav2_test_8 42 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/modestates/ms_nav2_test_4_run_mode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #include 22 | namespace sm_nav2_test_8 { 23 | // STATE DECLARATION 24 | class MsNav2Test1RunMode 25 | : public smacc2::SmaccState { 27 | public: 28 | using SmaccState::SmaccState; 29 | 30 | // TRANSITION TABLE 31 | typedef mpl::list< 32 | 33 | Transition 34 | 35 | > 36 | reactions; 37 | }; 38 | } // namespace sm_nav2_test_8 39 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/orthogonals/or_keyboard.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace sm_nav2_test_8 { 27 | using namespace std::chrono_literals; 28 | 29 | class OrKeyboard : public smacc2::Orthogonal { 30 | public: 31 | void onInitialize() override { 32 | auto keyboardClient = this->createClient(); 33 | } 34 | }; 35 | } // namespace sm_nav2_test_8 36 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/orthogonals/or_lifecyclenode.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | 28 | namespace sm_nav2_test_8 29 | { 30 | using namespace std::chrono_literals; 31 | class OrLifecycleNode : public smacc2::Orthogonal 32 | { 33 | public: 34 | void onInitialize() override 35 | { 36 | auto client = this->createClient("/sync_slam_toolbox_node"); 37 | } 38 | }; 39 | } // namespace sm_nav2_test_8 40 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/orthogonals/or_obstacle_perception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace sm_nav2_test_8 29 | { 30 | using sm_nav2_test_8::cl_lidar::CpForwardObstacleDetector; 31 | using sm_nav2_test_8::cl_lidar::ClLidarSensor; 32 | 33 | class OrObstaclePerception : public smacc2::Orthogonal 34 | { 35 | public: 36 | void onInitialize() override 37 | { 38 | auto lidarClient = this->createClient(); 39 | lidarClient->createComponent(); 40 | 41 | lidarClient->topicName = "/scan2"; 42 | lidarClient->timeout_ = rclcpp::Duration(std::chrono::seconds(10)); 43 | } 44 | }; 45 | } // namespace sm_dance_bot_warehouse 46 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/orthogonals/or_timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | #include 23 | #include 24 | 25 | namespace sm_nav2_test_8 26 | { 27 | class OrTimer : public smacc2::Orthogonal 28 | { 29 | public: 30 | void onInitialize() override 31 | { 32 | auto actionclient = this->createClient( 33 | rclcpp::Duration(std::chrono::milliseconds(500))); 34 | } 35 | }; 36 | } // namespace sm_nav2_test_8 37 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/states/st_final_state.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 RobosoftAI Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /***************************************************************************************************************** 16 | * 17 | * Authors: Pablo Inigo Blasco, Brett Aldrich 18 | * 19 | ******************************************************************************************************************/ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace sm_nav2_test_8 { 26 | using namespace smacc2::default_events; 27 | using smacc2::client_behaviors::CbSleepFor; 28 | using namespace std::chrono_literals; 29 | 30 | // STATE DECLARATION 31 | struct StFinalState 32 | : smacc2::SmaccState { 33 | using SmaccState::SmaccState; 34 | 35 | // TRANSITION TABLE 36 | typedef mpl::list<> reactions; 37 | 38 | // STATE FUNCTIONS 39 | static void staticConfigure() { 40 | configure_orthogonal(); 41 | } 42 | 43 | void runtimeConfigure() {} 44 | }; 45 | } // namespace sm_nav2_test_8 46 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/states/st_initial_move_stop.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_8 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StInitialMoveStop 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StSwitchYard, SUCCESS>, 27 | //Keyboard events 28 | Transition, StSwitchYard, NEXT>, 29 | Transition, StInitialMove, PREVIOUS> 30 | > reactions; 31 | 32 | // STATE FUNCTIONS 33 | static void staticConfigure() { 34 | configure_orthogonal(2s); 35 | configure_orthogonal(); 36 | } 37 | 38 | void runtimeConfigure() {} 39 | 40 | void onEntry() { 41 | cl_nav2z::ClNav2Z* clNav; 42 | this->requiresClient(clNav); 43 | auto pub = clNav->getComponent>(); 44 | auto twist_msg = std::make_shared(); 45 | twist_msg->linear.x = 0.0; 46 | twist_msg->angular.z = 0.0; 47 | pub->publish(*twist_msg); 48 | } 49 | }; 50 | } 51 | -------------------------------------------------------------------------------- /sm_nav2_test_8/include/sm_nav2_test_8/states/st_pause_to_setup_video.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace sm_nav2_test_8 { 8 | using namespace smacc2::default_events; 9 | using smacc2::client_behaviors::CbSleepFor; 10 | using namespace std::chrono_literals; 11 | using namespace cl_nav2z; 12 | using namespace cl_keyboard; 13 | 14 | // STATE DECLARATION 15 | struct StPauseToSetupVideo 16 | : smacc2::SmaccState { 17 | using SmaccState::SmaccState; 18 | 19 | // DECLARE CUSTOM OBJECT TAGS 20 | struct NEXT : SUCCESS{}; 21 | struct PREVIOUS : ABORT{}; 22 | 23 | 24 | // TRANSITION TABLE 25 | typedef mpl::list< 26 | Transition, StInitialMove, SUCCESS>, 27 | //Keyboard events 28 | Transition, StInitialMove, NEXT> 29 | > reactions; 30 | 31 | // STATE FUNCTIONS 32 | static void staticConfigure() { 33 | configure_orthogonal(5s); 34 | configure_orthogonal(); 35 | } 36 | 37 | void runtimeConfigure() {} 38 | 39 | void onEntry() {} 40 | }; 41 | } 42 | -------------------------------------------------------------------------------- /sm_nav2_test_8/launch/slam_toolbox_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | def generate_launch_description(): 11 | use_sim_time = LaunchConfiguration("use_sim_time") 12 | slam_params_file = LaunchConfiguration("slam_params_file") 13 | 14 | declare_use_sim_time_argument = DeclareLaunchArgument( 15 | "use_sim_time", default_value="true", description="Use simulation/Gazebo clock" 16 | ) 17 | declare_slam_params_file_cmd = DeclareLaunchArgument( 18 | "slam_params_file", 19 | # default_value=os.path.join(get_package_share_directory("slam_toolbox"), 20 | default_value=os.path.join( 21 | get_package_share_directory("sm_nav2_test_8"), 22 | "config", 23 | "slam_toolbox_config.yaml", 24 | ), 25 | description="Full path to the ROS2 parameters file to use for the slam_toolbox node", 26 | ) 27 | 28 | start_sync_slam_toolbox_node = Node( 29 | parameters=[slam_params_file, {"use_sim_time": use_sim_time}], 30 | package="slam_toolbox", 31 | executable="sync_slam_toolbox_node", 32 | name="slam_toolbox", 33 | prefix="xterm -hold -e", 34 | output="screen" 35 | # remappings=[("/scan", "/front_2d_lidar/scan")], 36 | ) 37 | 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(declare_use_sim_time_argument) 41 | ld.add_action(declare_slam_params_file_cmd) 42 | ld.add_action(start_sync_slam_toolbox_node) 43 | 44 | return ld 45 | -------------------------------------------------------------------------------- /sm_nav2_test_8/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robosoft-ai/nova_carter_sm_library/54612c5648f0d38c2962163460f9a5172a412dd6/sm_nav2_test_8/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.png -------------------------------------------------------------------------------- /sm_nav2_test_8/maps/placeholder_for_when_isaacsim_gets_mapcreator_act_together.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /sm_nav2_test_8/src/sm_nav2_test_8/sm_nav2_test_8.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Robosoft Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | // Main entry point of the application 18 | int main(int argc, char **argv) { 19 | rclcpp::init(argc, argv); 20 | smacc2::run(); 21 | } 22 | 23 | --------------------------------------------------------------------------------