def _BroadcastJointStateinfo_P1(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target P3 = float(lineParts[3])# current P4 = float(lineParts[4])# speed val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P2_MotorPublisher.publish(Motor_State) Joint_State = JointState() Joint_State.name = "right_arm_pan_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp) self._P1_JointPublisher.publish(Joint_State) #rospy.logwarn(Joint_State) except: rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P1(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: P1 = ((321-float(lineParts[1]))*0.008)-0.15 P2 = self.left_tilt P3 = float(lineParts[3])# current P4 = 0#float(lineParts[4])# speed val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P1_MotorPublisher.publish(Motor_State) self._left_tilt_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "left_arm_tilt_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp) self._P1_JointPublisher.publish(Joint_State) #rospy.logwarn(Joint_State) except: rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P4(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57) P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57) P3 = float(lineParts[3]) P4 = float(lineParts[4]) val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P4_MotorPublisher.publish(Motor_State) #rospy.logwarn(Motor_State) #rospy.logwarn(val) Joint_State = JointState() Joint_State.name = "right_arm_rotate_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now() self._P4_JointPublisher.publish(Joint_State) except: rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P5(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: P1 = 0 - ((radians((float(lineParts[1]))) / 10) - 0.65) P2 = self.right_rotate #0-((float(lineParts[2])* 0.00174532925)-1.57) P3 = float(lineParts[3]) P4 = 0 val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P5_MotorPublisher.publish(Motor_State) #rospy.logwarn(Motor_State) self._right_rotate_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "right_arm_rotate_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now() self._P5_JointPublisher.publish(Joint_State) #rospy.logwarn(val) except: rospy.logwarn("Unexpected error:right_arm_rotate_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 4): pass try: P1 = radians(float(lineParts[1]) * 0.2) P2 = self.right_tilt P3 = 0 #float(lineParts[3])# current P4 = 0 #float(lineParts[4])# speed val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P2_MotorPublisher.publish(Motor_State) self._right_tilt_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "right_arm_tilt_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now( ) #.stamprospy.Time.from_sec(state.timestamp) self._P2_JointPublisher.publish(Joint_State) #rospy.logwarn(Joint_State) except: rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts): partsCount = len(lineParts) # rospy.logwarn(partsCount) if partsCount < 7: pass try: off = 299 A1 = float(lineParts[1]) - off P1 = 0 - ((A1 * 0.00174532925) - 0) A2 = float(lineParts[2]) - off P2 = 0 - ((A2 * 0.00174532925) - 0) P3 = float(lineParts[3]) # current P4 = 0 # float(lineParts[4])# speed val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P2_MotorPublisher.publish(Motor_State) self._right_tilt_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "right_arm_tilt_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now() # .stamprospy.Time.from_sec(state.timestamp) self._P2_JointPublisher.publish(Joint_State) # rospy.logwarn(Joint_State) except: rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: cal = 0 A1 = (1023 + cal) - float(lineParts[1]) P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position A2 = (1023 + cal) - float(lineParts[1]) P2 = (A2* 0.003070961)#float(lineParts[2])# target P3 = float(lineParts[3])# current P4 = float(lineParts[4])# speed val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P2_MotorPublisher.publish(Motor_State) Joint_State = JointState() Joint_State.name = "right_arm_lift_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp) self._P2_JointPublisher.publish(Joint_State) #rospy.logwarn(Joint_State) except: rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P3(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 7): pass try: #offset = Float(-1.57) P1 = (float(lineParts[1])/1000)-0.6 P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57) P3 = 0#float(lineParts[3]) P4 = 0 val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P3_MotorPublisher.publish(Motor_State) #rospy.logwarn(Motor_State) self._left_lift_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "left_arm_lift_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now() self._P3_JointPublisher.publish(Joint_State) except: rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P8(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(lineParts) if (partsCount < 4): pass try: P1 = radians(float(lineParts[1]))#P1 = radians(float(lineParts[1])) P2 = self.pan P3 = 0#float(lineParts[3]) P4 = 0 val = [P1, P2, P3, P4] Motor_State = MotorState() Motor_State.id = 11 Motor_State.goal = P2 Motor_State.position = P1 Motor_State.speed = P4 Motor_State.load = P3 Motor_State.moving = 0 Motor_State.timestamp = time.time() self.P8_MotorPublisher.publish(Motor_State) self._pan_Publisher.publish(P1) Joint_State = JointState() Joint_State.name = "pan_joint" Joint_State.goal_pos = P2 Joint_State.current_pos = P1 Joint_State.velocity = P4 Joint_State.load = P3 Joint_State.error = P1 - P2 Joint_State.is_moving = 0 Joint_State.header.stamp = rospy.Time.now() self._P8_JointPublisher.publish(Joint_State) #rospy.logwarn(val) except: pass#rospy.logwarn("Unexpected error:pan_joint" + str(sys.exc_info()[0]))