def _Broadcast_left_j2_joint(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(P1)
if (partsCount < 2):
pass
try:
P1 = 0 - (radians(float(lineParts[1])) - 1.57)
Joint_State = JointStateDY()
Joint_State.name = "left_j2_joint"
Joint_State.current_pos = P1
Joint_State.header.stamp = rospy.Time.now()
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_j2_joint" +
str(sys.exc_info()[0]))
def _Broadcast_Right_gripper_Joint(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 2):
pass
try:
P1 = radians(float(lineParts[1])) + 1.57
Joint_State = JointStateDY()
Joint_State.name = "right_gripper_joint"
Joint_State.current_pos = P1
Joint_State.header.stamp = rospy.Time.now()
self._P8_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_gripper_joint" +
str(sys.exc_info()[0]))