From b05aa0ed72052e53133f5e5ea317bf9aa7e73d9a Mon Sep 17 00:00:00 2001 From: brettpac Date: Sat, 28 Sep 2024 14:28:39 -0700 Subject: [PATCH] Brettpac branch (#561) * nitros client creation * precommit * sm_panda_inventory keyboard addition * Adding keyboard server to arm launch files * sm_panda_1 tune up * Working Panda1 * Added keyboard server to sm_multi_panda * sm_multi_panda round 2 --------- Co-authored-by: brettpac --- .../panda_arm_1_moveit_config/CHANGELOG.rst | 60 -------------- .../launch/demo.launch.py | 3 + .../panda_arm_2_moveit_config/CHANGELOG.rst | 60 -------------- .../launch/demo.launch.py | 3 + .../launch/keyboard_server.launch.py | 54 ++++++++++++ .../launch/sm_multi_panda_sim.launch.py | 12 ++- .../control_authority_posture2.yaml | 3 + .../sm_panda_moveit2z_cb_inventory.hpp | 44 ++++++---- .../states/st_acquire_sensors.hpp | 6 +- .../states/st_acquire_sensors_2.hpp | 82 ------------------- .../states/st_end_effector_rotate.hpp | 10 ++- .../states/st_move_end_effector.hpp | 4 +- ...t_move_joints.hpp => st_move_joints_1.hpp} | 11 +-- .../states/st_move_joints_2.hpp | 9 +- .../states/st_move_joints_3.hpp | 9 +- .../states/st_move_known_state_1.hpp | 70 ++++++++++++++++ ...wn_state.hpp => st_move_known_state_2.hpp} | 2 +- .../states/st_move_known_state_3.hpp | 70 ++++++++++++++++ ...t_acquire_sensors_3.hpp => st_pause_1.hpp} | 2 +- .../states/st_pause_2.hpp | 66 +++++++++++++++ .../states/st_pause_3.hpp | 68 +++++++++++++++ .../states/st_pause_4.hpp | 68 +++++++++++++++ .../states/st_pause_5.hpp | 68 +++++++++++++++ .../states/st_pause_6.hpp | 68 +++++++++++++++ .../states/st_pouring_motion.hpp | 4 +- 25 files changed, 616 insertions(+), 240 deletions(-) delete mode 100644 smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/CHANGELOG.rst delete mode 100644 smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/CHANGELOG.rst create mode 100644 smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/keyboard_server.launch.py create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/config/move_group_client/known_states/control_authority_posture2.yaml delete mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp rename smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/{st_move_joints.hpp => st_move_joints_1.hpp} (86%) create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_1.hpp rename smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/{st_move_known_state.hpp => st_move_known_state_2.hpp} (96%) create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_3.hpp rename smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/{st_acquire_sensors_3.hpp => st_pause_1.hpp} (95%) create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_2.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_3.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_4.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_5.hpp create mode 100644 smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_6.hpp diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/CHANGELOG.rst b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/CHANGELOG.rst deleted file mode 100644 index c1f87820e..000000000 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,60 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package panda_arm_1_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.3.16 (2023-07-16) -------------------- -* Merge branch 'humble' of https://github.com/robosoft-ai/SMACC2 into humble -* Brettpac branch (`#518 `_) - * Attempt to fix weird issue with ros buildfarm - * More on this buildfarm issue - --------- - Co-authored-by: brettpac -* Contributors: brettpac, pabloinigoblasco - -2.3.6 (2023-03-12) ------------------- -* minor -* Contributors: pabloinigoblasco - -* fix -* removing mongo dependency (`#402 `_) - * removing mongo dependency - * fix - * fix -* Contributors: Pablo Iñigo Blasco, pabloinigoblasco - -2.3.3 (2023-03-02 22:58) ------------------------- - -2.3.2 (2023-03-02 22:22) ------------------------- -* Feature/format fix humble (`#383 `_) - * several changes for panda demo - * panda configs - * updating and refactoring code - * fixing format check - * fixing format - * missing - * fixing format - * format fix - * humble moveit environment - * fixes - * missing - * minor - * missing - * missing - * fix - * fixing - * fixing format fix - * fix -* Contributors: Pablo Iñigo Blasco - -2.3.1 (2022-11-28) ------------------- - -1.22.1 (2022-11-09 20:22) -------------------------- - -1.22.0 (2022-11-09 19:53) -------------------------- diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/launch/demo.launch.py b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/launch/demo.launch.py index b8e18bfad..f987b79fd 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/launch/demo.launch.py +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_1_moveit_config/launch/demo.launch.py @@ -40,6 +40,7 @@ def generate_launch_description(): output="screen", parameters=[moveit_config.to_dict() | {"ros_control_namespace": "/panda_arm_1"}], arguments=["--ros-args", "--log-level", "move_group:=DEBUG"], + prefix="xterm -hold -e", ) # RViz @@ -68,6 +69,7 @@ def generate_launch_description(): name="robot_state_publisher", output="both", parameters=[moveit_config.robot_description], + prefix="xterm -hold -e", ) # ros2_control using FakeSystem as hardware @@ -82,6 +84,7 @@ def generate_launch_description(): parameters=[moveit_config.robot_description, ros2_controllers_path], output="both", arguments=["--ros-args", "--log-level", "debug"], + prefix="xterm -hold -e", ) # Load controllers diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/CHANGELOG.rst b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/CHANGELOG.rst deleted file mode 100644 index b8d942c19..000000000 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,60 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package panda_arm_2_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.3.16 (2023-07-16) -------------------- -* Merge branch 'humble' of https://github.com/robosoft-ai/SMACC2 into humble -* Brettpac branch (`#518 `_) - * Attempt to fix weird issue with ros buildfarm - * More on this buildfarm issue - --------- - Co-authored-by: brettpac -* Contributors: brettpac, pabloinigoblasco - -2.3.6 (2023-03-12) ------------------- -* minor -* Contributors: pabloinigoblasco - -* fix -* removing mongo dependency (`#402 `_) - * removing mongo dependency - * fix - * fix -* Contributors: Pablo Iñigo Blasco, pabloinigoblasco - -2.3.3 (2023-03-02 22:58) ------------------------- - -2.3.2 (2023-03-02 22:22) ------------------------- -* Feature/format fix humble (`#383 `_) - * several changes for panda demo - * panda configs - * updating and refactoring code - * fixing format check - * fixing format - * missing - * fixing format - * format fix - * humble moveit environment - * fixes - * missing - * minor - * missing - * missing - * fix - * fixing - * fixing format fix - * fix -* Contributors: Pablo Iñigo Blasco - -2.3.1 (2022-11-28) ------------------- - -1.22.1 (2022-11-09 20:22) -------------------------- - -1.22.0 (2022-11-09 19:53) -------------------------- diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/launch/demo.launch.py b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/launch/demo.launch.py index f6a29ebb3..b66d31899 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/launch/demo.launch.py +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/panda_arm_2_moveit_config/launch/demo.launch.py @@ -67,6 +67,7 @@ def generate_launch_description(): parameters=[ panda_arm_2_moveit_config.to_dict() | {"ros_control_namespace": "/panda_arm_2"} ], + prefix="xterm -hold -e", ) # RViz @@ -95,6 +96,7 @@ def generate_launch_description(): name="robot_state_publisher", output="both", parameters=[panda_arm_2_moveit_config.robot_description], + prefix="xterm -hold -e", ) # ros2_control using FakeSystem as hardware @@ -109,6 +111,7 @@ def generate_launch_description(): parameters=[panda_arm_2_moveit_config.robot_description, ros2_controllers_path], output="both", # arguments = ["--ros-args", "--log-level", "debug"] + prefix="xterm -hold -e", ) # Load controllers diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/keyboard_server.launch.py b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/keyboard_server.launch.py new file mode 100644 index 000000000..234e9b74c --- /dev/null +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/keyboard_server.launch.py @@ -0,0 +1,54 @@ +# Copyright 2021 RobosoftAI Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config", + default_value="panda_moveit_config_demo.rviz", + description="RViz configuration file", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + + +def launch_setup(context, *args, **kwargs): + + keyboard_client_node = Node( + package="keyboard_client", + executable="keyboard_server_node.py", + name="keyboard_client", + output="screen", + prefix="xterm -hold -e", + arguments=["--ros-args", "--log-level", "INFO"], + ) + + nodes_to_start = [ + keyboard_client_node, + ] + + return nodes_to_start diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py index 2ad945367..c6886fc72 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/launch/sm_multi_panda_sim.launch.py @@ -41,4 +41,14 @@ def generate_launch_description(): ) ) - return LaunchDescription([panda_arm_1, panda_arm_2]) + keyboard_server = IncludeLaunchDescription( + AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("sm_multi_panda_sim"), + "launch", + "keyboard_server.launch.py", + ) + ) + ) + + return LaunchDescription([panda_arm_1, panda_arm_2, keyboard_server]) diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/config/move_group_client/known_states/control_authority_posture2.yaml b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/config/move_group_client/known_states/control_authority_posture2.yaml new file mode 100644 index 000000000..28318c0c3 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/config/move_group_client/known_states/control_authority_posture2.yaml @@ -0,0 +1,3 @@ +joint_states: + panda_finger_joint1: 0.03 + panda_finger_joint2: 0.03 diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp index c99b8e63e..e56c5757f 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/sm_panda_moveit2z_cb_inventory.hpp @@ -38,27 +38,22 @@ #include -// STATE REACTORS -//#include -//#include -//#include - using namespace cl_moveit2z; using namespace cl_keyboard; -//using namespace smacc2::state_reactors; namespace sm_panda_moveit2z_cb_inventory { -//using namespace cl_moveit2z; -//using namespace cl_keyboard; -//using namespace smacc2::state_reactors; - //STATES struct StAcquireSensors; -struct StAcquireSensors3; -struct StMoveJoints; +struct StPause1; +struct StPause2; +struct StPause3; +struct StPause4; +struct StPause5; +struct StPause6; +struct StMoveJoints1; struct StMoveJoints2; struct StMoveJoints3; struct StMoveEndEffector; @@ -69,13 +64,17 @@ struct StAttachObject; struct StDetatchObject; struct StEndEffectorRotate; struct StExecuteLastTrajectory; -struct StMoveKnownState; +struct StMoveKnownState1; +struct StMoveKnownState2; +struct StMoveKnownState3; struct StPouringMotion; struct StUndoLastTrajectory; //-------------------------------------------------------------------- //STATE_MACHINE -struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase + +struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase + { using SmaccStateMachineBase::SmaccStateMachineBase; @@ -89,15 +88,30 @@ struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase>>>>>> humble #include "states/st_move_joints_2.hpp" #include "states/st_move_joints_3.hpp" #include "states/st_detach_object.hpp" -#include "states/st_move_known_state.hpp" +#include "states/st_move_known_state_1.hpp" +#include "states/st_move_known_state_2.hpp" +#include "states/st_move_known_state_3.hpp" #include "states/st_end_effector_rotate.hpp" #include "states/st_move_last_trajectory_initial_state.hpp" #include "states/st_execute_last_trajectory.hpp" diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp index 759805ac3..c0791a35e 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors.hpp @@ -48,9 +48,11 @@ struct StAcquireSensors : smacc2::SmaccState, OrArm>, StMoveJoints, SUCCESS>, + + Transition, OrArm>, StMoveKnownState1, SUCCESS>, - Transition, StMoveJoints, NEXT> + Transition, StMoveKnownState1, NEXT> + > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp deleted file mode 100644 index e75c26346..000000000 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_2.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2021 RobosoftAI Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/***************************************************************************************************************** - * - * Authors: Pablo Inigo Blasco, Brett Aldrich - * - ******************************************************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -namespace sm_panda_moveit2z_cb_inventory -{ -using namespace std::chrono_literals; -using namespace smacc2::default_events; -using namespace cl_keyboard; - -using smacc2::client_behaviors::CbSleepFor; -using smacc2::client_behaviors::CbWaitActionServer; -using smacc2::client_behaviors::CbWaitTopic; -// using cl_nav2z::CbWaitNav2Nodes; -using smacc2::client_behaviors::CbRosLaunch2; - -// STATE DECLARATION -struct StAcquireSensors2 - : smacc2::SmaccState { - using SmaccState::SmaccState; - - // DECLARE CUSTOM OBJECT AND TRANSITION TAGS - struct ON_SENSORS_AVAILABLE : SUCCESS {}; - struct SrAcquireSensors; - struct NEXT : SUCCESS{}; - struct PREVIOUS : ABORT{}; - - // TRANSITION TABLE - typedef mpl::list< - - Transition, StMoveJoints, ON_SENSORS_AVAILABLE>, - - Transition, StAcquireSensors, ABORT>, - - Transition, StMoveJoints, NEXT> - > - reactions; - - // STATE FUNCTIONS - static void staticConfigure() - { - configure_orthogonal(); - configure_orthogonal(10s); - //configure_orthogonal(); - configure_orthogonal(12s); - configure_orthogonal(); - - // Create State Reactor - auto srAllSensorsReady = static_createStateReactor< - SrAllEventsGo, - smacc2::state_reactors::EvAllGo, - mpl::list< - EvCbSuccess//, - // EvCbSuccess - // EvCbSuccess> - >>(); - } -}; -} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp index 9704c55aa..6c0acfa02 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_end_effector_rotate.hpp @@ -44,11 +44,13 @@ struct StEndEffectorRotate : smacc2::SmaccState, StMoveKnownState, SUCCESS>, + + Transition, StMoveKnownState1, SUCCESS>, Transition, StEndEffectorRotate, SUCCESS>, Transition, StMoveEndEffector, PREVIOUS>, - Transition, StMoveKnownState, NEXT> + Transition, StMoveKnownState1, NEXT> + > reactions; @@ -59,7 +61,9 @@ struct StEndEffectorRotate : smacc2::SmaccState(deltaRadians, tipLink); configure_orthogonal(); diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp index 86d1a11d6..4ffc383a5 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_end_effector.hpp @@ -46,7 +46,9 @@ struct StMoveEndEffector : smacc2::SmaccState, StEndEffectorRotate, SUCCESS>, Transition, StMoveEndEffector, ABORT>, - Transition, StMoveJoints, PREVIOUS>, + + Transition, StMoveJoints1, PREVIOUS>, + Transition, StEndEffectorRotate, NEXT> > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_1.hpp similarity index 86% rename from smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp rename to smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_1.hpp index 86068f602..7f2f78b69 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_1.hpp @@ -32,7 +32,7 @@ using namespace cl_moveit2z; using namespace cl_keyboard; // STATE DECLARATION -struct StMoveJoints : smacc2::SmaccState +struct StMoveJoints1 : smacc2::SmaccState { using SmaccState::SmaccState; @@ -42,11 +42,12 @@ struct StMoveJoints : smacc2::SmaccState, StMoveJoints2, SUCCESS>, - Transition, StMoveJoints, ABORT>, - Transition, StAcquireSensors, PREVIOUS>, - Transition, StMoveJoints2, NEXT> + Transition, StPause3, SUCCESS>, + Transition, StMoveJoints1, ABORT>, + + Transition, StPause3, NEXT> + > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp index 01ce4d51b..a8a982f09 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_2.hpp @@ -42,11 +42,12 @@ struct StMoveJoints2 : smacc2::SmaccState, StMoveJoints3, SUCCESS>, - Transition, StMoveJoints, ABORT>, - Transition, StMoveJoints, PREVIOUS>, - Transition, StMoveJoints3, NEXT> + Transition, StPause4, SUCCESS>, + Transition, StMoveJoints2, ABORT>, + + Transition, StPause4, NEXT> + > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp index 0b3ae26cc..4de73896f 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_joints_3.hpp @@ -42,11 +42,12 @@ struct StMoveJoints3 : smacc2::SmaccState, StMoveEndEffector, SUCCESS>, - Transition, StMoveJoints2, ABORT>, - Transition, StMoveJoints2, PREVIOUS>, - Transition, StMoveEndEffector, NEXT> + Transition, StPause5, SUCCESS>, + Transition, StMoveJoints3, ABORT>, + + Transition, StPause5, NEXT> + > reactions; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_1.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_1.hpp new file mode 100644 index 000000000..8e2b12dcc --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_1.hpp @@ -0,0 +1,70 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "smacc2/smacc.hpp" + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StMoveKnownState1 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + + Transition, StPause2, SUCCESS>, + + Transition, StPause2, NEXT> + + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + std::string pkg = "sm_panda_moveit2z_cb_inventory"; + std::string filepath = "config/move_group_client/known_states/control_authority_posture.yaml"; + + configure_orthogonal(pkg, filepath); + configure_orthogonal(); + } + + void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveKnownState"); } + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_2.hpp similarity index 96% rename from smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp rename to smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_2.hpp index 1868deb42..bc379bf90 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_2.hpp @@ -33,7 +33,7 @@ using namespace smacc2; using namespace cl_keyboard; // STATE DECLARATION -struct StMoveKnownState : smacc2::SmaccState +struct StMoveKnownState2 : smacc2::SmaccState { using SmaccState::SmaccState; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_3.hpp new file mode 100644 index 000000000..1672faddb --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_move_known_state_3.hpp @@ -0,0 +1,70 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "smacc2/smacc.hpp" + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StMoveKnownState3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StPouringMotion, SUCCESS>, + Transition, StPouringMotion, ABORT>, + + Transition, StExecuteLastTrajectory, PREVIOUS>, + Transition, StPouringMotion, NEXT> + > + reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + std::string pkg = "sm_panda_moveit2z_cb_inventory"; + std::string filepath = "config/move_group_client/known_states/control_authority_posture.yaml"; + + configure_orthogonal(pkg, filepath); + configure_orthogonal(); + } + + void runtimeConfigure() { RCLCPP_INFO(getLogger(), "Entering StMoveKnownState"); } + + void onEntry() { RCLCPP_INFO(getLogger(), "On Entry!"); } + + void onExit() { RCLCPP_INFO(getLogger(), "On Exit!"); } +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_1.hpp similarity index 95% rename from smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp rename to smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_1.hpp index 16286619b..9c8809f5d 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_acquire_sensors_3.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_1.hpp @@ -38,7 +38,7 @@ using namespace std::chrono_literals; using namespace cl_keyboard; // STATE DECLARATION -struct StAcquireSensors3 : smacc2::SmaccState +struct StPause1 : smacc2::SmaccState { using SmaccState::SmaccState; diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_2.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_2.hpp new file mode 100644 index 000000000..944bf12ee --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_2.hpp @@ -0,0 +1,66 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StPause2 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveJoints1, SUCCESS>, + + Transition, StMoveJoints1, NEXT> + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_3.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_3.hpp new file mode 100644 index 000000000..a8ea511d2 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_3.hpp @@ -0,0 +1,68 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StPause3 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveJoints2, SUCCESS>, + + Transition, StMoveJoints2, NEXT> + + + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_4.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_4.hpp new file mode 100644 index 000000000..69dbe7b39 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_4.hpp @@ -0,0 +1,68 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StPause4 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveJoints3, SUCCESS>, + + Transition, StMoveJoints3, NEXT> + + + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_5.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_5.hpp new file mode 100644 index 000000000..8e7d3fd54 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_5.hpp @@ -0,0 +1,68 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StPause5 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StMoveKnownState2, SUCCESS>, + + Transition, StMoveKnownState2, NEXT> + + + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_6.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_6.hpp new file mode 100644 index 000000000..cf5ddf822 --- /dev/null +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pause_6.hpp @@ -0,0 +1,68 @@ +// 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. + +/***************************************************************************************************************** + * + * Authors: Pablo Inigo Blasco, Brett Aldrich + * + *****************************************************************************************************************/ + +#pragma once + +#include +#include + +namespace sm_panda_moveit2z_cb_inventory +{ +// SMACC2 classes +using smacc2::EvStateRequestFinish; +using smacc2::Transition; +using smacc2::default_transition_tags::SUCCESS; +using namespace smacc2; +using namespace cl_moveit2z; +using smacc2::client_behaviors::CbWaitTopicMessage; +using smacc2::client_behaviors::CbSleepFor; +using namespace std::chrono_literals; +using namespace cl_keyboard; + +// STATE DECLARATION +struct StPause6 : smacc2::SmaccState +{ + using SmaccState::SmaccState; + + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + + // TRANSITION TABLE + typedef boost::mpl::list< + Transition, StAcquireSensors, SUCCESS>, + + Transition, StAcquireSensors, NEXT> + + + > reactions; + + // STATE FUNCTIONS + static void staticConfigure() + { + // configure_orthogonal>("/joint_states"); + configure_orthogonal(15s); + configure_orthogonal(); + }; + + void runtimeConfigure() {} +}; +} // namespace sm_panda_moveit2z_cb_inventory diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp index 8b41b57a6..d113be48b 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/include/sm_panda_moveit2z_cb_inventory/states/st_pouring_motion.hpp @@ -46,7 +46,9 @@ struct StPouringMotion : smacc2::SmaccState, StMoveLastTrajectoryInitialState, SUCCESS>, Transition, StMoveLastTrajectoryInitialState, ABORT>, - Transition, StMoveKnownState, PREVIOUS>, + + Transition, StMoveKnownState1, PREVIOUS>, + Transition, StMoveLastTrajectoryInitialState, NEXT> > reactions;