From b52cff22d30d7839c691dbc37036b8635f01f4b3 Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Thu, 5 Sep 2024 15:45:05 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D280AR-mega2560=20ros=E4=BD=BF?= =?UTF-8?q?=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/mycobot_arduino.rviz | 65 +++++++++++++------ .../scripts/follow_display.py | 62 +++++++++--------- .../mycobot_280arduino/scripts/simple_gui.py | 2 + .../scripts/slider_control.py | 25 ++++--- .../scripts/teleop_keyboard.py | 3 +- .../mycobot_280arduino/scripts/test.py | 40 ++++++++++++ .../scripts/sync_plan.py | 30 ++++----- .../scripts/mycobot_services.py | 1 + 8 files changed, 150 insertions(+), 78 deletions(-) create mode 100644 mycobot_280/mycobot_280arduino/scripts/test.py diff --git a/mycobot_280/mycobot_280arduino/config/mycobot_arduino.rviz b/mycobot_280/mycobot_280arduino/config/mycobot_arduino.rviz index c1ab88b..cb05106 100644 --- a/mycobot_280/mycobot_280arduino/config/mycobot_arduino.rviz +++ b/mycobot_280/mycobot_280arduino/config/mycobot_arduino.rviz @@ -7,10 +7,9 @@ Panels: - /Global Options1 - /Status1 - /RobotModel1 - - /RobotModel1/Status1 - /TF1 Splitter Ratio: 0.5 - Tree Height: 645 + Tree Height: 609 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -19,17 +18,18 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -41,7 +41,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -106,16 +106,40 @@ Visualization Manager: Visual Enabled: true - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true - Marker Scale: 1 + joint1: + Value: true + joint2: + Value: true + joint3: + Value: true + joint4: + Value: true + joint5: + Value: true + joint6: + Value: true + joint6_flange: + Value: true + Marker Alpha: 1 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + joint1: + joint2: + joint3: + joint4: + joint5: + joint6: + joint6_flange: + {} Update Interval: 0 Value: true Enabled: true @@ -133,7 +157,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -143,33 +170,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.27731943 + Distance: 1.539350986480713 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.785398006 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6003978252410889 Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398006 + Yaw: 5.470398426055908 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 926 + Height: 906 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000018900000314fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000314000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000314fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000314000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000049b0000031400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000189000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000494000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -178,6 +205,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1855 - X: 65 - Y: 24 + Width: 1848 + X: 72 + Y: 27 diff --git a/mycobot_280/mycobot_280arduino/scripts/follow_display.py b/mycobot_280/mycobot_280arduino/scripts/follow_display.py index f0bbab9..5915b59 100644 --- a/mycobot_280/mycobot_280arduino/scripts/follow_display.py +++ b/mycobot_280/mycobot_280arduino/scripts/follow_display.py @@ -18,6 +18,7 @@ def talker(): print("port: {}, baud: {}\n".format(port, baud)) try: mycobot = MyCobot(port, baud) + time.sleep(2) # open serial need wait except Exception as e: print(e) print( @@ -29,7 +30,7 @@ def talker(): ) exit(1) mycobot.release_all_servos() - time.sleep(0.1) + time.sleep(2) print("Rlease all servos over.\n") pub = rospy.Publisher("joint_states", JointState, queue_size=10) @@ -56,44 +57,47 @@ def talker(): marker_.ns = "my_namespace" 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: + pass if __name__ == "__main__": diff --git a/mycobot_280/mycobot_280arduino/scripts/simple_gui.py b/mycobot_280/mycobot_280arduino/scripts/simple_gui.py index e06f6b4..db683f6 100644 --- a/mycobot_280/mycobot_280arduino/scripts/simple_gui.py +++ b/mycobot_280/mycobot_280arduino/scripts/simple_gui.py @@ -385,6 +385,7 @@ class Window: # print(c_value) try: self.set_coords(*c_value) + time.sleep(2) except ServiceException: pass self.show_j_date(c_value[:-2], "coord") @@ -402,6 +403,7 @@ class Window: try: self.set_angles(*j_value) + time.sleep(2) except ServiceException: pass self.show_j_date(j_value[:-1]) diff --git a/mycobot_280/mycobot_280arduino/scripts/slider_control.py b/mycobot_280/mycobot_280arduino/scripts/slider_control.py index e68a416..de9941a 100644 --- a/mycobot_280/mycobot_280arduino/scripts/slider_control.py +++ b/mycobot_280/mycobot_280arduino/scripts/slider_control.py @@ -20,15 +20,14 @@ mc = None def callback(data): - # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) - data_list = [] - for index, value in enumerate(data.position): - radians_to_angles = round(math.degrees(value), 2) - data_list.append(radians_to_angles) - - rospy.loginfo(rospy.get_caller_id() + "%s", data_list) - mc.send_angles(data_list, 25) + global latest_data + latest_data = [round(math.degrees(value), 2) for value in data.position] + rospy.loginfo(f"Joint angles: {latest_data}") +def control_loop(event): + if latest_data: + rospy.loginfo(f"Sending angles: {latest_data}") + mc.send_angles(latest_data, 25) def listener(): global mc @@ -39,14 +38,14 @@ def listener(): baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) - time.sleep(0.05) - # mc.set_fresh_mode(1) - time.sleep(0.05) + time.sleep(2) # open port,need wait + + # 启动定时器，每0.5秒执行一次控制循环 + rospy.Timer(rospy.Duration(0.5), control_loop) - # spin() simply keeps python from exiting until this node is stopped - print("spin ...") rospy.spin() + if __name__ == "__main__": listener() diff --git a/mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py b/mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py index efb4e3d..a8569a6 100644 --- a/mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py +++ b/mycobot_280/mycobot_280arduino/scripts/teleop_keyboard.py @@ -84,9 +84,10 @@ def teleop_keyboard(): home_pose = [0, 8, -127, 40, 0, 0, speed] # rsp = set_angles(*init_pose) - while True: res = get_coords() + time.sleep(1) + print('res:', res) if res.x > 1: break time.sleep(0.1) diff --git a/mycobot_280/mycobot_280arduino/scripts/test.py b/mycobot_280/mycobot_280arduino/scripts/test.py new file mode 100644 index 0000000..c676afa --- /dev/null +++ b/mycobot_280/mycobot_280arduino/scripts/test.py @@ -0,0 +1,40 @@ +import time +from pymycobot.mycobot import MyCobot + + +from pymycobot import * +import time +import datetime +m = MyCobot('/dev/ttyUSB0', 115200) +time.sleep(2) +delay_time = 0.1 +run_delay_time = 1 +angles = [0,0,0,0,0,0] +coords = [0,0,0,0,0,0] +angle_loss_count = 0 +coord_loss_count = 0 +total_count = 0 +send_angles = [[0,0,0,0,0,0], [0,0,0,0,20,20]] +sp = 50 +# m = MyCobot("com64", 115200) + +# time.sleep(2) #open port,need wait +# print(m.get_radians()) +while 1: + for i in range(len(send_angles)) : + angles = m.get_angles() + if (angles is None): + angle_loss_count = angle_loss_count + 1 + #angles = m.get_angles() + time.sleep(delay_time) + coords = m.get_coords() + if (coords is None): + coord_loss_count = coord_loss_count + 1 + #coords = m.get_coords() + time.sleep(delay_time) + m.send_angles(send_angles[i], sp) + time.sleep(run_delay_time) + total_count = total_count + 1 + now = datetime.datetime.now() + print(now, "angles, coords ", angles, coords, angle_loss_count, coord_loss_count, total_count) + time.sleep(delay_time) \ No newline at end of file diff --git a/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py b/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py index e046e63..8ab140b 100644 --- a/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py +++ b/mycobot_280/mycobot_280arduino_moveit/scripts/sync_plan.py @@ -11,31 +11,29 @@ mc = None def callback(data): - # rospy.loginfo(rospy.get_caller_id() + "%s", data) - data_list = [] - for index, value in enumerate(data.position): - radians_to_angles = round(math.degrees(value), 2) - data_list.append(radians_to_angles) - - rospy.loginfo(rospy.get_caller_id() + "%s", data_list) - mc.send_angles(data_list, 25) + global latest_data + latest_data = [round(math.degrees(value), 2) for value in data.position] + rospy.loginfo(f"Joint angles: {latest_data}") +def control_loop(event): + if latest_data: + rospy.loginfo(f"Sending angles: {latest_data}") + mc.send_angles(latest_data, 25) def listener(): global mc - rospy.init_node("mycobot_reciver", anonymous=True) + rospy.init_node("control_slider", anonymous=True) - port = rospy.get_param("~port", "/dev/ttyACM0") + rospy.Subscriber("joint_states", JointState, callback) + port = rospy.get_param("~port", "/dev/ttyUSB0") baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyCobot(port, baud) - time.sleep(0.05) - # mc.set_fresh_mode(1) - time.sleep(0.05) - - rospy.Subscriber("joint_states", JointState, callback) + time.sleep(2) # open port,need wait + + # 启动定时器，每0.5秒执行一次控制循环 + rospy.Timer(rospy.Duration(0.5), control_loop) - # spin() simply keeps python from exiting until this node is stopped rospy.spin() diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py index cd5fd00..4124cbe 100755 --- a/mycobot_communication/scripts/mycobot_services.py +++ b/mycobot_communication/scripts/mycobot_services.py @@ -57,6 +57,7 @@ def create_handle(): baud = rospy.get_param("~baud") rospy.loginfo("%s,%s" % (port, baud)) mc = MyCobot(port, baud) + time.sleep(2) # open serial need wait def create_services():