Merge branch 'main' into gripper_support

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>
alliander-opensource · Sep 17, 2024 · 3d3b174 · 3d3b174
2 parents 6bc1777 + c16f347
commit 3d3b174
Show file tree

Hide file tree

Showing 4 changed files with 67 additions and 4 deletions.
diff --git a/rcdt_franka/config/ros_controller.yaml → rcdt_franka/config/robot_controllers.yaml b/rcdt_franka/config/ros_controller.yaml → rcdt_franka/config/robot_controllers.yaml
@@ -6,7 +6,7 @@ fr3_arm_controller:
   ros__parameters:
     type: joint_trajectory_controller/JointTrajectoryController
     command_interfaces:
-      - position
+      - effort
     state_interfaces:
       - position
       - velocity

diff --git a/rcdt_franka/config/simulation_controllers.yaml b/rcdt_franka/config/simulation_controllers.yaml
@@ -0,0 +1,31 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+fr3_arm_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - fr3_joint1
+      - fr3_joint2
+      - fr3_joint3
+      - fr3_joint4
+      - fr3_joint5
+      - fr3_joint6
+      - fr3_joint7
+
+gripper_action_controller:
+  ros__parameters:
+    type: position_controllers/GripperActionController
+    action_monitor_rate: 20.0
+    allow_stalling: false
+    goal_tolerance: 0.01
+    joint: "fr3_finger_joint1"
+    max_effort: 0.0
+    stall_timeout: 1.0
+    stall_velocity_threshold: 0.001
diff --git a/rcdt_franka/launch/franka.launch.py b/rcdt_franka/launch/franka.launch.py
@@ -25,11 +25,12 @@
 xacro_arguments = {"ros2_control": "true"}
 if ARGS.get_value("simulation") == "true":
     xacro_arguments["gazebo"] = "true"
+    controllers_yaml = "simulation_controllers.yaml"
 else:
     xacro_arguments["robot_ip"] = "172.16.0.2"
+    controllers_yaml = "robot_controllers.yaml"
 robot_description = get_robot_description(xacro_path, xacro_arguments)
-
-controllers_config = get_file_path("rcdt_franka", ["config"], "ros_controller.yaml")
+controllers_config = get_file_path("rcdt_franka", ["config"], controllers_yaml)
 moveit_config = MoveItConfigsBuilder("fr3", package_name="rcdt_franka_moveit_config")
 servo_config = get_yaml(get_file_path("rcdt_franka", ["config"], "servo_params.yaml"))
 # /configs

diff --git a/rcdt_franka/launch/robot.launch.py b/rcdt_franka/launch/robot.launch.py
@@ -20,15 +20,46 @@
         franka_controllers,
         {"arm_id": "fr3"},
     ],
-    remappings=[("/controller_manager/robot_description", "/robot_description")],
+    remappings=[
+        ("/controller_manager/robot_description", "/robot_description"),
+        ("joint_states", "franka/joint_states"),
+    ],
     on_exit=Shutdown(),
 )
+
+gripper_config = get_file_path("franka_gripper", ["config"], "franka_gripper_node.yaml")
+franka_gripper = Node(
+    package="franka_gripper",
+    executable="franka_gripper_node",
+    parameters=[
+        {
+            "robot_ip": "172.16.0.2",
+            "joint_names": ["fr3_finger_joint1", "fr3_finger_joint2"],
+        },
+        gripper_config,
+    ],
+)
+
+joint_state_publisher = Node(
+    package="joint_state_publisher",
+    executable="joint_state_publisher",
+    name="joint_state_publisher",
+    parameters=[
+        {
+            "source_list": ["/franka/joint_states", "franka_gripper_node/joint_states"],
+            "rate": 30,
+        }
+    ],
+)
+
 # /Opstarten robot
 
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             ros2_control_node,
+            franka_gripper,
+            joint_state_publisher,
         ]
     )