split 6dof and 7dof

Signed-off-by: Jakub Delicat <[email protected]>

config/kinova_6dof_controllers.yaml

@@ -0,0 +1,38 @@
+/**:
+  joint_trajectory_controller:
+    ros__parameters:
+      joints:
+        - joint_1
+        - joint_2
+        - joint_3
+        - joint_4
+        - joint_5
+        - joint_6
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      state_publish_rate: 100.0
+      action_monitor_rate: 20.0
+      allow_partial_joints_goal: false
+      constraints:
+        stopped_velocity_tolerance: 0.0
+        goal_time: 0.0
+
+  twist_controller:
+    ros__parameters:
+      joint: tcp
+      interface_names:
+        - twist.linear.x
+        - twist.linear.y
+        - twist.linear.z
+        - twist.angular.x
+        - twist.angular.y
+        - twist.angular.z
+
+  robotiq_gripper_controller:
+    ros__parameters:
+      default: true
+      joint: robotiq_85_left_knuckle_joint
+      allow_stalling: true

launch/gz_components.launch.py

@@ -37,7 +37,9 @@ def get_value(node: yaml.Node, key: str):
         return ""
 
 
-def get_launch_description(name: str, package: str, namespace: str, component: yaml.Node):
+def get_launch_description(
+    name: str, package: str, namespace: str, component: yaml.Node, controllers_file=""
+):
     return IncludeLaunchDescription(
         PythonLaunchDescriptionSource([package, "/launch/gz_", name, ".launch.py"]),
         launch_arguments={
@@ -46,6 +48,7 @@ def get_launch_description(name: str, package: str, namespace: str, component: y
             "tf_prefix": get_value(component, "tf_prefix"),
             "gz_bridge_name": component["namespace"][1:] + "_gz_bridge",
             "camera_name": get_value(component, "name"),
+            "controllers_file": controllers_file,
         }.items(),
     )
 
@@ -74,10 +77,26 @@ def get_launch_descriptions_from_yaml_node(
             component["type"] == "MAN04"
             # or component["type"] == "MAN03" # sim_isaac error
             or component["type"] == "MAN05"
-            or component["type"] == "MAN06"
-            or component["type"] == "MAN07"
         ):
-            actions.append(get_launch_description("kinova", package, namespace, component))
+            actions.append(
+                get_launch_description(
+                    "kinova",
+                    package,
+                    namespace,
+                    component,
+                    controllers_file="kinova_6dof_controllers.yaml",
+                )
+            )
+        if component["type"] == "MAN06" or component["type"] == "MAN07":
+            actions.append(
+                get_launch_description(
+                    "kinova",
+                    package,
+                    namespace,
+                    component,
+                    controllers_file="kinova_7dof_controllers.yaml",
+                )
+            )
 
     return actions
 

launch/gz_ur.launch.py

@@ -24,10 +24,9 @@ def generate_launch_description():
     robot_namespace = LaunchConfiguration("robot_namespace")
     device_namespace = LaunchConfiguration("device_namespace")
     tf_prefix = LaunchConfiguration("tf_prefix")
-    controllers_file = LaunchConfiguration("controllers_file")
 
     initial_joint_controllers = PathJoinSubstitution(
-        [FindPackageShare("ros_components_description"), "config", controllers_file]
+        [FindPackageShare("ros_components_description"), "config", "ur_controllers.yaml"]
     )
 
     namespace_warn = LogInfo(
@@ -70,12 +69,6 @@ def generate_launch_description():
         description="Prefix added for all links of device. Here used as fix for static transform publisher.",
     )
 
-    declare_controllers_file = DeclareLaunchArgument(
-        "controllers_file",
-        default_value="ur_controllers.yaml",
-        description="YAML file with the controllers configuration.",
-    )
-
     # There may be other controllers of the joints, but this is the initially-started one
     initial_joint_controller_spawner_started = Node(
         package="controller_manager",
@@ -101,7 +94,6 @@ def generate_launch_description():
             declare_device_namespace,
             declare_robot_namespace,
             declare_tf_prefix,
-            declare_controllers_file,
             namespace_warn,
             initial_joint_controller_spawner_started,
         ]