mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
280 模型跟随增加异常处理机制
This commit is contained in:
wangWking
parent
25a3fdc95a
commit
f00bbb2258
3 changed files with 93 additions and 86 deletions
|
|
@ -59,41 +59,44 @@ def talker():
|
|||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
angles = mycobot.get_radians()
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
try:
|
||||
angles = mycobot.get_radians()
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
|
||||
pub.publish(joint_state_send)
|
||||
pub.publish(joint_state_send)
|
||||
|
||||
coords = mycobot.get_coords()
|
||||
coords = mycobot.get_coords()
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
rate.sleep()
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -65,43 +65,45 @@ def talker():
|
|||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
try:
|
||||
angles = mycobot.get_radians()
|
||||
print('angles:',angles)
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
|
||||
angles = mycobot.get_radians()
|
||||
print('angles:',angles)
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
print('data_list:',data_list)
|
||||
pub.publish(joint_state_send)
|
||||
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
print('data_list:',data_list)
|
||||
pub.publish(joint_state_send)
|
||||
coords = mycobot.get_coords()
|
||||
|
||||
coords = mycobot.get_coords()
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
rate.sleep()
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -59,42 +59,44 @@ def talker():
|
|||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
try:
|
||||
angles = mycobot.get_radians()
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
|
||||
angles = mycobot.get_radians()
|
||||
data_list = []
|
||||
for index, value in enumerate(angles):
|
||||
data_list.append(value)
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
|
||||
# rospy.loginfo('{}'.format(data_list))
|
||||
joint_state_send.position = data_list
|
||||
pub.publish(joint_state_send)
|
||||
|
||||
pub.publish(joint_state_send)
|
||||
coords = mycobot.get_coords()
|
||||
|
||||
coords = mycobot.get_coords()
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = rospy.Time.now()
|
||||
marker_.type = marker_.SPHERE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
|
||||
# marker position initial.标记位置初始
|
||||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
marker_.pose.position.z = coords[2] / 1000
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
rate.sleep()
|
||||
except Exception as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue