Adding keyboard server to arm launch files

robosoft-ai · Sep 24, 2024 · 4c73647 · 4c73647
1 parent 8218193
commit 4c73647
Show file tree

Hide file tree

Showing 6 changed files with 56 additions and 65 deletions.
diff --git a/...y/sm_multi_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/sm_multi_panda_sim.hpp b/...y/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<EvToDeep>
-{
-};
-
-struct EvFail : sc::event<EvFail>
-{
-};
-
-struct EvSc : sc::event<EvSc>
-{
-};
-
-struct EvStart : sc::event<EvStart>
-{
-};
-
-struct EvReset : sc::event<EvReset>
-{
-};
-
-struct EvSuspend : sc::event<EvSuspend>
-{
-};
-
-struct EvUnSuspend : sc::event<EvUnSuspend>
-{
-};
-
-struct EvHold : sc::event<EvHold>
-{
-};
-
-struct EvUnhold : sc::event<EvUnhold>
-{
-};
-
-struct EvStop : sc::event<EvStop>
-{
-};
-
-struct EvClear : sc::event<EvClear>
-{
-};
+struct EvToDeep : sc::event<EvToDeep>{};
+struct EvFail : sc::event<EvFail>{};
+struct EvSc : sc::event<EvSc>{};
+struct EvStart : sc::event<EvStart>{};
+struct EvReset : sc::event<EvReset>{};
+struct EvSuspend : sc::event<EvSuspend>{};
+struct EvUnSuspend : sc::event<EvUnSuspend>{};
+struct EvHold : sc::event<EvHold>{};
+struct EvUnhold : sc::event<EvUnhold>{};
+struct EvStop : sc::event<EvStop>{};
+struct EvClear : sc::event<EvClear>{};
 
 //--------------------------------------------------------------------
 // STATE_MACHINE
@@ -131,6 +99,7 @@ struct SmMultiPandaSim : public smacc2::SmaccStateMachineBase<SmMultiPandaSim, S
   {
     this->createOrthogonal<OrArmLeft>();
     this->createOrthogonal<OrArmRight>();
+    this->createOrthogonal<OrKeyboard>();
   }
 };
 

diff --git a/...lti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_acquire_sensors.hpp b/...lti_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<StAcquireSensors, SmMultiPandaSim>
 {
   using SmaccState::SmaccState;
 
+  // DECLARE CUSTOM OBJECT TAGS
+  struct NEXT : SUCCESS{};
+  struct PREVIOUS : ABORT{};
+
   // TRANSITION TABLE
   typedef mpl::list<
       smacc2::Transition<EvStart, StLeftArmMoves, SUCCESS>,
-      smacc2::Transition<EvCbSuccess<CbWaitJointState, OrArmLeft>, StLeftArmMoves, SUCCESS>
+      smacc2::Transition<EvCbSuccess<CbWaitJointState, OrArmLeft>, StLeftArmMoves, SUCCESS>,
+
+      smacc2::Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StLeftArmMoves, NEXT>
     >reactions;
 
   // STATE FUNCTIONS
   static void staticConfigure()
   {
     configure_orthogonal<OrArmLeft, CbWaitJointState>();
+    configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
   }
 
   void runtimeConfigure() {}

diff --git a/...ulti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp b/...ulti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_both_arms_move.hpp
@@ -18,23 +18,23 @@ namespace sm_multi_panda_sim
 {
 
 using smacc2::client_behaviors::CbSequence;
+using namespace cl_keyboard;
 
 // STATE DECLARATION
 struct StBothArmsMove : smacc2::SmaccState<StBothArmsMove, SmMultiPandaSim>
 {
   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<EvCbSuccess<CbMoveJoints, OrArmLeft>, StLeftArmMoves, SUCCESS>
+      smacc2::Transition<EvCbSuccess<CbMoveJoints, OrArmLeft>, StLeftArmMoves, SUCCESS>,
+      smacc2::Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StLeftArmMoves, NEXT>
       >
       reactions;
 
@@ -58,6 +58,7 @@ struct StBothArmsMove : smacc2::SmaccState<StBothArmsMove, SmMultiPandaSim>
 
     configure_orthogonal<OrArmRight, CbMoveJoints>(rightJointValues);
     configure_orthogonal<OrArmLeft, CbMoveJoints>(leftJointValues);
+    configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
     // configure_orthogonal<OrArmRight, CbSleepFor>(15s);
   }
 

diff --git a/...ulti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp b/...ulti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_left_arm_moves.hpp
@@ -18,25 +18,26 @@ 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<StLeftArmMoves, SmMultiPandaSim>
 {
   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<EvCbSuccess<CbMoveJoints, OrArmLeft>, StRightArmMoves, SUCCESS>
+      smacc2::Transition<EvCbSuccess<CbMoveJoints, OrArmLeft>, StRightArmMoves, SUCCESS>,
       // smacc2::Transition<EvCbSuccess<CbSleepFor, OrArmLeft>, StRightArmMoves, SUCCESS>
 
+      smacc2::Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StRightArmMoves, NEXT>
+
       >
       reactions;
 
@@ -54,6 +55,7 @@ struct StLeftArmMoves : smacc2::SmaccState<StLeftArmMoves, SmMultiPandaSim>
 
     configure_orthogonal<OrArmLeft, CbMoveJoints>(leftJointValues);
     // configure_orthogonal<OrArmLeft, CbSleepFor>(15s);
+    configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
   }
 
   void runtimeConfigure()

diff --git a/...lti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp b/...lti_panda_sim/sm_multi_panda_sim/include/sm_multi_panda_sim/states/st_right_arm_moves.hpp
@@ -18,23 +18,23 @@ namespace sm_multi_panda_sim
 {
 
 using smacc2::client_behaviors::CbSequence;
+using namespace cl_keyboard;
 
 // STATE DECLARATION
 struct StRightArmMoves : smacc2::SmaccState<StRightArmMoves, SmMultiPandaSim>
 {
   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<EvCbSuccess<CbMoveJoints, OrArmRight>, StBothArmsMove, SUCCESS>
+      smacc2::Transition<EvCbSuccess<CbMoveJoints, OrArmRight>, StBothArmsMove, SUCCESS>,
+      smacc2::Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StBothArmsMove, NEXT>
 
       >
       reactions;
@@ -52,6 +52,7 @@ struct StRightArmMoves : smacc2::SmaccState<StRightArmMoves, SmMultiPandaSim>
 
     configure_orthogonal<OrArmRight, CbMoveJoints>(rightJointValues);
     // configure_orthogonal<OrArmRight, CbSleepFor>(15s);
+    configure_orthogonal<OrKeyboard, CbDefaultKeyboardBehavior>();
 
   }
 

diff --git a/...ce_library/sm_panda_moveit2z_cb_inventory/launch/sm_panda_moveit2z_cb_inventory.launch.py b/...ce_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