diff --git a/joint_state_publisher/CMakeLists.txt b/joint_state_publisher/CMakeLists.txt
index 1c50f93..c6b3808 100644
--- a/joint_state_publisher/CMakeLists.txt
+++ b/joint_state_publisher/CMakeLists.txt
@@ -6,3 +6,10 @@ find_package(catkin REQUIRED)
catkin_package()
install(PROGRAMS joint_state_publisher/joint_state_publisher DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(catkin REQUIRED COMPONENTS rostest)
+ add_rostest(test/test_mimic_chain.launch)
+ add_rostest(test/test_mimic_cycle.launch)
+endif()
diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher/joint_state_publisher
index 0f0f50d..7badd65 100755
--- a/joint_state_publisher/joint_state_publisher/joint_state_publisher
+++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher
@@ -214,9 +214,21 @@ class JointStatePublisher():
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
- joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
+ # Handle recursive mimic chain
+ recursive_mimic_chain_joints = [name]
+ while parent in self.dependent_joints:
+ if parent in recursive_mimic_chain_joints:
+ error_message = "Found an infinite recursive mimic chain"
+ rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent)
+ sys.exit(-1)
+ recursive_mimic_chain_joints.append(parent)
+ param = self.dependent_joints[parent]
+ parent = param['parent']
+ offset += factor * param.get('offset', 0)
+ factor *= param.get('factor', 1)
+ joint = self.free_joints[parent]
if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset
diff --git a/joint_state_publisher/test/mimic_chain.urdf b/joint_state_publisher/test/mimic_chain.urdf
new file mode 100644
index 0000000..9cf0c8b
--- /dev/null
+++ b/joint_state_publisher/test/mimic_chain.urdf
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/joint_state_publisher/test/mimic_cycle.urdf b/joint_state_publisher/test/mimic_cycle.urdf
new file mode 100644
index 0000000..233a7c5
--- /dev/null
+++ b/joint_state_publisher/test/mimic_cycle.urdf
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/joint_state_publisher/test/test_mimic_chain.launch b/joint_state_publisher/test/test_mimic_chain.launch
new file mode 100644
index 0000000..385edd0
--- /dev/null
+++ b/joint_state_publisher/test/test_mimic_chain.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/joint_state_publisher/test/test_mimic_cycle.launch b/joint_state_publisher/test/test_mimic_cycle.launch
new file mode 100644
index 0000000..cfec4c3
--- /dev/null
+++ b/joint_state_publisher/test/test_mimic_cycle.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+