From c0463f6d49976e4c8280c759b2d349f8bf405891 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 27 Oct 2021 23:22:51 +0200
Subject: [PATCH] Replace re-publishing joint_states with remapping

---
 franka_control/launch/franka_control.launch | 9 ++++-----
 franka_gazebo/launch/panda.launch           | 7 +++----
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
index 0afb5ffc0..a0d7d9510 100644
--- a/franka_control/launch/franka_control.launch
+++ b/franka_control/launch/franka_control.launch
@@ -5,6 +5,10 @@
 
   <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper)" />
 
+  <!-- Remap both, arm and gripper joint states to default ROS topic joint_states -->
+  <remap from="franka_state_controller/joint_states" to="joint_states" />
+  <remap from="franka_gripper/joint_states" to="joint_states" />
+
   <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
     <arg name="robot_ip" value="$(arg robot_ip)" />
   </include>
@@ -17,9 +21,4 @@
   <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
   <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
-  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
-    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
-    <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
-    <param name="rate" value="30"/>
-  </node>
 </launch>
diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch
index 571dec6e9..0701311a2 100644
--- a/franka_gazebo/launch/panda.launch
+++ b/franka_gazebo/launch/panda.launch
@@ -34,6 +34,9 @@
     <arg name="paused" value="true"/>
     <arg name="gui" value="$(eval not arg('headless'))"/>
     <arg name="use_sim_time" value="true"/>
+    <!-- Remap both, arm and gripper joint states to default ROS topic joint_states -->
+    <remap from="franka_state_controller/joint_states" to="joint_states" />
+    <remap from="franka_gripper/joint_states" to="joint_states" />
   </include>
 
   <group ns="$(arg arm_id)">
@@ -88,10 +91,6 @@
     <remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
 
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
-    <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
-      <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
-      <param name="rate" value="30"/>
-    </node>
 
     <!-- Start only if cartesian_impedance_example_controller -->
     <node name="interactive_marker"