diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp index 29e2cd42b..fde6b7e46 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp @@ -77,49 +77,17 @@ class StLeftArmMoves; class StRightArmMoves; class StBothArmsMove; -struct EvToDeep : sc::event -{ -}; - -struct EvFail : sc::event -{ -}; - -struct EvSc : sc::event -{ -}; - -struct EvStart : sc::event -{ -}; - -struct EvReset : sc::event -{ -}; - -struct EvSuspend : sc::event -{ -}; - -struct EvUnSuspend : sc::event -{ -}; - -struct EvHold : sc::event -{ -}; - -struct EvUnhold : sc::event -{ -}; - -struct EvStop : sc::event -{ -}; - -struct EvClear : sc::event -{ -}; +struct EvToDeep : sc::event{}; +struct EvFail : sc::event{}; +struct EvSc : sc::event{}; +struct EvStart : sc::event{}; +struct EvReset : sc::event{}; +struct EvSuspend : sc::event{}; +struct EvUnSuspend : sc::event{}; +struct EvHold : sc::event{}; +struct EvUnhold : sc::event{}; +struct EvStop : sc::event{}; +struct EvClear : sc::event{}; //-------------------------------------------------------------------- // STATE_MACHINE @@ -131,6 +99,7 @@ struct SmMultiPandaSim : public smacc2::SmaccStateMachineBasecreateOrthogonal(); this->createOrthogonal(); + this->createOrthogonal(); } }; diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp index b78da909a..7d1eeb476 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp @@ -34,22 +34,30 @@ using namespace cl_moveit2z; using smacc2::client_behaviors::CbWaitTopicMessage; using namespace std::chrono_literals; using cl_moveit2z::CbWaitJointState; +using namespace cl_keyboard; // STATE DECLARATION struct StAcquireSensors : smacc2::SmaccState { using SmaccState::SmaccState; + // DECLARE CUSTOM OBJECT TAGS + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; + // TRANSITION TABLE typedef mpl::list< smacc2::Transition, - smacc2::Transition, StLeftArmMoves, SUCCESS> + smacc2::Transition, StLeftArmMoves, SUCCESS>, + + smacc2::Transition, StLeftArmMoves, NEXT> >reactions; // STATE FUNCTIONS static void staticConfigure() { configure_orthogonal(); + configure_orthogonal(); } void runtimeConfigure() {} diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp index f3735d20e..e0664ce85 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StBothArmsMove : smacc2::SmaccState @@ -25,16 +26,15 @@ struct StBothArmsMove : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StLeftArmMoves, SUCCESS> + smacc2::Transition, StLeftArmMoves, SUCCESS>, + smacc2::Transition, StLeftArmMoves, NEXT> > reactions; @@ -58,6 +58,7 @@ struct StBothArmsMove : smacc2::SmaccState configure_orthogonal(rightJointValues); configure_orthogonal(leftJointValues); + configure_orthogonal(); // configure_orthogonal(15s); } diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp index b635833ae..f8322e3f9 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using namespace std::chrono_literals; using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StLeftArmMoves : smacc2::SmaccState @@ -25,18 +26,18 @@ struct StLeftArmMoves : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StRightArmMoves, SUCCESS> + smacc2::Transition, StRightArmMoves, SUCCESS>, // smacc2::Transition, StRightArmMoves, SUCCESS> + smacc2::Transition, StRightArmMoves, NEXT> + > reactions; @@ -54,6 +55,7 @@ struct StLeftArmMoves : smacc2::SmaccState configure_orthogonal(leftJointValues); // configure_orthogonal(15s); + configure_orthogonal(); } void runtimeConfigure() diff --git a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp index 1c164108b..e6a95be57 100644 --- a/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp +++ b/smacc2_sm_reference_library/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp @@ -18,6 +18,7 @@ namespace sm_multi_panda_sim { using smacc2::client_behaviors::CbSequence; +using namespace cl_keyboard; // STATE DECLARATION struct StRightArmMoves : smacc2::SmaccState @@ -25,16 +26,15 @@ struct StRightArmMoves : smacc2::SmaccState using SmaccState::SmaccState; // DECLARE CUSTOM OBJECT TAGS - struct start_sequence_a : SUCCESS - { - }; - struct start_sequence_b : SUCCESS - { - }; + struct start_sequence_a : SUCCESS{}; + struct start_sequence_b : SUCCESS{}; + struct NEXT : SUCCESS{}; + struct PREVIOUS : ABORT{}; // TRANSITION TABLE typedef mpl::list< - smacc2::Transition, StBothArmsMove, SUCCESS> + smacc2::Transition, StBothArmsMove, SUCCESS>, + smacc2::Transition, StBothArmsMove, NEXT> > reactions; @@ -52,6 +52,7 @@ struct StRightArmMoves : smacc2::SmaccState configure_orthogonal(rightJointValues); // configure_orthogonal(15s); + configure_orthogonal(); } diff --git a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py index b35cfcc7b..de7494186 100644 --- a/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py +++ b/smacc2_sm_reference_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py @@ -135,6 +135,15 @@ def launch_setup(context, *args, **kwargs): output="screen", ) + 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"], + ) + hand_controller_spawner = Node( package="controller_manager", executable="spawner", @@ -150,6 +159,7 @@ def launch_setup(context, *args, **kwargs): joint_state_broadcaster_spawner, arm_controller_spawner, hand_controller_spawner, + keyboard_client_node, ] return nodes_to_start