def send_lift_goal(self, lift_goal):
lift_pos_pub = rospy.Publisher('/joint_pos_cmd', xm_JointPos, queue_size=1000)
lift_pos = xm_JointPos()
lift_pos.command = 0x01
lift_pos.joint = 0
lift_pos.position = lift_goal
lift_pos_pub.publish(lift_pos)
rospy.loginfo("Publish lift's height to the topic!")
def send_lift_goal(self, lift_goal):
lift_pos_pub = rospy.Publisher('/joint_pos_cmd',
xm_JointPos,
queue_size=1000)
lift_pos = xm_JointPos()
lift_pos.command = 0x01
lift_pos.joint = 0
lift_pos.position = lift_goal
lift_pos_pub.publish(lift_pos)
rospy.loginfo("Publish lift's height to the topic!")
def send_wrist_goal_single(self, wrist_goal):
global judge_value, wrist_pos_current
wrist_pos = xm_JointPos()
way_points = 50
wrist_pos_step = (wrist_goal - wrist_pos_current) / way_points
rospy.loginfo("wrist_step:%lf", wrist_pos_step)
rospy.loginfo("Publish wrist's pitching angle to the topic!")
if (wrist_goal >= 0
and wrist_pos_current >= 0) or (wrist_goal <= 0
and wrist_pos_current <= 0):
if abs(wrist_goal) > abs(wrist_pos_current):
judge_value = 1E-10
while (abs(wrist_goal) - abs(wrist_pos_current) >=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
elif abs(wrist_goal) < abs(wrist_pos_current):
judge_value = -1E-10
while (abs(wrist_goal) - abs(wrist_pos_current) <=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal
elif wrist_goal > 0 > wrist_pos_current:
judge_value = -1E-10
wrist_pos_step = (0 - wrist_pos_current) / way_points
while (0 - abs(wrist_pos_current) <=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = 0
judge_value = 1E-10
wrist_pos_step = (wrist_goal - 0) / way_points
while (wrist_goal - wrist_pos_current >=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal
elif wrist_goal < 0 < wrist_pos_current:
judge_value = -1E-10
wrist_pos_step = (0 - wrist_pos_current) / way_points
while (0 - wrist_pos_current <=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = 0
judge_value = 1E-10
wrist_pos_step = (wrist_goal - 0) / way_points
while (abs(wrist_goal) - abs(wrist_pos_current) >=
judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal
def send_wrist_goal_single(self, wrist_goal):
global judge_value, wrist_pos_current
wrist_pos = xm_JointPos()
way_points = 50
wrist_pos_step = (wrist_goal - wrist_pos_current) / way_points
rospy.loginfo("wrist_step:%lf", wrist_pos_step)
rospy.loginfo("Publish wrist's pitching angle to the topic!")
if (wrist_goal >= 0 and wrist_pos_current >= 0) or (wrist_goal <= 0 and wrist_pos_current <= 0):
if abs(wrist_goal) > abs(wrist_pos_current):
judge_value = 1E-10
while(abs(wrist_goal) - abs(wrist_pos_current) >= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
elif abs(wrist_goal) < abs(wrist_pos_current):
judge_value = -1E-10
while(abs(wrist_goal) - abs(wrist_pos_current) <= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal
elif wrist_goal > 0 > wrist_pos_current:
judge_value = -1E-10
wrist_pos_step = (0 - wrist_pos_current) / way_points
while(0 - abs(wrist_pos_current) <= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = 0
judge_value = 1E-10
wrist_pos_step = (wrist_goal - 0) / way_points
while(wrist_goal - wrist_pos_current >= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal
elif wrist_goal < 0 < wrist_pos_current:
judge_value = -1E-10
wrist_pos_step = (0 - wrist_pos_current) / way_points
while (0 - wrist_pos_current <= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = 0
judge_value = 1E-10
wrist_pos_step = (wrist_goal - 0) / way_points
while(abs(wrist_goal) - abs(wrist_pos_current) >= judge_value) and not rospy.is_shutdown():
wrist_pos_current += wrist_pos_step
wrist_pos.command = 0x01
wrist_pos.joint = 4
wrist_pos.position = wrist_pos_current
self.wrist_pos_pub.publish(wrist_pos)
rospy.sleep(0.1)
rospy.loginfo("wrist_goal_pos:%f", wrist_goal)
rospy.loginfo("wrist_current_pos:%f", wrist_pos_current)
rospy.loginfo("---")
wrist_pos_current = wrist_goal