From c3e692804bff76dac84fe2dfe25c2b723175273d Mon Sep 17 00:00:00 2001 From: weijian Date: Mon, 12 Jun 2023 14:58:02 +0800 Subject: [PATCH] opt myarm urdf and and moveit config --- myArm/myarm/config/myarm.rviz | 18 ++++--- .../launch/detect_marker_with_topic.launch | 2 +- myArm/myarm/launch/simple_gui.launch | 2 +- myArm/myarm/launch/teleop_keyboard.launch | 2 +- myArm/myarm/scripts/follow_display.py | 2 +- myArm/myarm/scripts/listen_real.py | 2 +- myArm/myarm/scripts/listen_real_of_topic.py | 8 +-- myArm/myarm/scripts/simple_gui.py | 2 +- myArm/myarm/scripts/slider_control.py | 2 +- myArm/myarm/scripts/teleop_keyboard.py | 4 +- myArm/myarm_communication/CMakeLists.txt | 16 +++--- .../launch/communication_service.launch | 4 +- .../launch/communication_topic.launch | 4 +- .../launch/communication_topic_pi.launch | 4 +- .../{MycobotAngles.msg => MyArmAngles.msg} | 0 .../{MycobotCoords.msg => MyArmCoords.msg} | 0 ...ipperStatus.msg => MyArmGripperStatus.msg} | 0 ...obotPumpStatus.msg => MyArmPumpStatus.msg} | 0 ...ycobotSetAngles.msg => MyArmSetAngles.msg} | 0 ...ycobotSetCoords.msg => MyArmSetCoords.msg} | 0 ...{mycobot_services.py => myarm_services.py} | 33 ++++++------ .../{mycobot_topics.py => myarm_topics.py} | 50 ++++++++++--------- ...ycobot_topics_pi.py => myarm_topics_pi.py} | 50 +++++++++---------- myArm/myarm_moveit/.setup_assistant | 2 +- .../myarm_moveit/config/fake_controllers.yaml | 6 +-- myArm/myarm_moveit/config/firefighter.srdf | 11 +++- myArm/myarm_moveit/launch/gazebo.launch | 2 +- myArm/myarm_moveit/launch/moveit.rviz | 20 +++----- myArm/myarm_moveit/scripts/sync_plan.py | 2 +- .../urdf/myarm/myarm_urdf.urdf | 22 ++++---- 30 files changed, 139 insertions(+), 131 deletions(-) rename myArm/myarm_communication/msg/{MycobotAngles.msg => MyArmAngles.msg} (100%) rename myArm/myarm_communication/msg/{MycobotCoords.msg => MyArmCoords.msg} (100%) rename myArm/myarm_communication/msg/{MycobotGripperStatus.msg => MyArmGripperStatus.msg} (100%) rename myArm/myarm_communication/msg/{MycobotPumpStatus.msg => MyArmPumpStatus.msg} (100%) rename myArm/myarm_communication/msg/{MycobotSetAngles.msg => MyArmSetAngles.msg} (100%) rename myArm/myarm_communication/msg/{MycobotSetCoords.msg => MyArmSetCoords.msg} (100%) rename myArm/myarm_communication/scripts/{mycobot_services.py => myarm_services.py} (87%) rename myArm/myarm_communication/scripts/{mycobot_topics.py => myarm_topics.py} (83%) rename myArm/myarm_communication/scripts/{mycobot_topics_pi.py => myarm_topics_pi.py} (83%) diff --git a/myArm/myarm/config/myarm.rviz b/myArm/myarm/config/myarm.rviz index 5e56d99..857b8c2 100755 --- a/myArm/myarm/config/myarm.rviz +++ b/myArm/myarm/config/myarm.rviz @@ -10,7 +10,7 @@ Panels: - /TF1 - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 591 + Tree Height: 603 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -121,13 +121,15 @@ Visualization Manager: Value: true - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: - All Enabled: false + All Enabled: true base: Value: true joint1: - Value: false + Value: true joint2: Value: true joint3: @@ -186,7 +188,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.327453851699829 + Distance: 1.444238305091858 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -202,17 +204,17 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.23039796948432922 + Pitch: 0.36039817333221436 Target Frame: - Yaw: 3.0053975582122803 + Yaw: 2.790395975112915 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 888 + Height: 900 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002da000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002e6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/myArm/myarm/launch/detect_marker_with_topic.launch b/myArm/myarm/launch/detect_marker_with_topic.launch index 04a0d5e..c0ec280 100755 --- a/myArm/myarm/launch/detect_marker_with_topic.launch +++ b/myArm/myarm/launch/detect_marker_with_topic.launch @@ -1,6 +1,6 @@ - + diff --git a/myArm/myarm/launch/simple_gui.launch b/myArm/myarm/launch/simple_gui.launch index c389ea9..05c21e9 100755 --- a/myArm/myarm/launch/simple_gui.launch +++ b/myArm/myarm/launch/simple_gui.launch @@ -1,6 +1,6 @@ - + diff --git a/myArm/myarm/launch/teleop_keyboard.launch b/myArm/myarm/launch/teleop_keyboard.launch index 339660f..bd6ae27 100755 --- a/myArm/myarm/launch/teleop_keyboard.launch +++ b/myArm/myarm/launch/teleop_keyboard.launch @@ -1,6 +1,6 @@ - + diff --git a/myArm/myarm/scripts/follow_display.py b/myArm/myarm/scripts/follow_display.py index 465e723..20634e3 100755 --- a/myArm/myarm/scripts/follow_display.py +++ b/myArm/myarm/scripts/follow_display.py @@ -13,7 +13,7 @@ def talker(): rospy.init_node("display", anonymous=True) print("Try connect real mycobot...") - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyAMA0") baud = rospy.get_param("~baud", 115200) print("port: {}, baud: {}\n".format(port, baud)) try: diff --git a/myArm/myarm/scripts/listen_real.py b/myArm/myarm/scripts/listen_real.py index 7b79fdf..7ac6b97 100755 --- a/myArm/myarm/scripts/listen_real.py +++ b/myArm/myarm/scripts/listen_real.py @@ -7,7 +7,7 @@ import math import rospy from sensor_msgs.msg import JointState from std_msgs.msg import Header -from mycobot_communication.srv import GetAngles +from myarm_communication.srv import GetAngles def talker(): diff --git a/myArm/myarm/scripts/listen_real_of_topic.py b/myArm/myarm/scripts/listen_real_of_topic.py index 37e1ff4..daf3194 100755 --- a/myArm/myarm/scripts/listen_real_of_topic.py +++ b/myArm/myarm/scripts/listen_real_of_topic.py @@ -5,7 +5,7 @@ import math import rospy from sensor_msgs.msg import JointState from std_msgs.msg import Header -from mycobot_communication.msg import MycobotAngles +from myarm_communication.msg import MyArmAngles class Listener(object): @@ -17,14 +17,14 @@ class Listener(object): # init publisher.初始化发布者 self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) # init subscriber.初始化订阅者 - self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback) + self.sub = rospy.Subscriber("myarm/angles_real", MyArmAngles, self.callback) rospy.spin() def callback(self, data): - """`mycobot/angles_real` subscriber callback method. + """`myarm/angles_real` subscriber callback method. Args: - data (MycobotAngles): callback argument. + data (MyArmAngles): callback argument. """ # ini publisher object. 初始化发布者对象 joint_state_send = JointState() diff --git a/myArm/myarm/scripts/simple_gui.py b/myArm/myarm/scripts/simple_gui.py index 69ccc0d..dab365e 100755 --- a/myArm/myarm/scripts/simple_gui.py +++ b/myArm/myarm/scripts/simple_gui.py @@ -36,7 +36,7 @@ class Window: # 计算 Tk 根窗口的 x 和 y 坐标 x = int((self.ws / 2) - 190) y = int((self.hs / 2) - 250) - self.win.geometry("430x400+{}+{}".format(x, y)) + self.win.geometry("430x450+{}+{}".format(x, y)) # layout,布局 self.set_layout() # input section,输入部分 diff --git a/myArm/myarm/scripts/slider_control.py b/myArm/myarm/scripts/slider_control.py index 0b722ca..a5f6ce7 100755 --- a/myArm/myarm/scripts/slider_control.py +++ b/myArm/myarm/scripts/slider_control.py @@ -34,7 +34,7 @@ def listener(): rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyAMA0") baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyArm(port, baud) diff --git a/myArm/myarm/scripts/teleop_keyboard.py b/myArm/myarm/scripts/teleop_keyboard.py index 6bb47af..3f8ead5 100755 --- a/myArm/myarm/scripts/teleop_keyboard.py +++ b/myArm/myarm/scripts/teleop_keyboard.py @@ -1,6 +1,6 @@ #!/usr/bin/env python from __future__ import print_function -from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus +from myarm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import sys import select @@ -12,7 +12,7 @@ import roslib # Terminal output prompt information. 终端输出提示信息 msg = """\ -Mycobot Teleop Keyboard Controller +MyArm Teleop Keyboard Controller --------------------------- Movimg options(control coordinations [x,y,z,rx,ry,rz]): w(x+) diff --git a/myArm/myarm_communication/CMakeLists.txt b/myArm/myarm_communication/CMakeLists.txt index 70f145d..370415b 100755 --- a/myArm/myarm_communication/CMakeLists.txt +++ b/myArm/myarm_communication/CMakeLists.txt @@ -50,12 +50,12 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files(FILES - MycobotAngles.msg - MycobotCoords.msg - MycobotSetAngles.msg - MycobotSetCoords.msg - MycobotGripperStatus.msg - MycobotPumpStatus.msg + MyArmAngles.msg + MyArmCoords.msg + MyArmSetAngles.msg + MyArmSetCoords.msg + MyArmGripperStatus.msg + MyArmPumpStatus.msg ) ## Generate services in the 'srv' folder @@ -169,8 +169,8 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS - scripts/mycobot_services.py - scripts/mycobot_topics.py + scripts/myarm_services.py + scripts/myarm_topics.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/myArm/myarm_communication/launch/communication_service.launch b/myArm/myarm_communication/launch/communication_service.launch index b2b21b0..b4a9746 100755 --- a/myArm/myarm_communication/launch/communication_service.launch +++ b/myArm/myarm_communication/launch/communication_service.launch @@ -1,12 +1,12 @@ - + - + diff --git a/myArm/myarm_communication/launch/communication_topic.launch b/myArm/myarm_communication/launch/communication_topic.launch index 633f1bd..1eacf83 100755 --- a/myArm/myarm_communication/launch/communication_topic.launch +++ b/myArm/myarm_communication/launch/communication_topic.launch @@ -1,10 +1,10 @@ - + - + diff --git a/myArm/myarm_communication/launch/communication_topic_pi.launch b/myArm/myarm_communication/launch/communication_topic_pi.launch index c0c3fe9..08a0841 100755 --- a/myArm/myarm_communication/launch/communication_topic_pi.launch +++ b/myArm/myarm_communication/launch/communication_topic_pi.launch @@ -1,10 +1,10 @@ - + - + diff --git a/myArm/myarm_communication/msg/MycobotAngles.msg b/myArm/myarm_communication/msg/MyArmAngles.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotAngles.msg rename to myArm/myarm_communication/msg/MyArmAngles.msg diff --git a/myArm/myarm_communication/msg/MycobotCoords.msg b/myArm/myarm_communication/msg/MyArmCoords.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotCoords.msg rename to myArm/myarm_communication/msg/MyArmCoords.msg diff --git a/myArm/myarm_communication/msg/MycobotGripperStatus.msg b/myArm/myarm_communication/msg/MyArmGripperStatus.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotGripperStatus.msg rename to myArm/myarm_communication/msg/MyArmGripperStatus.msg diff --git a/myArm/myarm_communication/msg/MycobotPumpStatus.msg b/myArm/myarm_communication/msg/MyArmPumpStatus.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotPumpStatus.msg rename to myArm/myarm_communication/msg/MyArmPumpStatus.msg diff --git a/myArm/myarm_communication/msg/MycobotSetAngles.msg b/myArm/myarm_communication/msg/MyArmSetAngles.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotSetAngles.msg rename to myArm/myarm_communication/msg/MyArmSetAngles.msg diff --git a/myArm/myarm_communication/msg/MycobotSetCoords.msg b/myArm/myarm_communication/msg/MyArmSetCoords.msg similarity index 100% rename from myArm/myarm_communication/msg/MycobotSetCoords.msg rename to myArm/myarm_communication/msg/MyArmSetCoords.msg diff --git a/myArm/myarm_communication/scripts/mycobot_services.py b/myArm/myarm_communication/scripts/myarm_services.py similarity index 87% rename from myArm/myarm_communication/scripts/mycobot_services.py rename to myArm/myarm_communication/scripts/myarm_services.py index af4dbf7..64b5f1c 100755 --- a/myArm/myarm_communication/scripts/mycobot_services.py +++ b/myArm/myarm_communication/scripts/myarm_services.py @@ -1,12 +1,12 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # -*- coding: utf-8 -* import time import rospy import os import fcntl -from mycobot_communication.srv import * +from myarm_communication.srv import * -from pymycobot.mycobot import MyCobot +from pymycobot.myarm import MyArm mc = None @@ -51,12 +51,12 @@ def release(lock_file_fd): def create_handle(): global mc - rospy.init_node("mycobot_services") + rospy.init_node("myarm") rospy.loginfo("start ...") port = rospy.get_param("~port") baud = rospy.get_param("~baud") rospy.loginfo("%s,%s" % (port, baud)) - mc = MyCobot(port, baud) + mc = MyArm(port, baud) def create_services(): @@ -79,11 +79,12 @@ def set_angles(req): req.joint_4, req.joint_5, req.joint_6, + req.joint_7, ] sp = req.speed if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") mc.send_angles(angles, sp) release(lock) @@ -93,7 +94,7 @@ def set_angles(req): def get_angles(req): """get angles,获取角度""" if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") angles = mc.get_angles() release(lock) return GetAnglesResponse(*angles) @@ -112,7 +113,7 @@ def set_coords(req): mod = req.model if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") mc.send_coords(coords, sp, mod) release(lock) @@ -121,7 +122,7 @@ def set_coords(req): def get_coords(req): if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") coords = mc.get_coords() release(lock) return GetCoordsResponse(*coords) @@ -131,7 +132,7 @@ def switch_status(req): """Gripper switch status""" """夹爪开关状态""" if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") if req.Status: mc.set_gripper_state(0, 80) else: @@ -143,7 +144,7 @@ def switch_status(req): def toggle_pump(req): if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") if req.Status: mc.set_basic_output(req.Pin1, 0) mc.set_basic_output(req.Pin2, 0) @@ -157,10 +158,10 @@ def toggle_pump(req): robot_msg = """ -MyCobot Status +MyArm Status -------------------------------- Joint Limit: - joint 1: -170 ~ +170 + joint 1: -180 ~ +180 joint 2: -170 ~ +170 joint 3: -170 ~ +170 joint 4: -170 ~ +170 @@ -184,13 +185,13 @@ def output_robot_message(): atom_version = "unknown" if mc: - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") cn = mc.is_controller_connected() release(lock) if cn == 1: connect_status = True time.sleep(0.1) - lock = acquire("/tmp/mycobot_lock") + lock = acquire("/tmp/myarm_lock") si = mc.is_all_servo_enable() release(lock) if si == 1: @@ -203,7 +204,7 @@ def output_robot_message(): if __name__ == "__main__": - # print(MyCobot.__dict__) + # print(MyArm.__dict__) create_handle() output_robot_message() create_services() diff --git a/myArm/myarm_communication/scripts/mycobot_topics.py b/myArm/myarm_communication/scripts/myarm_topics.py similarity index 83% rename from myArm/myarm_communication/scripts/mycobot_topics.py rename to myArm/myarm_communication/scripts/myarm_topics.py index 5039b0d..9b84e37 100755 --- a/myArm/myarm_communication/scripts/mycobot_topics.py +++ b/myArm/myarm_communication/scripts/myarm_topics.py @@ -8,16 +8,16 @@ import threading import rospy -from mycobot_communication.msg import ( - MycobotAngles, - MycobotCoords, - MycobotSetAngles, - MycobotSetCoords, - MycobotGripperStatus, - MycobotPumpStatus, +from myarm_communication.msg import ( + MyArmAngles, + MyArmCoords, + MyArmSetAngles, + MyArmSetCoords, + MyArmGripperStatus, + MyArmPumpStatus, ) -from pymycobot.mycobot import MyCobot +from pymycobot.myarm import MyArm class Watcher: @@ -64,18 +64,18 @@ class Watcher: pass -class MycobotTopics(object): +class MyarmTopics(object): def __init__(self): - super(MycobotTopics, self).__init__() + super(MyarmTopics, self).__init__() - rospy.init_node("mycobot_topics") + rospy.init_node("myarm_topics") rospy.loginfo("start ...") - port = rospy.get_param("~port", os.popen("ls /dev/ttyUSB*").readline()[:-1]) + port = rospy.get_param("~port", os.popen("ls /dev/ttyAMA*").readline()[:-1]) if not port: port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) - self.mc = MyCobot(port, baud) + self.mc = MyArm(port, baud) self.lock = threading.Lock() def start(self): @@ -109,9 +109,9 @@ class MycobotTopics(object): def pub_real_angles(self): """Publish real angle""" """发布真实角度""" - pub = rospy.Publisher("mycobot/angles_real", - MycobotAngles, queue_size=5) - ma = MycobotAngles() + pub = rospy.Publisher("myarm/angles_real", + MyArmAngles, queue_size=5) + ma = MyArmAngles() while not rospy.is_shutdown(): self.lock.acquire() angles = self.mc.get_angles() @@ -123,15 +123,16 @@ class MycobotTopics(object): ma.joint_4 = angles[3] ma.joint_5 = angles[4] ma.joint_6 = angles[5] + ma.joint_7 = angles[6] pub.publish(ma) time.sleep(0.25) def pub_real_coords(self): """publish real coordinates""" """发布真实坐标""" - pub = rospy.Publisher("mycobot/coords_real", - MycobotCoords, queue_size=5) - ma = MycobotCoords() + pub = rospy.Publisher("myarm/coords_real", + MyArmCoords, queue_size=5) + ma = MyArmCoords() while not rospy.is_shutdown(): self.lock.acquire() @@ -158,12 +159,13 @@ class MycobotTopics(object): data.joint_4, data.joint_5, data.joint_6, + data.joint_7, ] sp = int(data.speed) self.mc.send_angles(angles, sp) sub = rospy.Subscriber( - "mycobot/angles_goal", MycobotSetAngles, callback=callback + "myarm/angles_goal", MyArmSetAngles, callback=callback ) rospy.spin() @@ -175,7 +177,7 @@ class MycobotTopics(object): self.mc.send_coords(angles, sp, model) sub = rospy.Subscriber( - "mycobot/coords_goal", MycobotSetCoords, callback=callback + "myarm/coords_goal", MyArmSetCoords, callback=callback ) rospy.spin() @@ -189,7 +191,7 @@ class MycobotTopics(object): self.mc.set_gripper_state(1, 80) sub = rospy.Subscriber( - "mycobot/gripper_status", MycobotGripperStatus, callback=callback + "myarm/gripper_status", MyArmGripperStatus, callback=callback ) rospy.spin() @@ -203,14 +205,14 @@ class MycobotTopics(object): self.mc.set_basic_output(data.Pin2, 1) sub = rospy.Subscriber( - "mycobot/pump_status", MycobotPumpStatus, callback=callback + "myarm/pump_status", MyArmPumpStatus, callback=callback ) rospy.spin() if __name__ == "__main__": Watcher() - mc_topics = MycobotTopics() + mc_topics = MyarmTopics() mc_topics.start() # while True: # mc_topics.pub_real_coords() diff --git a/myArm/myarm_communication/scripts/mycobot_topics_pi.py b/myArm/myarm_communication/scripts/myarm_topics_pi.py similarity index 83% rename from myArm/myarm_communication/scripts/mycobot_topics_pi.py rename to myArm/myarm_communication/scripts/myarm_topics_pi.py index 2c0bcb4..89e9ed2 100755 --- a/myArm/myarm_communication/scripts/mycobot_topics_pi.py +++ b/myArm/myarm_communication/scripts/myarm_topics_pi.py @@ -8,18 +8,18 @@ import threading import rospy -from mycobot_communication.msg import ( - MycobotAngles, - MycobotCoords, - MycobotSetAngles, - MycobotSetCoords, - MycobotGripperStatus, - MycobotPumpStatus, +from myarm_communication.msg import ( + MyArmAngles, + MyArmCoords, + MyArmSetAngles, + MyArmSetCoords, + MyArmGripperStatus, + MyArmPumpStatus, ) -from pymycobot import MyCobot -# from pymycobot import MyCobotSocket +from pymycobot import MyArm +# from pymyarm import myarmSocket class Watcher: @@ -67,19 +67,17 @@ class Watcher: pass -class MycobotTopics(object): +class MyArmTopics(object): def __init__(self): - super(MycobotTopics, self).__init__() + super(MyArmTopics, self).__init__() - rospy.init_node("mycobot_topics_pi") + rospy.init_node("myarm_topics_pi") rospy.loginfo("start ...") # problem port = rospy.get_param("~port", os.popen("ls /dev/ttyAMA*").readline()[:-1]) - baud = rospy.get_param("~baud", 1000000) + baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) - # self.mc = MyCobotSocket(port, baud) # port - # self.mc.connect() #pi - self.mc = MyCobot(port, baud) + self.mc = MyArm(port, baud) self.lock = threading.Lock() def start(self): @@ -113,8 +111,8 @@ class MycobotTopics(object): def pub_real_angles(self): """Publish real angle""" """发布真实角度""" - pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) - ma = MycobotAngles() + pub = rospy.Publisher("myarm/angles_real", MyArmAngles, queue_size=5) + ma = MyArmAngles() while not rospy.is_shutdown(): self.lock.acquire() angles = self.mc.get_angles() @@ -126,14 +124,15 @@ class MycobotTopics(object): ma.joint_4 = angles[3] ma.joint_5 = angles[4] ma.joint_6 = angles[5] + ma.joint_7 = angles[6] pub.publish(ma) time.sleep(0.25) def pub_real_coords(self): """publish real coordinates""" """发布真实坐标""" - pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) - ma = MycobotCoords() + pub = rospy.Publisher("myarm/coords_real", MyArmCoords, queue_size=5) + ma = MyArmCoords() while not rospy.is_shutdown(): self.lock.acquire() @@ -160,12 +159,13 @@ class MycobotTopics(object): data.joint_4, data.joint_5, data.joint_6, + data.joint_7, ] sp = int(data.speed) self.mc.send_angles(angles, sp) sub = rospy.Subscriber( - "mycobot/angles_goal", MycobotSetAngles, callback=callback + "myarm/angles_goal", MyArmSetAngles, callback=callback ) rospy.spin() @@ -177,7 +177,7 @@ class MycobotTopics(object): self.mc.send_coords(angles, sp, model) sub = rospy.Subscriber( - "mycobot/coords_goal", MycobotSetCoords, callback=callback + "myarm/coords_goal", MyArmSetCoords, callback=callback ) rospy.spin() @@ -191,7 +191,7 @@ class MycobotTopics(object): self.mc.set_gripper_state(1, 80) sub = rospy.Subscriber( - "mycobot/gripper_status", MycobotGripperStatus, callback=callback + "myarm/gripper_status", MyArmGripperStatus, callback=callback ) rospy.spin() @@ -205,13 +205,13 @@ class MycobotTopics(object): self.mc.set_basic_output(data.Pin2, 1) sub = rospy.Subscriber( - "mycobot/pump_status", MycobotPumpStatus, callback=callback + "myarm/pump_status", MyArmPumpStatus, callback=callback ) rospy.spin() if __name__ == "__main__": Watcher() - mc_topics = MycobotTopics() + mc_topics = MyArmTopics() mc_topics.start() # while True: # mc_topics.pub_real_coords() diff --git a/myArm/myarm_moveit/.setup_assistant b/myArm/myarm_moveit/.setup_assistant index a456ab1..db6fa2b 100644 --- a/myArm/myarm_moveit/.setup_assistant +++ b/myArm/myarm_moveit/.setup_assistant @@ -8,4 +8,4 @@ moveit_setup_assistant_config: CONFIG: author_name: zachary author_email: lijun.zhang@elephantrobotics.com - generated_timestamp: 1685513868 \ No newline at end of file + generated_timestamp: 1686548761 \ No newline at end of file diff --git a/myArm/myarm_moveit/config/fake_controllers.yaml b/myArm/myarm_moveit/config/fake_controllers.yaml index b7ddb35..0f74054 100644 --- a/myArm/myarm_moveit/config/fake_controllers.yaml +++ b/myArm/myarm_moveit/config/fake_controllers.yaml @@ -10,7 +10,5 @@ controller_list: - joint6_to_joint5 - joint7_to_joint6 initial: # Define initial robot poses per group -# - group: arm_group -# pose: home - - [] \ No newline at end of file + - group: arm_group + pose: init_pose \ No newline at end of file diff --git a/myArm/myarm_moveit/config/firefighter.srdf b/myArm/myarm_moveit/config/firefighter.srdf index 1fc2d7b..2d56054 100644 --- a/myArm/myarm_moveit/config/firefighter.srdf +++ b/myArm/myarm_moveit/config/firefighter.srdf @@ -12,6 +12,16 @@ + + + + + + + + + + @@ -39,7 +49,6 @@ - diff --git a/myArm/myarm_moveit/launch/gazebo.launch b/myArm/myarm_moveit/launch/gazebo.launch index 6167c5b..350f259 100644 --- a/myArm/myarm_moveit/launch/gazebo.launch +++ b/myArm/myarm_moveit/launch/gazebo.launch @@ -5,7 +5,7 @@ - + diff --git a/myArm/myarm_moveit/launch/moveit.rviz b/myArm/myarm_moveit/launch/moveit.rviz index f5ce96d..cde65dd 100644 --- a/myArm/myarm_moveit/launch/moveit.rviz +++ b/myArm/myarm_moveit/launch/moveit.rviz @@ -5,13 +5,9 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Scene Geometry1 - - /MotionPlanning1/Scene Robot1 - - /MotionPlanning1/Planning Request1 - - /MotionPlanning1/Planning Metrics1 - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5 - Tree Height: 99 + Tree Height: 155 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -222,7 +218,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.2993544340133667 + Distance: 1.7599999904632568 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -238,14 +234,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.13999994099140167 + Pitch: 0.44000014662742615 Target Frame: base - Yaw: 2.949949026107788 + Yaw: 2.44994854927063 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 888 + Height: 848 Help: collapsed: false Hide Left Dock: false @@ -254,9 +250,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000031efc0200000007fb000000100044006900730070006c006100790073010000003d000000f4000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000001370000004f0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018c000001cf0000017d00ffffff0000053f0000031e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d0000012c000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000016f000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1848 - X: 72 + Width: 1291 + X: 454 Y: 27 diff --git a/myArm/myarm_moveit/scripts/sync_plan.py b/myArm/myarm_moveit/scripts/sync_plan.py index 0b722ca..a5f6ce7 100644 --- a/myArm/myarm_moveit/scripts/sync_plan.py +++ b/myArm/myarm_moveit/scripts/sync_plan.py @@ -34,7 +34,7 @@ def listener(): rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyAMA0") baud = rospy.get_param("~baud", 115200) print(port, baud) mc = MyArm(port, baud) diff --git a/mycobot_description/urdf/myarm/myarm_urdf.urdf b/mycobot_description/urdf/myarm/myarm_urdf.urdf index f2f4498..9d7a756 100644 --- a/mycobot_description/urdf/myarm/myarm_urdf.urdf +++ b/mycobot_description/urdf/myarm/myarm_urdf.urdf @@ -39,13 +39,13 @@ - + - + @@ -55,14 +55,14 @@ - + - + @@ -72,13 +72,13 @@ - + - + @@ -89,14 +89,14 @@ - + - + @@ -114,7 +114,7 @@ - + @@ -131,7 +131,7 @@ - + @@ -151,7 +151,7 @@ - +