Python JointState.position[index["ToolRotate"]]方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import position[index["ToolRotate"]] [as 别名]
 def run(self):
     timeout = True
     rate = rospy.Rate(10)
     while not rospy.is_shutdown() and not self.joint_state:
         rate.sleep()
     if rospy.is_shutdown():
         return
     command = JointState()
     command.header = self.joint_state.header
     command.name = self.joint_state.name
     command.velocity = [0.0] * len(self.joint_state.name)
     command.effort = [0.0] * len(self.joint_state.name)
     command.position = [x for x in self.joint_state.position]
     index = dict(zip(command.name,range(0,len(command.name))))
     ((a,b,c),_) = self.listener.lookupTransform('/%s/ArmPan'%(self.name),
             '/%s/ArmTilt'%(self.name), rospy.Time(0))
     zoffset = c
     ((a,b,c),_) = self.listener.lookupTransform('/%s/ArmTilt'%(self.name),
             '/%s/ToolRotate'%(self.name), rospy.Time(0))
     x = -b; y = a; z = c;
     state = "Ready"
     while not rospy.is_shutdown():
         if (rospy.rostime.get_time() - self.last_joy) < 0.5: 
             if timeout:
                 timeout = False
                 rospy.loginfo("Accepting joystick commands")
             ((a,b,c),_) = self.listener.lookupTransform('/%s/ArmPan'%(self.name),
                     '/%s/ToolRotate'%(self.name), rospy.Time(0))
             c -= zoffset
             self.tool.publish(Vector3(-b,a,c))
             if (state == "Homing") or (self.joy_value.buttons[self.home_button]):
                 if state != "Homing":
                     rospy.loginfo("Homing");
                 state = "Homing"
                 command.position[index["ArmPan"]] = 0.0
                 command.position[index["ArmTilt"]] = 0.0
                 command.position[index["ArmFold"]] = pi/3
                 command.position[index["ArmExtend"]] = 0.0
                 command.position[index["ToolRotate"]] = -pi/3
             if (state == "Ready") or (self.joy_value.buttons[self.ready_button]):
                 if state != "Ready":
                     rospy.loginfo("Getting ready");
                 state = "Ready"
                 command.position[index["ArmPan"]] = 0.0
                 command.position[index["ArmTilt"]] = pi/4
                 command.position[index["ArmFold"]] = 0.0
                 command.position[index["ArmExtend"]] = 0.0
                 command.position[index["ToolRotate"]] = pi/4
             if (state == "Default") or (self.joy_value.buttons[self.move_button]):
                 first = False
                 if state != "Default":
                     first = True
                     x = -b; y = a; z = c;
                     # Prepare the kinematic solver. The assumption is that
                     # extend is at zero
                     ((x1,y1,z1),_) = self.listener.lookupTransform('/%s/ArmTilt'%(self.name),
                             '/%s/ArmFold'%(self.name), rospy.Time(0))
                     print (x1,y1,z1)
                     l1 = hypot(x1,y1)
                     ((x2,y2,z2),_) = self.listener.lookupTransform('/%s/ArmFold'%(self.name),
                             '/%s/ToolRotate'%(self.name), rospy.Time(0))
                     print (x2,y2,z2)
                     l2 = hypot(x2,y2)
                     rospy.loginfo("Creating kinematic solver: l1=%.3f l2=%.3f"%(l1,l2))
                     self.arm_ik = ArmIK(l1,l2)
                     rospy.loginfo("Switching to default behaviour");
                 state = "Default"
                 dx = self.joy_value.axes[self.axis_arm_x]*self.arm_step
                 dy = self.joy_value.axes[self.axis_arm_y]*self.arm_step
                 dz = self.joy_value.axes[self.axis_arm_z]*self.arm_step
                 if first or (abs(dx)>1e-3) or (abs(dy)>1e-3) or (abs(dz)>1e-3):
                     new_x = x + dx
                     new_y = y + dy
                     new_z = z + dz
                     rospy.loginfo("Trying to move to %.2f %.2f %.2f from %.2f %.2f %.2f" \
                             % (new_x,new_y,new_z, x,y,z))
                     S = self.arm_ik.ik_xyz(new_x,new_y,new_z,0.0)
                     if S:
                         x = new_x; y = new_y; z = new_z
                         (theta0,theta1,theta2,u) = S;
                         print S
                         command.position[index["ArmPan"]] = theta0
                         command.position[index["ArmTilt"]] = pi/2-theta1
                         command.position[index["ArmFold"]] = -pi/2-theta2
                         command.position[index["ArmExtend"]] = u
                     else:
                         rospy.loginfo("No Solution")
                 (_,rot) = self.listener.lookupTransform('/%s/Tool'%(self.name),
                         '/%s/ArmPan'%(self.name), rospy.Time(0))
                 euler = euler_from_quaternion(rot)
                 error = (euler[0]+pi/2)
                 # print (euler[0], euler[0]*180./pi, error)
                 command.position[index["ToolRotate"]] += 0.1*error
         else:
             if not timeout:
                 timeout = True
                 rospy.loginfo("Timeout: ignoring joystick commands")
         self.joint.publish(command)
         rate.sleep()