From 344ff3cb08bbb13f462baa27ac8d2a5b8cd50244 Mon Sep 17 00:00:00 2001 From: sukhrajklair <38185092+sukhrajklair@users.noreply.github.com> Date: Wed, 19 Jun 2024 12:26:07 -0400 Subject: [PATCH] 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 --- .../sm_simple_action_client/CMakeLists.txt | 56 ++++++++++++++ .../sm_simple_action_client/README.md | 39 ++++++++++ .../config/simple_action_client_example.yaml | 7 ++ .../simple_action_client_example_config.yaml | 3 + .../fibonacci_action_client/cl_fibonacci.hpp | 45 +++++++++++ .../client_behaviors/cb_fibonacci.hpp | 68 +++++++++++++++++ .../mode_selection_client/cl_mode_select.hpp | 74 +++++++++++++++++++ .../client_behaviors/cb_mode_select.hpp | 65 ++++++++++++++++ .../orthogonals/or_fibonacci.hpp | 37 ++++++++++ .../orthogonals/or_mode_select.hpp | 36 +++++++++ .../sm_simple_action_client.hpp | 58 +++++++++++++++ .../states/st_state_1.hpp | 48 ++++++++++++ .../states/st_state_2.hpp | 58 +++++++++++++++ .../launch/sm_simple_action_client.launch | 5 ++ .../sm_simple_action_client/package.xml | 22 ++++++ .../sm_simple_action_client_node.cpp | 23 ++++++ 16 files changed, 644 insertions(+) create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/CMakeLists.txt create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/README.md create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example.yaml create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example_config.yaml create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/client_behaviors/cb_fibonacci.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/cl_mode_select.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/client_behaviors/cb_mode_select.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_fibonacci.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_mode_select.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/sm_simple_action_client.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_1.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_2.hpp create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/launch/sm_simple_action_client.launch create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/package.xml create mode 100644 smacc2_sm_reference_library/sm_simple_action_client/src/sm_simple_action_client/sm_simple_action_client_node.cpp diff --git a/smacc2_sm_reference_library/sm_simple_action_client/CMakeLists.txt b/smacc2_sm_reference_library/sm_simple_action_client/CMakeLists.txt new file mode 100644 index 00000000..40af4359 --- /dev/null +++ b/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() diff --git a/smacc2_sm_reference_library/sm_simple_action_client/README.md b/smacc2_sm_reference_library/sm_simple_action_client/README.md new file mode 100644 index 00000000..21632741 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/README.md @@ -0,0 +1,39 @@ +

Build Instructions

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

Operating Instructions

+After you 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 +``` + +

Viewer Instructions

+If you have the SMACC2 Runtime Analyzer installed then type... + +``` +ros2 run smacc2_rta smacc2_rta +``` + +If you don't have the SMACC2 Runtime Analyzer click here diff --git a/smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example.yaml b/smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example.yaml new file mode 100644 index 00000000..8b8cdfb1 --- /dev/null +++ b/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 diff --git a/smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example_config.yaml b/smacc2_sm_reference_library/sm_simple_action_client/config/simple_action_client_example_config.yaml new file mode 100644 index 00000000..2e53d257 --- /dev/null +++ b/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 diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp new file mode 100644 index 00000000..b017a65d --- /dev/null +++ b/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 +{ +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 FibonacciResultSignal; + + ClFibonacci(std::string action_name = "/fibonacci") + : smacc2::client_bases::SmaccActionClientBase( + action_name) + { + } + + virtual ~ClFibonacci() {} +}; +} // namespace robot_state_machine + +#endif // ROBOT_STATE_MACHINE__CLIENTS__CL_FIBONACCI_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/client_behaviors/cb_fibonacci.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/fibonacci_action_client/client_behaviors/cb_fibonacci.hpp new file mode 100644 index 00000000..ed6a341e --- /dev/null +++ b/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 +#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 + void onOrthogonalAllocation() + { + this->requiresClient(cl_fibonacci_); + smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(); + } + + void onEntry() override + { + ClFibonacci::Goal goal; + goal.order = 100; + + fibonacciCallback_ = std::make_shared(); + 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 > > + goalHandleFuture_; +}; +} // namespace robot_state_machine + +#endif // ROBOT_STATE_MACHINE__CLIENTS__CB_FIBONACCI_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/cl_mode_select.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/cl_mode_select.hpp new file mode 100644 index 00000000..c39e3050 --- /dev/null +++ b/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 +struct EvAutonomousMode : sc::event> +{ +}; + +template +struct EvManualMode : sc::event> +{ +}; + +class ClModeSelect : public SmaccSubscriberClient +{ +public: + ClModeSelect() : SmaccSubscriberClient("mode_command") {} + + template + void onOrthogonalAllocation() + { + post_autonomous_mode_event_ = [this]() + { + RCLCPP_INFO(getLogger(), "ClModeSelect::post_autonomous_mode_event_"); + // auto event = new EvAutonomousMode(); + this->postEvent>(); + }; + post_manual_mode_event_ = [this]() + { + RCLCPP_INFO(getLogger(), "ClModeSelect::post_manual_mode_event_"); + // auto event = new EvManualMode(); + this->postEvent>(); + }; + + SmaccSubscriberClient::onOrthogonalAllocation< + TOrthogonal, TSourceObject>(); + } + + std::function post_manual_mode_event_; + std::function post_autonomous_mode_event_; +}; + +} // namespace robot_state_machine + +#endif diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/client_behaviors/cb_mode_select.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/mode_selection_client/client_behaviors/cb_mode_select.hpp new file mode 100644 index 00000000..8638fd98 --- /dev/null +++ b/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 +{ +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_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_fibonacci.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_fibonacci.hpp new file mode 100644 index 00000000..7757cce4 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_fibonacci.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** +* + +Author: Sukhraj Klair +******************************************************************************************************************/ + +#ifndef ROBOT__STATE_MACHINE__ORTHOGONALS__OR_FIBONACCI_HPP_ +#define ROBOT__STATE_MACHINE__ORTHOGONALS__OR_FIBONACCI_HPP_ + +#include "sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp" +#include "smacc2/smacc_orthogonal.hpp" + +namespace robot_state_machine +{ + +class OrFibonacci : public smacc2::Orthogonal +{ +public: + void onInitialize() override { auto fibonacci_action_client = this->createClient(); } +}; +} // namespace robot_state_machine + +#endif // ROBOT__STATE_MACHINE__ORTHOGONALS__OR_FIBONACCI_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_mode_select.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_mode_select.hpp new file mode 100644 index 00000000..133cd512 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/orthogonals/or_mode_select.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/***************************************************************************************************************** +* + +Author: Sukhraj Klair +******************************************************************************************************************/ + +#ifndef ROBOT_STATE_MACHINE__ORTHOGONALS__OR_MODE_SELECT_HPP_ +#define ROBOT_STATE_MACHINE__ORTHOGONALS__OR_MODE_SELECT_HPP_ + +#include +#include + +namespace robot_state_machine +{ +class OrModeSelect : public smacc2::Orthogonal +{ +public: + void onInitialize() override { auto client = this->createClient(); } +}; +} // namespace robot_state_machine + +#endif // ROBOT_STATE_MACHINE__ORTHOGONALS__OR_MODE_SELECT_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/sm_simple_action_client.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/sm_simple_action_client.hpp new file mode 100644 index 00000000..4ab4af93 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/sm_simple_action_client.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "smacc2/smacc.hpp" + +//CLIENTS AND BEHAVIORS +#include "sm_simple_action_client/fibonacci_action_client/cl_fibonacci.hpp" +#include "sm_simple_action_client/fibonacci_action_client/client_behaviors/cb_fibonacci.hpp" +#include "sm_simple_action_client/mode_selection_client/cl_mode_select.hpp" +#include "sm_simple_action_client/mode_selection_client/client_behaviors/cb_mode_select.hpp" + +// ORTHOGONALS +#include "sm_simple_action_client/orthogonals/or_fibonacci.hpp" +#include "sm_simple_action_client/orthogonals/or_mode_select.hpp" + +namespace robot_state_machine +{ +// SMACC2 clases + +//STATES +struct StState1; +struct StState2; + +//-------------------------------------------------------------------- +//STATE_MACHINE +struct SmSimpleActionClient : public smacc2::SmaccStateMachineBase +{ + using SmaccStateMachineBase::SmaccStateMachineBase; + + void onInitialize() override + { + this->createOrthogonal(); + this->createOrthogonal(); + } +}; + +} // namespace robot_state_machine + +//STATES +#include "states/st_state_1.hpp" +#include "states/st_state_2.hpp" diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_1.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_1.hpp new file mode 100644 index 00000000..15777051 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_1.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROBOT_STATE_MACHINE__STATES__ST_STATE_1_HPP_ +#define ROBOT_STATE_MACHINE__STATES__ST_STATE_1_HPP_ + +#include "smacc2/smacc.hpp" + +namespace robot_state_machine +{ + +//STATE DECLARATION +struct StState1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + //Declare custom object tags + struct AUTONOMOUS_MODE : SUCCESS + { + }; + struct MANUAL_MODE : SUCCESS + { + }; + + //TRANSITION TABLE + typedef mpl::list< + smacc2::Transition, StState2, AUTONOMOUS_MODE> > + reactions; + + //state functions + static void staticConfigure() { configure_orthogonal(); } + + void runtimeConfigure() {} +}; +} // namespace robot_state_machine + +#endif // ROBOT_STATE_MACHINE__STATES__ST_STATE_1_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_2.hpp b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_2.hpp new file mode 100644 index 00000000..a58d88cd --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/include/sm_simple_action_client/states/st_state_2.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 RobosoftAI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROBOT_STATE_MACHINE__STATES__ST_STATE_2_HPP_ +#define ROBOT_STATE_MACHINE__STATES__ST_STATE_2_HPP_ + +#include "smacc2/smacc.hpp" + +namespace robot_state_machine +{ +//STATE DECLARATION +struct StState2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + //Declare custom object tags + struct MANUAL_MODE: SUCCESS{}; + struct START_PATROL: SUCCESS{}; + struct STOP_PATROL: ABORT{}; + + //TRANSITION TABLE + typedef mpl::list< + smacc2::Transition, StState1, MANUAL_MODE>, + smacc2::Transition, StState2>, + smacc2::Transition, StState2> + > reactions; + + //state functions + static void staticConfigure() + { + configure_orthogonal(); + configure_orthogonal(); + } + + void runtimeConfigure() + { + + } + + void onEntry() + { + } +}; + +} // namespace robot_state_machine + +#endif // ROBOT_STATE_MACHINE__STATES__ST_STATE_2_HPP_ diff --git a/smacc2_sm_reference_library/sm_simple_action_client/launch/sm_simple_action_client.launch b/smacc2_sm_reference_library/sm_simple_action_client/launch/sm_simple_action_client.launch new file mode 100644 index 00000000..f48e8429 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/launch/sm_simple_action_client.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/smacc2_sm_reference_library/sm_simple_action_client/package.xml b/smacc2_sm_reference_library/sm_simple_action_client/package.xml new file mode 100644 index 00000000..630b58a8 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/package.xml @@ -0,0 +1,22 @@ + + + + sm_simple_action_client + 2.3.15 + The sm_simple_action_client package + SMACC2 is cool + Apache-2.0 + MyLicense + + ament_cmake + + libboost-thread-dev + + action_tutorials_interfaces + smacc2 + std_msgs + + + ament_cmake + + diff --git a/smacc2_sm_reference_library/sm_simple_action_client/src/sm_simple_action_client/sm_simple_action_client_node.cpp b/smacc2_sm_reference_library/sm_simple_action_client/src/sm_simple_action_client/sm_simple_action_client_node.cpp new file mode 100644 index 00000000..1eb8e8d3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_simple_action_client/src/sm_simple_action_client/sm_simple_action_client_node.cpp @@ -0,0 +1,23 @@ +// Copyright 2021 MyName/MyCompany Inc. +// Copyright 2021 RobosoftAI Inc. (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +//-------------------------------------------------------------------- +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + smacc2::run(); +}