scripts/arm_ros_conn.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import rospy
   5 import arm
   6 from sensor_msgs.msg import JointState
   7
   8 class ARMRosConn():
   9         def __init__(self):
  10                 rospy.init_node('arm')
  11
  12                 arm.switch(0)
  13                 arm.switch(2)
  14                 arm.set_hall_mode(3, 0)
  15                 arm.set_hall_mode(5, 0)
  16                 arm.set_tolerance(3, 0)
  17                 arm.set_tolerance(5, 0)
  18
  19                 self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16)
  20                 self.run()
  21
  22         def run(self):
  23                 rate = rospy.Rate(20)
  24                 while not rospy.is_shutdown():
  25                         self.publish_joint_states()
  26                         rate.sleep()
  27
  28         def publish_joint_states(self):
  29                 joint_state = JointState()
  30                 joint_state.header.stamp = rospy.Time.now()
  31                 joint_state.name = ["base_to_link1", "link_1_2_joint", "link_2_3_joint", "gripper_joint_1", "gripper_joint_2", "left_gripper_joint", "right_gripper_joint"]
  32                 joint_state.position = [arm.get_angle(0), arm.get_angle(1), arm.get_angle(2), arm.get_angle(3), arm.get_angle(4), arm.get_angle(5)/2, arm.get_angle(5)/2]
  33                 self.pub_joint_states.publish(joint_state)
  34
  35
  36 if __name__ == '__main__':
  37         ARMRosConn()