Creation of sm_nav2_unit_test_1 (#687)

* Update to smacc2_client_library/CLAUDE.md reflecting migration to pure component based architecture

* Few formatting moves

* Refactoring boost signals usage to pure smacc signals - SmaccSignalConnection

* Cleaned up a few dangling boost signal references

* Creation of sm_nav2_unit_test_1 gazebo example in sm_reference_library

* Pre-commit format fixes

Author: brettpac

smacc2_sm_reference_library/sm_nav2_unit_test_1/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.5)
+project(sm_nav2_unit_test_1)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(smacc2 REQUIRED)
+find_package(cl_ros2_timer REQUIRED)
+find_package(cl_keyboard REQUIRED)
+find_package(cl_nav2z REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Boost COMPONENTS thread REQUIRED)
+
+# Include directories
+include_directories(
+  include
+  ${smacc2_INCLUDE_DIRS}
+  ${cl_ros2_timer_INCLUDE_DIRS}
+  ${cl_keyboard_INCLUDE_DIRS}
+  ${cl_nav2z_INCLUDE_DIRS}
+)
+
+# Executable
+add_executable(${PROJECT_NAME}_node
+  src/sm_nav2_unit_test_1/sm_nav2_unit_test_1_node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${smacc2_LIBRARIES}
+  ${cl_ros2_timer_LIBRARIES}
+  ${cl_keyboard_LIBRARIES}
+  ${cl_nav2z_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node
+  smacc2
+  cl_ros2_timer
+  cl_keyboard
+  cl_nav2z
+  geometry_msgs
+)
+
+# Install
+install(DIRECTORY include/ DESTINATION include)
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

smacc2_sm_reference_library/sm_nav2_unit_test_1/README.md

@@ -0,0 +1,45 @@
+ <h2>State Machine Diagram</h2>
+
+ ![sm_nav2_unit_test_1](docs/SmNav2UnitTest1.svg)
+
+ <h2>Description</h2> A Nav2 unit test state machine demonstrating basic navigation and rotation behaviors using the Nav2 stack with TurtleBot3 simulation. The state machine navigates to a waypoint, rotates 180 degrees, then navigates to a second waypoint.<br></br>
+
+ <h2>Build Instructions</h2>
+
+First, source your ros2 installation.
+```
+source /opt/ros/jazzy/setup.bash
+```
+
+Before you build, make sure you've installed all the dependencies...
+
+```
+rosdep install --ignore-src --from-paths src -y -r
+```
+
+Then build with colcon build...
+
+```
+colcon build
+```
+<h2>Operating Instructions</h2>
+After you build, remember to source the workspace...
+
+```
+source install/setup.bash
+```
+
+And then run the launch file...
+
+```
+ros2 launch sm_nav2_unit_test_1 sm_nav2_unit_test_1.launch.py
+```
+
+ <h2>Viewer Instructions</h2>
+If you have the SMACC2 Runtime Analyzer installed then type...
+
+```
+ros2 run smacc2_rta smacc2_rta
+```
+
+If you don't have the SMACC2 Runtime Analyzer click <a href="https://robosoft.ai/product-category/smacc2-runtime-analyzer/">here</a>.

smacc2_sm_reference_library/sm_nav2_unit_test_1/config/sm_nav2_unit_test_1_config.yaml

@@ -0,0 +1,4 @@
+# Configuration for sm_nav2_unit_test_1
+sm_nav2_unit_test_1:
+  ros__parameters:
+    use_sim_time: true

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/orthogonals/or_keyboard.hpp

@@ -0,0 +1,32 @@
+// Copyright 2024 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <cl_keyboard/cl_keyboard.hpp>
+#include <smacc2/smacc_orthogonal.hpp>
+
+namespace sm_nav2_unit_test_1
+{
+
+class OrKeyboard : public smacc2::Orthogonal<OrKeyboard>
+{
+public:
+  void onInitialize() override
+  {
+    auto client = this->createClient<cl_keyboard::ClKeyboard>();
+  }
+};
+
+}  // namespace sm_nav2_unit_test_1

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/orthogonals/or_navigation.hpp

@@ -0,0 +1,53 @@
+// Copyright 2024 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <cl_nav2z/cl_nav2z.hpp>
+#include <cl_nav2z/components/pose/cp_pose.hpp>
+#include <cl_nav2z/components/odom_tracker/cp_odom_tracker.hpp>
+#include <cl_nav2z/components/planner_switcher/cp_planner_switcher.hpp>
+#include <cl_nav2z/components/goal_checker_switcher/cp_goal_checker_switcher.hpp>
+#include <cl_nav2z/components/amcl/cp_amcl.hpp>
+#include <smacc2/smacc_orthogonal.hpp>
+
+namespace sm_nav2_unit_test_1
+{
+
+class OrNavigation : public smacc2::Orthogonal<OrNavigation>
+{
+public:
+  void onInitialize() override
+  {
+    auto client = this->createClient<cl_nav2z::ClNav2Z>();
+
+    // Create required components for navigation behaviors
+    // CpPose: provides robot pose from TF
+    client->createComponent<cl_nav2z::CpPose>("base_link", "map");
+
+    // CpOdomTracker: tracks odometry path for undo operations
+    client->createComponent<cl_nav2z::odom_tracker::CpOdomTracker>();
+
+    // CpPlannerSwitcher: switches between planners/controllers
+    client->createComponent<cl_nav2z::CpPlannerSwitcher>();
+
+    // CpGoalCheckerSwitcher: switches goal checker algorithms
+    client->createComponent<cl_nav2z::CpGoalCheckerSwitcher>();
+
+    // CpAmcl: provides initial pose setting for AMCL localization
+    client->createComponent<cl_nav2z::CpAmcl>();
+  }
+};
+
+}  // namespace sm_nav2_unit_test_1

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/orthogonals/or_timer.hpp

@@ -0,0 +1,35 @@
+// Copyright 2024 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <chrono>
+#include <cl_ros2_timer/cl_ros2_timer.hpp>
+#include <smacc2/smacc.hpp>
+
+using namespace std::chrono_literals;
+
+namespace sm_nav2_unit_test_1
+{
+
+class OrTimer : public smacc2::Orthogonal<OrTimer>
+{
+public:
+  void onInitialize() override
+  {
+    auto client = this->createClient<cl_ros2_timer::ClRos2Timer>(1s);
+  }
+};
+
+}  // namespace sm_nav2_unit_test_1

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/sm_nav2_unit_test_1.hpp

@@ -0,0 +1,77 @@
+// Copyright 2024 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <smacc2/smacc.hpp>
+
+// CLIENTS
+#include <cl_ros2_timer/cl_ros2_timer.hpp>
+#include <cl_keyboard/cl_keyboard.hpp>
+#include <cl_nav2z/cl_nav2z.hpp>
+
+// CLIENT BEHAVIORS
+#include <cl_ros2_timer/client_behaviors/cb_timer_countdown_once.hpp>
+#include <cl_keyboard/client_behaviors/cb_default_keyboard_behavior.hpp>
+#include <cl_nav2z/client_behaviors/cb_navigate_global_position.hpp>
+#include <cl_nav2z/client_behaviors/cb_wait_nav2_nodes.hpp>
+#include <cl_nav2z/client_behaviors/cb_wait_transform.hpp>
+#include <cl_nav2z/client_behaviors/cb_pure_spinning.hpp>
+#include <cl_nav2z/components/amcl/cp_amcl.hpp>
+
+// ORTHOGONALS
+#include "orthogonals/or_timer.hpp"
+#include "orthogonals/or_keyboard.hpp"
+#include "orthogonals/or_navigation.hpp"
+
+using namespace boost;
+using namespace smacc2;
+using namespace cl_ros2_timer;
+using namespace cl_keyboard;
+using namespace cl_nav2z;
+
+namespace sm_nav2_unit_test_1
+{
+
+// FORWARD DECLARATIONS
+class StAllSensorsGo;
+class StSetInitialPose;
+class StNavigateToWaypoint1;
+class StRotate;
+class StNavigateToWaypoint2;
+class StFinalState;
+
+//--------------------------------------------------------------------
+// STATE MACHINE
+struct SmNav2UnitTest1 : public smacc2::SmaccStateMachineBase<SmNav2UnitTest1, StAllSensorsGo>
+{
+  using SmaccStateMachineBase::SmaccStateMachineBase;
+
+  void onInitialize() override
+  {
+    this->createOrthogonal<OrTimer>();
+    this->createOrthogonal<OrKeyboard>();
+    this->createOrthogonal<OrNavigation>();
+  }
+};
+
+}  // namespace sm_nav2_unit_test_1
+
+// STATE INCLUDES (must be after state machine definition)
+#include "states/st_all_sensors_go.hpp"
+#include "states/st_set_initial_pose.hpp"
+#include "states/st_navigate_to_waypoint_1.hpp"
+#include "states/st_rotate.hpp"
+#include "states/st_navigate_to_waypoint_2.hpp"
+#include "states/st_final_state.hpp"

smacc2_sm_reference_library/sm_nav2_unit_test_1/include/sm_nav2_unit_test_1/states/st_all_sensors_go.hpp

@@ -0,0 +1,72 @@
+// Copyright 2024 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <smacc2/smacc.hpp>
+
+namespace sm_nav2_unit_test_1
+{
+
+using namespace cl_ros2_timer;
+using namespace cl_keyboard;
+using namespace cl_nav2z;
+using namespace smacc2::default_transition_tags;
+
+// STATE DECLARATION
+struct StAllSensorsGo : smacc2::SmaccState<StAllSensorsGo, SmNav2UnitTest1>
+{
+  using SmaccState::SmaccState;
+
+  // CUSTOM TRANSITION TAGS
+  struct NEXT : SUCCESS {};
+  struct NAV2_READY : SUCCESS {};
+
+  // TRANSITION TABLE
+  typedef mpl::list<
+    Transition<EvCbSuccess<CbWaitNav2Nodes, OrNavigation>, StSetInitialPose, NAV2_READY>,
+    Transition<EvTimer<CbTimerCountdownOnce, OrTimer>, StSetInitialPose, SUCCESS>,
+    Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StSetInitialPose, NEXT>
+  > reactions;
+
+  // STATE FUNCTIONS
+  static void staticConfigure()
+  {
+    // Wait for Nav2 nodes to be ready (PlannerServer, ControllerServer, BtNavigator)
+    configure_orthogonal<OrNavigation, CbWaitNav2Nodes>();
+
+    // Timer countdown: triggers EvTimer after 15 seconds (backup if Nav2 ready signal not received)
+    configure_orthogonal<OrTimer, CbTimerCountdownOnce>(15);
+
+    // Keyboard behavior: allows manual state advancement with 'N' key
+    configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
+  }
+
+  void runtimeConfigure()
+  {
+    RCLCPP_INFO(getLogger(), "StAllSensorsGo: runtimeConfigure()");
+  }
+
+  void onEntry()
+  {
+    RCLCPP_INFO(getLogger(), "StAllSensorsGo: onEntry() - Waiting for Nav2 nodes to be ready...");
+  }
+
+  void onExit()
+  {
+    RCLCPP_INFO(getLogger(), "StAllSensorsGo: onExit()");
+  }
+};
+
+}  // namespace sm_nav2_unit_test_1