ur5_ros_controller.py

JOINT_NAMES = [
    'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
    'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]


class UR5Controller(object):
    def __init__(self):
        rospy.init_node('node', anonymous=True)
        self.speed_pub = rospy.Publisher('ur_driver/joint_speed', JointTrajectory, queue_size=1)
        rospy.Subscriber('/joint_states', JointState, self._joint_state_cb)
        self.control_rate = rospy.Rate(frequency)

    def update(self, velocity):
        self._send_velocity(velocity)
        self.control_rate.sleep()
        return self._observation

    def _joint_state_cb(self, msg):
        self._observation = msg.velocity

    def _send_velocity(self, velocity):
        msg = JointTrajectory()
        msg.joint_names = JOINT_NAMES
        msg.points = [
            JointTrajectoryPoint(
                velocities=velocity,
                accelerations=[10.0]*6
            )
        ]
        self.speed_pub.publish(msg)