From 4dad4aa9f36f4dadd05836f087fb6cbfce3722d5 Mon Sep 17 00:00:00 2001
From: Abdurrahman Yilmaz <yilmazabdurrah@itu.edu.tr>
Date: Tue, 26 Nov 2024 12:19:16 +0000
Subject: [PATCH] Mock Franka Setup fixed for ros2 controllers

---
 .../config/panda_mock_ros_controllers.yaml    | 34 +++++++++++++++++++
 .../launch/moveit_real_arm_platform.launch.py |  8 +++--
 2 files changed, 39 insertions(+), 3 deletions(-)
 create mode 100644 franka_moveit_config/config/panda_mock_ros_controllers.yaml

diff --git a/franka_moveit_config/config/panda_mock_ros_controllers.yaml b/franka_moveit_config/config/panda_mock_ros_controllers.yaml
new file mode 100644
index 0000000..21613b2
--- /dev/null
+++ b/franka_moveit_config/config/panda_mock_ros_controllers.yaml
@@ -0,0 +1,34 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    panda_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+panda_arm_controller:
+  ros__parameters:
+    command_interfaces:
+      - position # for real arm: -effort, for fake arm: -position
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+    gains:
+      panda_joint1: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint2: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint3: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint4: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint5: { p: 250., d: 10., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint6: { p: 150., d: 10., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint7: { p: 50., d: 5., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
diff --git a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py
index 6e3f3d9..fb8e748 100644
--- a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py
+++ b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py
@@ -219,19 +219,21 @@ def generate_launch_description():
         package='robot_state_publisher',
         executable='robot_state_publisher',
         name='robot_state_publisher',
-        output='both',
+        output='screen',
         parameters=[robot_description],
+        remappings=[('joint_states', 'franka/joint_states')],
     )
 
     ros2_controllers_path = os.path.join(
         get_package_share_directory('franka_moveit_config'),
         'config',
-        'panda_ros_controllers.yaml',
+        'panda_mock_ros_controllers.yaml' if use_fake_hardware else 'panda_ros_controllers.yaml',
     )
+
     ros2_control_node = Node(
         package='controller_manager',
         executable='ros2_control_node',
-        parameters=[robot_description, ros2_controllers_path],
+        parameters=[robot_description, ros2_controllers_path], #[ros2_controllers_path]
         remappings=[('joint_states', 'franka/joint_states')],
         output={
             'stdout': 'screen',