kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

280 模型跟随增加异常处理机制

Browse source
This commit is contained in:
wangWking 2024-12-04 15:21:41 +08:00
parent 25a3fdc95a
commit f00bbb2258
3 changed files with 93 additions and 86 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

59
mycobot_280/mycobot_280/scripts/follow_display.py
View file

@ -58,42 +58,45 @@ def talker():
print("publishing ...") print("publishing ...")
while not rospy.is_shutdown(): while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now() 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() # rospy.loginfo('{}'.format(data_list))
data_list = [] joint_state_send.position = data_list
for index, value in enumerate(angles):
data_list.append(value)
# rospy.loginfo('{}'.format(data_list)) pub.publish(joint_state_send)
joint_state_send.position = 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 position initial.标记位置初始
marker_.header.stamp = rospy.Time.now() # print(coords)
marker_.type = marker_.SPHERE if not coords:
marker_.action = marker_.ADD coords = [0, 0, 0, 0, 0, 0]
marker_.scale.x = 0.04 rospy.loginfo("error [101]: can not get coord values")
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial.标记位置初始 marker_.pose.position.x = coords[1] / 1000 * -1
# print(coords) marker_.pose.position.y = coords[0] / 1000
if not coords: marker_.pose.position.z = coords[2] / 1000
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_.color.a = 1.0
marker_.pose.position.y = coords[0] / 1000 marker_.color.g = 1.0
marker_.pose.position.z = coords[2] / 1000 pub_marker.publish(marker_)
marker_.color.a = 1.0 rate.sleep()
marker_.color.g = 1.0 except Exception as e:
pub_marker.publish(marker_) print(e)
rate.sleep()
if __name__ == "__main__": if __name__ == "__main__":

62
mycobot_280/mycobot_280jn/scripts/follow_display.py
View file

@ -65,43 +65,45 @@ def talker():
print("publishing ...") print("publishing ...")
while not rospy.is_shutdown(): while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now() 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() # rospy.loginfo('{}'.format(data_list))
print('angles:',angles) joint_state_send.position = data_list
data_list = [] print('data_list:',data_list)
for index, value in enumerate(angles): pub.publish(joint_state_send)
data_list.append(value)
# rospy.loginfo('{}'.format(data_list)) coords = mycobot.get_coords()
joint_state_send.position = data_list
print('data_list:',data_list)
pub.publish(joint_state_send)
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 position initial.标记位置初始
marker_.header.stamp = rospy.Time.now() # print(coords)
marker_.type = marker_.SPHERE if not coords:
marker_.action = marker_.ADD coords = [0, 0, 0, 0, 0, 0]
marker_.scale.x = 0.04 rospy.loginfo("error [101]: can not get coord values")
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial.标记位置初始 marker_.pose.position.x = coords[1] / 1000 * -1
# print(coords) marker_.pose.position.y = coords[0] / 1000
if not coords: marker_.pose.position.z = coords[2] / 1000
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_.color.a = 1.0
marker_.pose.position.y = coords[0] / 1000 marker_.color.g = 1.0
marker_.pose.position.z = coords[2] / 1000 pub_marker.publish(marker_)
marker_.color.a = 1.0 rate.sleep()
marker_.color.g = 1.0 except Exception as e:
pub_marker.publish(marker_) print(e)
rate.sleep()
if __name__ == "__main__": if __name__ == "__main__":

58
mycobot_280/mycobot_280pi/scripts/follow_display.py
View file

@ -59,42 +59,44 @@ def talker():
print("publishing ...") print("publishing ...")
while not rospy.is_shutdown(): while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now() 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() # rospy.loginfo('{}'.format(data_list))
data_list = [] joint_state_send.position = data_list
for index, value in enumerate(angles):
data_list.append(value)
# rospy.loginfo('{}'.format(data_list)) pub.publish(joint_state_send)
joint_state_send.position = 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 position initial.标记位置初始
marker_.header.stamp = rospy.Time.now() # print(coords)
marker_.type = marker_.SPHERE if not coords:
marker_.action = marker_.ADD coords = [0, 0, 0, 0, 0, 0]
marker_.scale.x = 0.04 rospy.loginfo("error [101]: can not get coord values")
marker_.scale.y = 0.04
marker_.scale.z = 0.04
# marker position initial.标记位置初始 marker_.pose.position.x = coords[1] / 1000 * -1
# print(coords) marker_.pose.position.y = coords[0] / 1000
if not coords: marker_.pose.position.z = coords[2] / 1000
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_.color.a = 1.0
marker_.pose.position.y = coords[0] / 1000 marker_.color.g = 1.0
marker_.pose.position.z = coords[2] / 1000 pub_marker.publish(marker_)
marker_.color.a = 1.0 rate.sleep()
marker_.color.g = 1.0 except Exception as e:
pub_marker.publish(marker_) print(e)
rate.sleep()
if __name__ == "__main__": if __name__ == "__main__":