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_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_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 loop(self):
while not rospy.is_shutdown():
ms = MotorState()
self.lock.acquire()
ms.load = self.head.current_load
self.lock.release()
self.head_load.publish(ms)
rospy.Rate(50)
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]))
