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 <[email protected]>

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

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

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

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])

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

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 <smacc2/client_behaviors/cb_wait_topic_message.hpp>
 
-// STATE REACTORS
-//#include <sr_all_events_go/sr_all_events_go.hpp>
-//#include <sr_conditional/sr_conditional.hpp>
-//#include <sr_event_countdown/sr_event_countdown.hpp>
-
 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<SmPandaMoveit2zCbInventory, StAcquireSensors3>
+
+struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase<SmPandaMoveit2zCbInventory, StPause1>
+
 {
   using SmaccStateMachineBase::SmaccStateMachineBase;
 
@@ -89,15 +88,30 @@ struct SmPandaMoveit2zCbInventory : public smacc2::SmaccStateMachineBase<SmPanda
 
 // STATES
 #include "states/st_acquire_sensors.hpp"
+<<<<<<< HEAD
+#include "states/st_pause_1.hpp"
+#include "states/st_pause_2.hpp"
+#include "states/st_pause_3.hpp"
+#include "states/st_pause_4.hpp"
+#include "states/st_pause_5.hpp"
+#include "states/st_pause_6.hpp"
+#include "states/st_attach_object.hpp"
+#include "states/st_move_end_effector.hpp"
+#include "states/st_circular_pivot_motion.hpp"
+#include "states/st_move_joints_1.hpp"
+=======
 #include "states/st_acquire_sensors_3.hpp"
 #include "states/st_attach_object.hpp"
 #include "states/st_move_end_effector.hpp"
 #include "states/st_circular_pivot_motion.hpp"
 #include "states/st_move_joints.hpp"
+>>>>>>> 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"

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<StAcquireSensors, SmPandaMoveit2zCb
 
   // TRANSITION TABLE
   typedef boost::mpl::list<
-    Transition<EvCbSuccess<CbWaitTopicMessage<sensor_msgs::msg::JointState>, OrArm>, StMoveJoints, SUCCESS>,
+
+    Transition<EvCbSuccess<CbWaitTopicMessage<sensor_msgs::msg::JointState>, OrArm>, StMoveKnownState1, SUCCESS>,
 
-    Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StMoveJoints, NEXT>  
+    Transition<EvKeyPressN<CbDefaultKeyboardBehavior, OrKeyboard>, StMoveKnownState1, NEXT>  
+
 
 
     > reactions;