mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
opt myarm urdf and and moveit config
This commit is contained in:
weijian
parent
91553fbc8a
commit
c3e692804b
30 changed files with 139 additions and 131 deletions
|
|
@ -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: <Fixed 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:
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm/myarm_urdf.urdf"/>
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm/myarm_urdf.urdf"/>
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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():
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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,输入部分
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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+)
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -1,12 +1,12 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_services.py" output="screen">
|
||||
<node name="myarm_services" pkg="myarm_communication" type="myarm_services.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyUSB0" />
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service，开启通讯服务 -->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics.py" output="screen">
|
||||
<node name="myarm_services" pkg="myarm_communication" type="myarm_topics.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
<arg name="baud" default="115200" />
|
||||
|
||||
<!-- Open communication service --><!-- 开启通讯服务 -->
|
||||
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.py" output="screen">
|
||||
<node name="myarm_services" pkg="myarm_communication" type="myarm_topics_pi.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -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()
|
||||
|
|
@ -8,4 +8,4 @@ moveit_setup_assistant_config:
|
|||
CONFIG:
|
||||
author_name: zachary
|
||||
author_email: lijun.zhang@elephantrobotics.com
|
||||
generated_timestamp: 1685513868
|
||||
generated_timestamp: 1686548761
|
||||
|
|
@ -10,7 +10,5 @@ controller_list:
|
|||
- joint6_to_joint5
|
||||
- joint7_to_joint6
|
||||
initial: # Define initial robot poses per group
|
||||
# - group: arm_group
|
||||
# pose: home
|
||||
|
||||
[]
|
||||
- group: arm_group
|
||||
pose: init_pose
|
||||
|
|
@ -12,6 +12,16 @@
|
|||
<group name="arm_group">
|
||||
<chain base_link="base" tip_link="joint7"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state name="init_pose" group="arm_group">
|
||||
<joint name="joint1_to_base" value="0"/>
|
||||
<joint name="joint2_to_joint1" value="0"/>
|
||||
<joint name="joint3_to_joint2" value="0"/>
|
||||
<joint name="joint4_to_joint3" value="0"/>
|
||||
<joint name="joint5_to_joint4" value="0"/>
|
||||
<joint name="joint6_to_joint5" value="0"/>
|
||||
<joint name="joint7_to_joint6" value="0"/>
|
||||
</group_state>
|
||||
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
|
||||
<virtual_joint name="vitual_joint" type="fixed" parent_frame="world" child_link="base"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
|
|
@ -39,7 +49,6 @@
|
|||
<disable_collisions link1="joint3" link2="joint7" reason="Always"/>
|
||||
<disable_collisions link1="joint4" link2="joint5" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint4" link2="joint6" reason="Default"/>
|
||||
<disable_collisions link1="joint4" link2="joint7" reason="Never"/>
|
||||
<disable_collisions link1="joint5" link2="joint6" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint5" link2="joint7" reason="Always"/>
|
||||
<disable_collisions link1="joint6" link2="joint7" reason="Adjacent"/>
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
<arg name="paused" default="false" doc="Start Gazebo paused"/>
|
||||
<arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
|
||||
<arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
|
||||
<arg name="initial_joint_positions" default="" doc="Initial joint configuration of the robot"/>
|
||||
<arg name="initial_joint_positions" default=" -J joint1_to_base 0 -J joint2_to_joint1 0 -J joint3_to_joint2 0 -J joint4_to_joint3 0 -J joint5_to_joint4 0 -J joint6_to_joint5 0 -J joint7_to_joint6 0" doc="Initial joint configuration of the robot"/>
|
||||
|
||||
<!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -39,13 +39,13 @@
|
|||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j2.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 0.0 " rpy = " 0 3.14159 1.5708"/>
|
||||
<origin xyz = "0.0 0 0.0 " rpy = " 3.14159 3.14159 1.5708 "/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j2.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 0.0 " rpy = " 0 3.14159 1.5708"/>
|
||||
<origin xyz = "0.0 0 0.0 " rpy = " 3.14159 3.14159 1.5708"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -55,14 +55,14 @@
|
|||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j3.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 -0.054 " rpy = " 0 0 0"/>
|
||||
<origin xyz = "0.0 0 -0.054 " rpy = " 0 0 3.14159"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j3.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 0.054" rpy = " 0 0 0"/>
|
||||
<origin xyz = "0.0 0 0.054" rpy = " 0 0 3.14159"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -72,13 +72,13 @@
|
|||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j4.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 0 " rpy = " 3.14159 0 -1.5708"/>
|
||||
<origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j4.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 -0.03356 " rpy = " -1.5708 0 0"/>
|
||||
<origin xyz = "0.0 0 -0.03356 " rpy = " 0 0 -1.5708"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -89,14 +89,14 @@
|
|||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j5.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0 0.00 -0.051 " rpy = " 0 0 0"/>
|
||||
<origin xyz = "0 0.00 -0.051 " rpy = " 0 0 3.14159"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j5.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0 0.00 -0.038 " rpy = " 0 0 0"/>
|
||||
<origin xyz = "0 0.00 -0.038 " rpy = " 0 0 3.14159"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -114,7 +114,7 @@
|
|||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j6.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 -0.0 " rpy = " 0 0 0"/>
|
||||
<origin xyz = "0.0 0 -0.0 " rpy = " 0 0 -1.5708"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -131,7 +131,7 @@
|
|||
|
||||
<mesh filename="package://mycobot_description/urdf/myarm/j7.dae"/>
|
||||
</geometry>
|
||||
<origin xyz = "0.0 0 -0.0 " rpy = " 0 0 0"/>
|
||||
<origin xyz = "0.0 0 -0.0 " rpy = " 0 0 -1.5708"/>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
|
|
@ -151,7 +151,7 @@
|
|||
<limit effort = "1000.0" lower = "-1.5708" upper = "1.5708" velocity = "0"/>
|
||||
<parent link="joint1"/>
|
||||
<child link="joint2"/>
|
||||
<origin xyz= "0 0 0" rpy = "-1.5708 0 0"/>
|
||||
<origin xyz= "0 0 0" rpy = "1.5708 3.14159 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue