simple state machine with an action client and a topic subscriber (#543)

* create a simple state machine with two states, one action server client and one subscriber client

* added the template copyright and license info to all code files

* renamed package and files to fit the established naming convention

* added missing license for mode_selection_client and orhtogonals files

* moved license to the top of the cl_mode_select.hpp file

* fixed formatting errors

---------

Co-authored-by: Sukhraj Klair <[email protected]>

smacc2_sm_reference_library/sm_simple_action_client/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.5)
+project(sm_simple_action_client)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(smacc2 REQUIRED)
+find_package(Boost COMPONENTS thread REQUIRED)
+find_package(action_tutorials_interfaces REQUIRED)
+find_package(example_interfaces REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  smacc2
+  example_interfaces
+  action_tutorials_interfaces
+)
+
+include_directories(
+  include
+)
+
+add_executable(${PROJECT_NAME}_node
+  src/${PROJECT_NAME}/${PROJECT_NAME}_node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_node
+  ${smacc2_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(TARGETS
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

smacc2_sm_reference_library/sm_simple_action_client/README.md

@@ -0,0 +1,39 @@
+ <h2>Build Instructions</h2>
+
+First, source your ros2 installation.
+```
+source /opt/ros/humble/setup.bash
+```
+
+Before you build, make sure you've installed all the dependencies...
+
+```
+rosdep install --ignore-src --from-paths src -y -r
+```
+
+Then build with colcon build...
+
+```
+colcon build
+```
+  <h2>Operating Instructions</h2>
+After you build, remember to source the proper workspace...
+
+```
+source ~/workspace/humble_ws/install/setup.sh
+```
+
+And then run the launch file...
+
+```
+ros2 launch simple_action_client_example simple_action_client_example.launch
+```
+
+ <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_simple_action_client/config/simple_action_client_example.yaml

@@ -0,0 +1,7 @@
+state_machine_rosparam_ws: /SmSimpleActionClient
+success_switch:
+   - type: state_reached
+     state_name: "sm_simple_action_client::State2"
+failure_switch:
+   - type: timeout
+     duration: 10.0 # sec

smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example_config.yaml

@@ -0,0 +1,3 @@
+sm_simple_action_client:
+  ros__parameters:
+    signal_detector_loop_freq: 2000.0

smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp

@@ -0,0 +1,45 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROBOT_STATE_MACHINE__CLIENTS__CL_FIBONACCI_HPP_
+#define ROBOT_STATE_MACHINE__CLIENTS__CL_FIBONACCI_HPP_
+
+#include "action_tutorials_interfaces/action/fibonacci.hpp"
+#include "smacc2/client_bases/smacc_action_client_base.hpp"
+#include "smacc2/smacc.hpp"
+
+namespace robot_state_machine
+{
+class ClFibonacci
+: public smacc2::client_bases::SmaccActionClientBase<action_tutorials_interfaces::action::Fibonacci>
+{
+public:
+  using smacc2::client_bases::SmaccActionClientBase<
+    action_tutorials_interfaces::action::Fibonacci>::GoalHandle;
+  using smacc2::client_bases::SmaccActionClientBase<
+    action_tutorials_interfaces::action::Fibonacci>::ResultCallback;
+
+  typedef smacc2::SmaccSignal<void(const WrappedResult &)> FibonacciResultSignal;
+
+  ClFibonacci(std::string action_name = "/fibonacci")
+  : smacc2::client_bases::SmaccActionClientBase<action_tutorials_interfaces::action::Fibonacci>(
+      action_name)
+  {
+  }
+
+  virtual ~ClFibonacci() {}
+};
+}  // namespace robot_state_machine
+
+#endif  // ROBOT_STATE_MACHINE__CLIENTS__CL_FIBONACCI_HPP_

smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/client_behaviors/cb_fibonacci.hpp

@@ -0,0 +1,68 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROBOT_STATE_MACHINE__CLIENTS__CB_FIBONACCI_HPP_
+#define ROBOT_STATE_MACHINE__CLIENTS__CB_FIBONACCI_HPP_
+
+#include <smacc2/smacc_asynchronous_client_behavior.hpp>
+#include "sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp"
+
+namespace robot_state_machine
+{
+class CbFibonacci : public smacc2::SmaccAsyncClientBehavior
+{
+public:
+  CbFibonacci() {}
+  // virtual ~CbFibonacci();
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    this->requiresClient(cl_fibonacci_);
+    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
+  }
+
+  void onEntry() override
+  {
+    ClFibonacci::Goal goal;
+    goal.order = 100;
+
+    fibonacciCallback_ = std::make_shared<ClFibonacci::FibonacciResultSignal>();
+    this->getStateMachine()->createSignalConnection(
+      *fibonacciCallback_, &CbFibonacci::onFibonacciResult, this);
+    goalHandleFuture_ = cl_fibonacci_->sendGoal(goal, fibonacciCallback_);
+  }
+  void onExit() override { cl_fibonacci_->cancelGoal(); }
+
+private:
+  void onFibonacciResult(const ClFibonacci::WrappedResult & result)
+  {
+    if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
+      onFibonacciActionSuccess();
+    else
+      onFibonacciActionAbort();
+  }
+  void onFibonacciActionSuccess() { this->postSuccessEvent(); }
+  void onFibonacciActionAbort() { this->postFailureEvent(); }
+
+  ClFibonacci * cl_fibonacci_;
+  ClFibonacci::FibonacciResultSignal::SharedPtr fibonacciCallback_;
+  rclcpp_action::ResultCode fibonacciResult_;
+  std::shared_future<std::shared_ptr<
+    rclcpp_action::ClientGoalHandle<action_tutorials_interfaces::action::Fibonacci> > >
+    goalHandleFuture_;
+};
+}  // namespace robot_state_machine
+
+#endif  // ROBOT_STATE_MACHINE__CLIENTS__CB_FIBONACCI_HPP_

smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/cl_mode_select.hpp

@@ -0,0 +1,74 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+*
+
+Author: Sukhraj Klair
+******************************************************************************************************************/
+
+#ifndef ROBOT_STATE_MACHINE__CLIENTS__CL_MODE_SELECT_HPP_
+#define ROBOT_STATE_MACHINE__CLIENTS__CL_MODE_SELECT_HPP_
+
+#include "example_interfaces/msg/int32.hpp"
+#include "smacc2/client_bases/smacc_subscriber_client.hpp"
+
+namespace robot_state_machine
+{
+
+using namespace smacc2::client_bases;
+
+//declare the events
+template <typename TSource, typename TOrthogonal>
+struct EvAutonomousMode : sc::event<EvAutonomousMode<TSource, TOrthogonal>>
+{
+};
+
+template <typename TSource, typename TOrthogonal>
+struct EvManualMode : sc::event<EvManualMode<TSource, TOrthogonal>>
+{
+};
+
+class ClModeSelect : public SmaccSubscriberClient<example_interfaces::msg::Int32>
+{
+public:
+  ClModeSelect() : SmaccSubscriberClient<example_interfaces::msg::Int32>("mode_command") {}
+
+  template <typename TOrthogonal, typename TSourceObject>
+  void onOrthogonalAllocation()
+  {
+    post_autonomous_mode_event_ = [this]()
+    {
+      RCLCPP_INFO(getLogger(), "ClModeSelect::post_autonomous_mode_event_");
+      // auto event = new EvAutonomousMode<TSourceObject, TOrthogonal>();
+      this->postEvent<EvAutonomousMode<TSourceObject, TOrthogonal>>();
+    };
+    post_manual_mode_event_ = [this]()
+    {
+      RCLCPP_INFO(getLogger(), "ClModeSelect::post_manual_mode_event_");
+      // auto event = new EvManualMode<TSourceObject, TOrthogonal>();
+      this->postEvent<EvManualMode<TSourceObject, TOrthogonal>>();
+    };
+
+    SmaccSubscriberClient<example_interfaces::msg::Int32>::onOrthogonalAllocation<
+      TOrthogonal, TSourceObject>();
+  }
+
+  std::function<void()> post_manual_mode_event_;
+  std::function<void()> post_autonomous_mode_event_;
+};
+
+}  // namespace robot_state_machine
+
+#endif

smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/client_behaviors/cb_mode_select.hpp

@@ -0,0 +1,65 @@
+
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+*
+
+Author: Sukhraj Klair
+******************************************************************************************************************/
+
+#ifndef ROBOT_STATE_MACHINE__CLIENTS__CB_MODE_SELECT_HPP_
+#define ROBOT_STATE_MACHINE__CLIENTS__CB_MODE_SELECT_HPP_
+
+#include "sm_simple_action_client/mode_selection_client/cl_mode_select.hpp"
+#include "smacc2/client_behaviors/cb_subscription_callback_base.hpp"
+namespace robot_state_machine
+{
+
+using namespace smacc2::client_behaviors;
+
+class CbModeSelect : public CbSubscriptionCallbackBase<example_interfaces::msg::Int32>
+{
+public:
+  CbModeSelect() {}
+  void onEntry() override
+  {
+    RCLCPP_INFO(getLogger(), "CbModeSelect::onEntry()");
+    this->requiresClient(mode_select_client_);
+    mode_select_client_->onFirstMessageReceived(&CbModeSelect::onFirstMessageReceived, this);
+    mode_select_client_->onMessageReceived(&CbModeSelect::onMessageReceived, this);
+  }
+  void onFirstMessageReceived(const example_interfaces::msg::Int32 & msg)
+  {
+    RCLCPP_INFO(getLogger(), "CbModeSelect::onFirstMessageReceived()");
+  }
+  void onMessageReceived(const example_interfaces::msg::Int32 & msg) override
+  {
+    if (msg.data == 0)
+    {
+      mode_select_client_->post_manual_mode_event_();
+      RCLCPP_INFO(getLogger(), "CbModeSelect::onMessageReceived() - MANUAL");
+    }
+    else if (msg.data == 1)
+    {
+      mode_select_client_->post_autonomous_mode_event_();
+      RCLCPP_INFO(getLogger(), "CbModeSelect::onMessageReceived() - AUTONOMOUS");
+    }
+  }
+
+private:
+  ClModeSelect * mode_select_client_;
+};
+}  // namespace robot_state_machine
+#endif  // ROBOT_STATE_MACHINE__CLIENTS__CB_MODE_SELECT_HPP_