class KukaMarkerBaseController(object):
"""KukaMarkerBaseController"""
def __init__(self):
self.marker_server = MarkerServer(topic = 'kuka_marker')
self.shadow_cmd = CommanderBase()
self.joint_names = copy(self.shadow_cmd.joint_names)
self.dof = len(self.joint_names)
# Joint states for shadow robot
self._joint_pub = rospy.Publisher('joint_states', JointState, queue_size=5)
self._joint_states = copy(self.shadow_cmd.joint_states)
self._add_ik_marker()
self._add_menu()
threading.Thread(target=self._publish_joint_states).start()
def _publish_joint_states(self):
r = rospy.Rate(30)
while not rospy.is_shutdown():
self._joint_states.header.stamp = rospy.Time.now()
self._joint_pub.publish(self._joint_states)
r.sleep()
def _add_ik_marker(self):
opt = OptionMarker()
opt.name = 'ik_marker'
opt.frame_id = 'shadow/base_link'
opt.description = 'IK Marker for KUKA Robot'
opt.init_pose = self.shadow_cmd.kinematics.forward_position_kinematics(self._joint_states.position)
opt.callback = self.ik_marker_callback
opt.base_marker = get_marker_sphere()
self.marker_server.add_6DOF(opt)
def _add_menu(self):
# Get menu marker
opt = OptionMenu()
opt.marker_name = 'ik_marker'
opt.entry_name = 'PTP'
opt.callback = self.ptp
self.marker_server.add_menu_entry(opt)
def ptp(self, feedback):
print(self._joint_states.position)
def ik_marker_callback(self, feedback):
# Check event
if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
# Update joint value using inverse kinematics
result = self.shadow_cmd.kinematics.inverse_kinematics(
feedback.pose.position, feedback.pose.orientation, self._joint_states.position)
if result is not None:
self._joint_states.position = result
def __init__(self):
self.marker_server = MarkerServer(topic = 'kuka_marker')
self.shadow_cmd = CommanderBase()
self.joint_names = copy(self.shadow_cmd.joint_names)
self.dof = len(self.joint_names)
# Joint states for shadow robot
self._joint_pub = rospy.Publisher('joint_states', JointState, queue_size=5)
self._joint_states = copy(self.shadow_cmd.joint_states)
self._add_ik_marker()
self._add_menu()
threading.Thread(target=self._publish_joint_states).start()
