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()