mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
chore: format code.
This commit is contained in:
Zachary
parent
586b86b3c8
commit
cd4ad7922c
19 changed files with 780 additions and 617 deletions
|
|
@ -3,14 +3,15 @@ import cv2
|
|||
import cv2.aruco as aruco
|
||||
import glob
|
||||
|
||||
|
||||
def calibrateKd(im_fpath, aruco_len=60.0):
|
||||
(w, h) = (6, 4)
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)
|
||||
objp = np.zeros((w*h, 3), np.float32)
|
||||
objp = np.zeros((w * h, 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
|
||||
objp *= aruco_len
|
||||
objpoints, imgpoints = [], []
|
||||
images = glob.glob(f'{im_fpath}/*')
|
||||
images = glob.glob(f"{im_fpath}/*")
|
||||
for fname in images:
|
||||
img = cv2.imread(fname)
|
||||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
|
|
@ -21,11 +22,13 @@ def calibrateKd(im_fpath, aruco_len=60.0):
|
|||
imgpoints.append(corners2)
|
||||
img = cv2.drawChessboardCorners(img, (6, 4), corners2, ret)
|
||||
|
||||
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shap[::-1], None, None)
|
||||
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
|
||||
objpoints, imgpoints, gray.shap[::-1], None, None
|
||||
)
|
||||
tot_error = 0
|
||||
for i in range(len(objpoints)):
|
||||
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
|
||||
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2)/len(imgpoints2)
|
||||
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
|
||||
tot_error += error
|
||||
print(tot_error)
|
||||
return mtx, dist
|
||||
|
|
|
|||
|
|
@ -5,15 +5,25 @@ from mycobot_ros.srv import *
|
|||
|
||||
|
||||
def test():
|
||||
rospy.wait_for_service('get_joint_angles')
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
try:
|
||||
func = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
res = func(*[104.9000015258789, -61.79999923706055, 381.0, -
|
||||
106.43000030517578, 2.1600000858306885, -87.80999755859375, 50, 0])
|
||||
print('res:', res)
|
||||
func = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
res = func(
|
||||
*[
|
||||
104.9000015258789,
|
||||
-61.79999923706055,
|
||||
381.0,
|
||||
-106.43000030517578,
|
||||
2.1600000858306885,
|
||||
-87.80999755859375,
|
||||
50,
|
||||
0,
|
||||
]
|
||||
)
|
||||
print("res:", res)
|
||||
except:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
test()
|
||||
|
|
|
|||
|
|
@ -40,8 +40,7 @@ def processFeedback(feedback):
|
|||
center_x_changed = current_x
|
||||
center_y_changed = current_y
|
||||
center_z_changed = current_z
|
||||
print(center_x_changed, center_y_changed, center_z_changed,
|
||||
_x, _y, _z)
|
||||
print(center_x_changed, center_y_changed, center_z_changed, _x, _y, _z)
|
||||
|
||||
coords = [
|
||||
current_y * 1000,
|
||||
|
|
@ -165,33 +164,33 @@ def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof=Fal
|
|||
server.insert(int_marker, processFeedback)
|
||||
menu_handler.apply(server, int_marker.name)
|
||||
|
||||
|
||||
# marker box finish
|
||||
|
||||
|
||||
def listener():
|
||||
global server
|
||||
rospy.init_node('control_marker', anonymous=True)
|
||||
rospy.init_node("control_marker", anonymous=True)
|
||||
|
||||
coords = mycobot.get_coords()
|
||||
# print(coords)
|
||||
|
||||
# crate a timer to update the pushlished transforms
|
||||
server = InteractiveMarkerServer('mycobot_controller')
|
||||
menu_handler.insert('First Entry', callback=processFeedback)
|
||||
menu_handler.insert('Second Entry', callback=processFeedback)
|
||||
server = InteractiveMarkerServer("mycobot_controller")
|
||||
menu_handler.insert("First Entry", callback=processFeedback)
|
||||
menu_handler.insert("Second Entry", callback=processFeedback)
|
||||
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo('error [101]: can not get coord values')
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
# initial position
|
||||
position = Point(coords[1] / -1000, coords[0] / 1000, coords[2] / 1000)
|
||||
# orientation = Quaternion(coords[4] / 100, coords[3] / 100, coords[5] / 100, 1)
|
||||
orientation = Quaternion(0, 0, 0, 1)
|
||||
make6DofMarker(True, InteractiveMarkerControl.NONE,
|
||||
position, orientation, True)
|
||||
make6DofMarker(True, InteractiveMarkerControl.NONE, position, orientation, True)
|
||||
server.applyChanges()
|
||||
|
||||
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
|
||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
rate = rospy.Rate(30) # 10hz
|
||||
|
||||
# pub joint state
|
||||
|
|
@ -199,19 +198,19 @@ def listener():
|
|||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
'joint2_to_joint1',
|
||||
'joint3_to_joint2',
|
||||
'joint4_to_joint3',
|
||||
'joint5_to_joint4',
|
||||
'joint6_to_joint5',
|
||||
'joint6output_to_joint6'
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
]
|
||||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = '/joint1'
|
||||
marker_.ns = 'my_namespace'
|
||||
marker_.header.frame_id = "/joint1"
|
||||
marker_.ns = "my_namespace"
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
|
@ -232,8 +231,7 @@ def listener():
|
|||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
|
||||
shell=True).decode()
|
||||
if __name__ == "__main__":
|
||||
port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode()
|
||||
mycobot = MyCobot(port)
|
||||
listener()
|
||||
|
|
|
|||
|
|
@ -2,15 +2,12 @@
|
|||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from geometry_msgs.msg import Twist
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_ros.srv import (
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
|
||||
from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
|
|
@ -19,63 +16,73 @@ class ImageConverter:
|
|||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage('calibrationFileName.xml', cv.FILE_STORAGE_READ)
|
||||
self.dist_coeffs = calibrationParams.getNode('distCoeffs').mat()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# rospy.wait_for_service('get_joint_coords')
|
||||
# rospy.wait_for_service('set_joint_coords')
|
||||
|
||||
# try:
|
||||
# self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
||||
# self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
# except:
|
||||
# print('Error: cannot connect service...')
|
||||
# exit(1)
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
|
||||
def callback(self, data):
|
||||
'''Callback function.
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
'''
|
||||
"""
|
||||
try:
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1]/2, size[0]/2]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
self.camera_matrix = np.array([
|
||||
[focal_length,0,center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0,0,1],
|
||||
], dtype=np.float32)
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
corners, ids, rejectImaPoint = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
|
||||
gray, self.aruco_dict, parameters=self.aruo_params
|
||||
)
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(corners, 0.05, self.camera_matrix, self.dist_coeffs)
|
||||
rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec - tvec).any()
|
||||
|
||||
print('rvec:', rvec, 'tvec:', tvec)
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
# Just process first one detected.
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] -0.03]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
|
||||
print('tf_change:', tf_change)
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
self.br.sendTransform(xyz, tf_change, rospy.Time.now(), 'basic_shapes', 'joint6_flange' )
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
|
@ -84,13 +91,15 @@ class ImageConverter:
|
|||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print e
|
||||
if __name__ == '__main__':
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print "Shutting down cv_bridge_test node."
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
|
|||
|
|
@ -4,13 +4,13 @@ from visualization_msgs.msg import Marker
|
|||
import time
|
||||
import random
|
||||
|
||||
from mycobot_ros.msg import (MycobotSetAngles, MycobotSetCoords,MycobotPumpStatus)
|
||||
from mycobot_ros.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node('gipper_subscriber',anonymous=True)
|
||||
angle_pub = rospy.Publisher('mycobot/angles_goal', MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher('mycobot/coords_goal', MycobotSetCoords, queue_size=5)
|
||||
pump_pub = rospy.Publisher('mycobot/pump_status', MycobotPumpStatus, queue_size=5)
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5)
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5)
|
||||
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
|
|
@ -27,7 +27,7 @@ temp_x = temp_y = temp_z = 0.0
|
|||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70 , m=2):
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
|
|
@ -39,6 +39,7 @@ def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70 , m=2):
|
|||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
|
|
@ -46,13 +47,15 @@ def pub_angles(a, b, c, d, e, f, sp):
|
|||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag):
|
||||
pump.Status = flag
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
|
|
@ -72,16 +75,20 @@ def grippercallback(data):
|
|||
return
|
||||
|
||||
# pump lenght: 88mm
|
||||
x = float(format(data.pose.position.x*1000, '.2f'))
|
||||
y = float(format(data.pose.position.y*1000, '.2f'))
|
||||
z = float(format(data.pose.position.z*1000, '.2f'))
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
if time.time() - temp_time < 30 or (temp_x == temp_y == temp_z == 0.0) or target_is_moving(x - x_offset, y - y_offset ,z):
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x-20, y, 280)
|
||||
time.sleep(.1)
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
|
|
@ -91,16 +98,15 @@ def grippercallback(data):
|
|||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1,17):
|
||||
pub_coords(x, y, z - i * 5,rx=-160, sp=10)
|
||||
time.sleep(.1)
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
|
|
@ -114,16 +120,16 @@ def grippercallback(data):
|
|||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range (1,8):
|
||||
pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2)
|
||||
time.sleep(.1)
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False)
|
||||
|
||||
time.sleep(.5)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
|
@ -131,20 +137,20 @@ def grippercallback(data):
|
|||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(.5)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False)
|
||||
# time.sleep(2.5)
|
||||
rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
|
||||
print 'gripper test'
|
||||
rospy.Subscriber("visualization_marker", Marker, grippercallback, queue_size=1)
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
|
@ -11,28 +11,30 @@ from pymycobot.mycobot import MyCobot
|
|||
|
||||
|
||||
def talker():
|
||||
rospy.init_node('display', anonymous=True)
|
||||
rospy.init_node("display", anonymous=True)
|
||||
|
||||
print('Try connect real mycobot...')
|
||||
port = rospy.get_param('~port', '/dev/ttyUSB0')
|
||||
baud = rospy.get_param('~baud', 115200)
|
||||
print('port: {}, baud: {}\n'.format(port, baud))
|
||||
print("Try connect real mycobot...")
|
||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print("port: {}, baud: {}\n".format(port, baud))
|
||||
try:
|
||||
mycobot = MyCobot(port, baud)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
print('''\
|
||||
print(
|
||||
"""\
|
||||
\rTry connect mycobot failed!
|
||||
\rPlease check wether connected with mycobot.
|
||||
\rPlease chckt wether the port or baud is right.
|
||||
''')
|
||||
"""
|
||||
)
|
||||
exit(1)
|
||||
mycobot.release_all_servos()
|
||||
time.sleep(.1)
|
||||
print('Rlease all servos over.\n')
|
||||
time.sleep(0.1)
|
||||
print("Rlease all servos over.\n")
|
||||
|
||||
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
|
||||
pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
|
||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(30) # 30hz
|
||||
|
||||
# pub joint state
|
||||
|
|
@ -40,21 +42,21 @@ def talker():
|
|||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
'joint2_to_joint1',
|
||||
'joint3_to_joint2',
|
||||
'joint4_to_joint3',
|
||||
'joint5_to_joint4',
|
||||
'joint6_to_joint5',
|
||||
'joint6output_to_joint6'
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
]
|
||||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = '/joint1'
|
||||
marker_.ns = 'my_namespace'
|
||||
marker_.header.frame_id = "/joint1"
|
||||
marker_.ns = "my_namespace"
|
||||
|
||||
print('publishing ...')
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
|
|
@ -82,7 +84,7 @@ def talker():
|
|||
# print(coords)
|
||||
if not coords:
|
||||
coords = [0, 0, 0, 0, 0, 0]
|
||||
rospy.loginfo('error [101]: can not get coord values')
|
||||
rospy.loginfo("error [101]: can not get coord values")
|
||||
|
||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
||||
marker_.pose.position.y = coords[0] / 1000
|
||||
|
|
@ -95,7 +97,7 @@ def talker():
|
|||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -9,24 +9,23 @@ import tf
|
|||
|
||||
|
||||
def talker():
|
||||
rospy.init_node('following_marker', anonymous=True)
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10)
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = '/joint1'
|
||||
marker_.ns = 'basic_cube'
|
||||
marker_.header.frame_id = "/joint1"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print('publishing ...')
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform('joint1', 'basic_shapes', now)
|
||||
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
|
@ -58,7 +57,7 @@ def talker():
|
|||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -10,9 +10,9 @@ from mycobot_ros.srv import GetAngles
|
|||
|
||||
|
||||
def talker():
|
||||
rospy.loginfo('start ...')
|
||||
rospy.init_node('real_listener', anonymous=True)
|
||||
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener", anonymous=True)
|
||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
rate = rospy.Rate(30) # 30hz
|
||||
|
||||
# pub joint state
|
||||
|
|
@ -20,21 +20,21 @@ def talker():
|
|||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
'joint2_to_joint1',
|
||||
'joint3_to_joint2',
|
||||
'joint4_to_joint3',
|
||||
'joint5_to_joint4',
|
||||
'joint6_to_joint5',
|
||||
'joint6output_to_joint6'
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
]
|
||||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
rospy.loginfo('wait service')
|
||||
rospy.wait_for_service('get_joint_angles')
|
||||
func = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
||||
rospy.loginfo("wait service")
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
|
||||
rospy.loginfo('start loop ...')
|
||||
rospy.loginfo("start loop ...")
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
|
||||
|
|
@ -49,14 +49,14 @@ def talker():
|
|||
res.joint_5 * (math.pi / 180),
|
||||
res.joint_6 * (math.pi / 180),
|
||||
]
|
||||
rospy.loginfo('res: {}'.format(radians_list))
|
||||
rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
joint_state_send.position = radians_list
|
||||
pub.publish(joint_state_send)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -13,10 +13,10 @@ class Listener(object):
|
|||
def __init__(self):
|
||||
super(Listener, self).__init__()
|
||||
|
||||
rospy.loginfo('start ...')
|
||||
rospy.init_node('real_listener_1', anonymous=True)
|
||||
self.pub = rospy.Publisher('joint_states', JointState, queue_size=10)
|
||||
self.sub = rospy.Subscriber('mycobot/angles_real', MycobotAngles, self.callback)
|
||||
rospy.loginfo("start ...")
|
||||
rospy.init_node("real_listener_1", anonymous=True)
|
||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
||||
rate = rospy.Rate(30) # 30hz
|
||||
rospy.spin()
|
||||
|
||||
|
|
@ -26,12 +26,12 @@ class Listener(object):
|
|||
joint_state_send.header = Header()
|
||||
|
||||
joint_state_send.name = [
|
||||
'joint2_to_joint1',
|
||||
'joint3_to_joint2',
|
||||
'joint4_to_joint3',
|
||||
'joint5_to_joint4',
|
||||
'joint6_to_joint5',
|
||||
'joint6output_to_joint6'
|
||||
"joint2_to_joint1",
|
||||
"joint3_to_joint2",
|
||||
"joint4_to_joint3",
|
||||
"joint5_to_joint4",
|
||||
"joint6_to_joint5",
|
||||
"joint6output_to_joint6",
|
||||
]
|
||||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
|
@ -45,14 +45,14 @@ class Listener(object):
|
|||
data.joint_5 * (math.pi / 180),
|
||||
data.joint_6 * (math.pi / 180),
|
||||
]
|
||||
rospy.loginfo('res: {}'.format(radians_list))
|
||||
rospy.loginfo("res: {}".format(radians_list))
|
||||
|
||||
joint_state_send.position = radians_list
|
||||
self.pub.publish(joint_state_send)
|
||||
# rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
Listener()
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -1,38 +1,38 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
import Tkinter as tk
|
||||
from mycobot_ros.srv import (
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
|
||||
from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import time
|
||||
from rospy import ServiceException
|
||||
|
||||
|
||||
|
||||
class Window:
|
||||
def __init__(self,handle):
|
||||
def __init__(self, handle):
|
||||
self.win = handle
|
||||
self.win.resizable(0,0) # 固定窗口大小
|
||||
|
||||
self.win.resizable(0, 0) # 固定窗口大小
|
||||
|
||||
self.model = 0
|
||||
self.speed = rospy.get_param("~speed", 50)
|
||||
|
||||
#设置默认速度
|
||||
|
||||
# 设置默认速度
|
||||
self.speed_d = tk.StringVar()
|
||||
self.speed_d.set(str(self.speed))
|
||||
# print(self.speed)
|
||||
self.connect_ser()
|
||||
|
||||
# 获取机械臂数据
|
||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed,self.model]
|
||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed,self.model]
|
||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
||||
self.get_date()
|
||||
|
||||
|
||||
# get screen width and height
|
||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
||||
# calculate x and y coordinates for the Tk root window
|
||||
x = (self.ws/2) - 190
|
||||
y = (self.hs/2) - 250
|
||||
self.win.geometry('430x370+{}+{}'.format(x,y))
|
||||
x = (self.ws / 2) - 190
|
||||
y = (self.hs / 2) - 250
|
||||
self.win.geometry("430x370+{}+{}".format(x, y))
|
||||
# 布局
|
||||
self.set_layout()
|
||||
# 输入部分
|
||||
|
|
@ -40,64 +40,72 @@ class Window:
|
|||
# 展示部分
|
||||
self.show_init()
|
||||
|
||||
|
||||
# joint 设置按钮
|
||||
tk.Button(self.frmLT,text="设置",width=5, command=self.get_joint_input).grid(row=6, column=1, sticky="w", padx=3, pady=2)
|
||||
tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# coordination 设置按钮
|
||||
tk.Button(self.frmRT,text="设置",width=5, command=self.get_coord_input).grid(row=6, column=1, sticky="w", padx=3, pady=2)
|
||||
tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
|
||||
row=6, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
# 夹爪开关按钮
|
||||
tk.Button(self.frmLB,text="夹爪(开)",command=self.gripper_open,width=5).grid(row=1, column=0, sticky="w", padx=3, pady=50)
|
||||
tk.Button(self.frmLB,text="夹爪(关)",command=self.gripper_close,width=5).grid(row=1, column=1, sticky="w", padx=3, pady=2)
|
||||
|
||||
def connect_ser(self):
|
||||
rospy.init_node('simple_gui', anonymous=True, disable_signals=True)
|
||||
tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
|
||||
row=1, column=0, sticky="w", padx=3, pady=50
|
||||
)
|
||||
tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid(
|
||||
row=1, column=1, sticky="w", padx=3, pady=2
|
||||
)
|
||||
|
||||
rospy.wait_for_service('get_joint_angles')
|
||||
rospy.wait_for_service('set_joint_angles')
|
||||
rospy.wait_for_service('get_joint_coords')
|
||||
rospy.wait_for_service('set_joint_coords')
|
||||
rospy.wait_for_service('switch_gripper_status')
|
||||
def connect_ser(self):
|
||||
rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
|
||||
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
rospy.wait_for_service("set_joint_angles")
|
||||
rospy.wait_for_service("get_joint_coords")
|
||||
rospy.wait_for_service("set_joint_coords")
|
||||
rospy.wait_for_service("switch_gripper_status")
|
||||
try:
|
||||
self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
||||
self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
self.get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
||||
self.set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
|
||||
self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
||||
self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
||||
self.switch_gripper = rospy.ServiceProxy(
|
||||
'switch_gripper_status', GripperStatus)
|
||||
"switch_gripper_status", GripperStatus
|
||||
)
|
||||
except:
|
||||
print('start error ...')
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
|
||||
print('Connect service success.')
|
||||
|
||||
print("Connect service success.")
|
||||
|
||||
def set_layout(self):
|
||||
self.frmLT = tk.Frame(width=200, height=200)
|
||||
self.frmLC = tk.Frame(width=200, height=200)
|
||||
self.frmLB = tk.Frame(width=200, height=200)
|
||||
self.frmRT = tk.Frame(width=200, height=200)
|
||||
self.frmLT.grid(row=0, column=0,padx=1,pady=3)
|
||||
self.frmLC.grid(row=1, column=0,padx=1,pady=3)
|
||||
self.frmLB.grid(row=1, column=1,padx=2,pady=3)
|
||||
self.frmRT.grid(row=0, column=1,padx=2,pady=3)
|
||||
|
||||
self.frmLT.grid(row=0, column=0, padx=1, pady=3)
|
||||
self.frmLC.grid(row=1, column=0, padx=1, pady=3)
|
||||
self.frmLB.grid(row=1, column=1, padx=2, pady=3)
|
||||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
||||
|
||||
def need_input(self):
|
||||
# 输入提示
|
||||
tk.Label(self.frmLT,text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLT,text="Joint 2 ").grid(row=1)#第二行
|
||||
tk.Label(self.frmLT,text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLT,text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLT,text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLT,text="Joint 6 ").grid(row=5)
|
||||
|
||||
tk.Label(self.frmRT,text=" x ").grid(row=0)
|
||||
tk.Label(self.frmRT,text=" y ").grid(row=1)#第二行
|
||||
tk.Label(self.frmRT,text=" z ").grid(row=2)
|
||||
tk.Label(self.frmRT,text=" rx ").grid(row=3)
|
||||
tk.Label(self.frmRT,text=" ry ").grid(row=4)
|
||||
tk.Label(self.frmRT,text=" rz ").grid(row=5)
|
||||
|
||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
|
||||
|
||||
tk.Label(self.frmRT, text=" x ").grid(row=0)
|
||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmRT, text=" z ").grid(row=2)
|
||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
||||
tk.Label(self.frmRT, text=" ry ").grid(row=4)
|
||||
tk.Label(self.frmRT, text=" rz ").grid(row=5)
|
||||
|
||||
# 设置输入框的默认值
|
||||
self.j1_default = tk.StringVar()
|
||||
self.j1_default.set(self.res_angles[0])
|
||||
|
|
@ -111,7 +119,7 @@ class Window:
|
|||
self.j5_default.set(self.res_angles[4])
|
||||
self.j6_default = tk.StringVar()
|
||||
self.j6_default.set(self.res_angles[5])
|
||||
|
||||
|
||||
self.x_default = tk.StringVar()
|
||||
self.x_default.set(self.record_coords[0])
|
||||
self.y_default = tk.StringVar()
|
||||
|
|
@ -124,99 +132,148 @@ class Window:
|
|||
self.ry_default.set(self.record_coords[4])
|
||||
self.rz_default = tk.StringVar()
|
||||
self.rz_default.set(self.record_coords[5])
|
||||
|
||||
|
||||
# joint 输入框
|
||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
||||
self.J_1.grid(row=0,column=1,pady=3)
|
||||
self.J_1.grid(row=0, column=1, pady=3)
|
||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
||||
self.J_2.grid(row=1,column=1,pady=3)
|
||||
self.J_2.grid(row=1, column=1, pady=3)
|
||||
self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default)
|
||||
self.J_3.grid(row=2,column=1,pady=3)
|
||||
self.J_3.grid(row=2, column=1, pady=3)
|
||||
self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default)
|
||||
self.J_4.grid(row=3,column=1,pady=3)
|
||||
self.J_4.grid(row=3, column=1, pady=3)
|
||||
self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default)
|
||||
self.J_5.grid(row=4,column=1,pady=3)
|
||||
self.J_5.grid(row=4, column=1, pady=3)
|
||||
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
|
||||
self.J_6.grid(row=5,column=1,pady=3)
|
||||
|
||||
self.J_6.grid(row=5, column=1, pady=3)
|
||||
|
||||
# coord 输入框
|
||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
||||
self.x.grid(row=0,column=1,pady=3,padx=0)
|
||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
||||
self.y.grid(row=1,column=1,pady=3)
|
||||
self.y.grid(row=1, column=1, pady=3)
|
||||
self.z = tk.Entry(self.frmRT, textvariable=self.z_default)
|
||||
self.z.grid(row=2,column=1,pady=3)
|
||||
self.z.grid(row=2, column=1, pady=3)
|
||||
self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default)
|
||||
self.rx.grid(row=3,column=1,pady=3)
|
||||
self.rx.grid(row=3, column=1, pady=3)
|
||||
self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default)
|
||||
self.ry.grid(row=4,column=1,pady=3)
|
||||
self.ry.grid(row=4, column=1, pady=3)
|
||||
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
|
||||
self.rz.grid(row=5,column=1,pady=3)
|
||||
|
||||
self.rz.grid(row=5, column=1, pady=3)
|
||||
|
||||
# 所有输入框，用于拿输入的数据
|
||||
self.all_j = [self.J_1,self.J_2,self.J_3,self.J_4,self.J_5,self.J_6]
|
||||
self.all_c = [self.x,self.y,self.z,self.rx,self.ry,self.rz]
|
||||
|
||||
self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6]
|
||||
self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
||||
|
||||
# 速度输入框
|
||||
tk.Label(self.frmLB,text="speed",).grid(row=0,column=0)
|
||||
self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d,width=10)
|
||||
self.get_speed.grid(row=0,column=1)
|
||||
|
||||
tk.Label(
|
||||
self.frmLB,
|
||||
text="speed",
|
||||
).grid(row=0, column=0)
|
||||
self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10)
|
||||
self.get_speed.grid(row=0, column=1)
|
||||
|
||||
def show_init(self):
|
||||
# 显示
|
||||
tk.Label(self.frmLC,text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLC,text="Joint 2 ").grid(row=1)#第二行
|
||||
tk.Label(self.frmLC,text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLC,text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLC,text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLC,text="Joint 6 ").grid(row=5)
|
||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # 第二行
|
||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
||||
tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
|
||||
tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
|
||||
tk.Label(self.frmLC, text="Joint 6 ").grid(row=5)
|
||||
|
||||
# get数据
|
||||
|
||||
|
||||
# ，展示出来
|
||||
self.cont_1 = tk.StringVar(self.frmLC)
|
||||
self.cont_1.set(str(self.res_angles[0])+"°")
|
||||
self.cont_1.set(str(self.res_angles[0]) + "°")
|
||||
self.cont_2 = tk.StringVar(self.frmLC)
|
||||
self.cont_2.set(str(self.res_angles[1])+"°")
|
||||
self.cont_2.set(str(self.res_angles[1]) + "°")
|
||||
self.cont_3 = tk.StringVar(self.frmLC)
|
||||
self.cont_3.set(str(self.res_angles[2])+"°")
|
||||
self.cont_3.set(str(self.res_angles[2]) + "°")
|
||||
self.cont_4 = tk.StringVar(self.frmLC)
|
||||
self.cont_4.set(str(self.res_angles[3])+"°")
|
||||
self.cont_4.set(str(self.res_angles[3]) + "°")
|
||||
self.cont_5 = tk.StringVar(self.frmLC)
|
||||
self.cont_5.set(str(self.res_angles[4])+"°")
|
||||
self.cont_5.set(str(self.res_angles[4]) + "°")
|
||||
self.cont_6 = tk.StringVar(self.frmLC)
|
||||
self.cont_6.set(str(self.res_angles[5])+"°")
|
||||
self.cont_all = [self.cont_1,self.cont_2,self.cont_3,self.cont_4,self.cont_5,self.cont_6,self.speed,self.model]
|
||||
|
||||
self.show_j1 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_1,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=1,padx=0,pady=5)
|
||||
|
||||
self.show_j2 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_2,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=1,padx=0,pady=5)
|
||||
self.show_j3 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_3,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=1,padx=0,pady=5)
|
||||
self.show_j4 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_4,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=1,padx=0,pady=5)
|
||||
self.show_j5 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_5,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=1,padx=0,pady=5)
|
||||
self.show_j6 = tk.Label(self.frmLC,
|
||||
textvariable=self.cont_6,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=1,padx=5,pady=5)
|
||||
|
||||
self.all_jo = [self.show_j1,self.show_j2,self.show_j3,self.show_j4,self.show_j5,self.show_j6]
|
||||
|
||||
self.cont_6.set(str(self.res_angles[5]) + "°")
|
||||
self.cont_all = [
|
||||
self.cont_1,
|
||||
self.cont_2,
|
||||
self.cont_3,
|
||||
self.cont_4,
|
||||
self.cont_5,
|
||||
self.cont_6,
|
||||
self.speed,
|
||||
self.model,
|
||||
]
|
||||
|
||||
self.show_j1 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_1,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=0, column=1, padx=0, pady=5)
|
||||
|
||||
self.show_j2 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_2,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=1, column=1, padx=0, pady=5)
|
||||
self.show_j3 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_3,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=2, column=1, padx=0, pady=5)
|
||||
self.show_j4 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_4,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=3, column=1, padx=0, pady=5)
|
||||
self.show_j5 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_5,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=4, column=1, padx=0, pady=5)
|
||||
self.show_j6 = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.cont_6,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=5, column=1, padx=5, pady=5)
|
||||
|
||||
self.all_jo = [
|
||||
self.show_j1,
|
||||
self.show_j2,
|
||||
self.show_j3,
|
||||
self.show_j4,
|
||||
self.show_j5,
|
||||
self.show_j6,
|
||||
]
|
||||
|
||||
# 显示
|
||||
tk.Label(self.frmLC,text=" x ").grid(row=0,column=3)
|
||||
tk.Label(self.frmLC,text=" y ").grid(row=1,column=3)#第二行
|
||||
tk.Label(self.frmLC,text=" z ").grid(row=2,column=3)
|
||||
tk.Label(self.frmLC,text=" rx ").grid(row=3,column=3)
|
||||
tk.Label(self.frmLC,text=" ry ").grid(row=4,column=3)
|
||||
tk.Label(self.frmLC,text=" rz ").grid(row=5,column=3)
|
||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # 第二行
|
||||
tk.Label(self.frmLC, text=" z ").grid(row=2, column=3)
|
||||
tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3)
|
||||
tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3)
|
||||
tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3)
|
||||
self.coord_x = tk.StringVar()
|
||||
self.coord_x.set(str(self.record_coords[0]))
|
||||
self.coord_y = tk.StringVar()
|
||||
|
|
@ -229,54 +286,97 @@ class Window:
|
|||
self.coord_ry.set(str(self.record_coords[4]))
|
||||
self.coord_rz = tk.StringVar()
|
||||
self.coord_rz.set(str(self.record_coords[5]))
|
||||
|
||||
self.coord_all = [self.coord_x,self.coord_y,self.coord_z,self.coord_rx,self.coord_ry,self.coord_rz,self.speed,self.model]
|
||||
|
||||
self.show_x = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_x,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=0,column=4,padx=5,pady=5)
|
||||
self.show_y = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_y,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=1,column=4,padx=5,pady=5)
|
||||
self.show_z = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_z,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=2,column=4,padx=5,pady=5)
|
||||
self.show_rx = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_rx,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=3,column=4,padx=5,pady=5)
|
||||
self.show_ry = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_ry,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=4,column=4,padx=5,pady=5)
|
||||
self.show_rz = tk.Label(self.frmLC,
|
||||
textvariable=self.coord_rz,
|
||||
font=('Arial',9),width=7,height=1,bg='white').grid(row=5,column=4,padx=5,pady=5)
|
||||
|
||||
|
||||
self.coord_all = [
|
||||
self.coord_x,
|
||||
self.coord_y,
|
||||
self.coord_z,
|
||||
self.coord_rx,
|
||||
self.coord_ry,
|
||||
self.coord_rz,
|
||||
self.speed,
|
||||
self.model,
|
||||
]
|
||||
|
||||
self.show_x = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_x,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=0, column=4, padx=5, pady=5)
|
||||
self.show_y = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_y,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=1, column=4, padx=5, pady=5)
|
||||
self.show_z = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_z,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=2, column=4, padx=5, pady=5)
|
||||
self.show_rx = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_rx,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=3, column=4, padx=5, pady=5)
|
||||
self.show_ry = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_ry,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=4, column=4, padx=5, pady=5)
|
||||
self.show_rz = tk.Label(
|
||||
self.frmLC,
|
||||
textvariable=self.coord_rz,
|
||||
font=("Arial", 9),
|
||||
width=7,
|
||||
height=1,
|
||||
bg="white",
|
||||
).grid(row=5, column=4, padx=5, pady=5)
|
||||
|
||||
# mm 单位展示
|
||||
self.unit = tk.StringVar()
|
||||
self.unit.set('mm')
|
||||
self.unit.set("mm")
|
||||
for i in range(6):
|
||||
tk.Label(self.frmLC,textvariable=self.unit,font=('Arial',9)).grid(row=i,column=5)
|
||||
|
||||
tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid(
|
||||
row=i, column=5
|
||||
)
|
||||
|
||||
def gripper_open(self):
|
||||
try:
|
||||
self.switch_gripper(True)
|
||||
except ServiceException:
|
||||
# 可能由于该方法没有返回值，服务抛出无法处理的错误
|
||||
pass
|
||||
|
||||
|
||||
def gripper_close(self):
|
||||
try:
|
||||
self.switch_gripper(False)
|
||||
except ServiceException:
|
||||
pass
|
||||
|
||||
|
||||
def get_coord_input(self):
|
||||
# 获取 coord 输入的数据，发送给机械臂
|
||||
c_value = []
|
||||
for i in self.all_c:
|
||||
# print(type(i.get()))
|
||||
c_value.append(float(i.get()))
|
||||
self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
||||
self.speed = (
|
||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
||||
)
|
||||
c_value.append(self.speed)
|
||||
c_value.append(self.model)
|
||||
# print(c_value)
|
||||
|
|
@ -284,15 +384,17 @@ class Window:
|
|||
self.set_coords(*c_value)
|
||||
except ServiceException:
|
||||
pass
|
||||
self.show_j_date(c_value[:-2],'coord')
|
||||
|
||||
self.show_j_date(c_value[:-2], "coord")
|
||||
|
||||
def get_joint_input(self):
|
||||
# 获取joint输入的数据，发送给机械臂
|
||||
j_value = []
|
||||
for i in self.all_j:
|
||||
# print(type(i.get()))
|
||||
j_value.append(float(i.get()))
|
||||
self.speed = int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
||||
self.speed = (
|
||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
||||
)
|
||||
j_value.append(self.speed)
|
||||
|
||||
try:
|
||||
|
|
@ -301,8 +403,7 @@ class Window:
|
|||
pass
|
||||
self.show_j_date(j_value[:-1])
|
||||
# return j_value,c_value,speed
|
||||
|
||||
|
||||
|
||||
def get_date(self):
|
||||
# 拿机械臂的数据，用于展示
|
||||
t = time.time()
|
||||
|
|
@ -310,33 +411,47 @@ class Window:
|
|||
self.res = self.get_coords()
|
||||
if self.res.x > 1:
|
||||
break
|
||||
time.sleep(.1)
|
||||
|
||||
time.sleep(0.1)
|
||||
|
||||
t = time.time()
|
||||
while time.time() - t < 2:
|
||||
self.angles = self.get_angles()
|
||||
if self.angles.joint_1 > 1:
|
||||
break
|
||||
time.sleep(.1)
|
||||
time.sleep(0.1)
|
||||
# print(self.angles.joint_1)
|
||||
self.record_coords = [
|
||||
round(self.res.x,2), round(self.res.y,2), round(self.res.z,2), round(self.res.rx,2), round(self.res.ry,2), round(self.res.rz,2), self.speed, self.model
|
||||
round(self.res.x, 2),
|
||||
round(self.res.y, 2),
|
||||
round(self.res.z, 2),
|
||||
round(self.res.rx, 2),
|
||||
round(self.res.ry, 2),
|
||||
round(self.res.rz, 2),
|
||||
self.speed,
|
||||
self.model,
|
||||
]
|
||||
self.res_angles = [
|
||||
round(self.angles.joint_1, 2),
|
||||
round(self.angles.joint_2, 2),
|
||||
round(self.angles.joint_3, 2),
|
||||
round(self.angles.joint_4, 2),
|
||||
round(self.angles.joint_5, 2),
|
||||
round(self.angles.joint_6, 2),
|
||||
]
|
||||
self.res_angles = [round(self.angles.joint_1,2),round(self.angles.joint_2,2),round(self.angles.joint_3,2),round(self.angles.joint_4,2),round(self.angles.joint_5,2),round(self.angles.joint_6,2)]
|
||||
# print('coord:',self.record_coords)
|
||||
# print('angles:',self.res_angles)
|
||||
|
||||
|
||||
# def send_input(self,dates):
|
||||
def show_j_date(self,date,way=""):
|
||||
def show_j_date(self, date, way=""):
|
||||
# 展示数据
|
||||
if way == "coord":
|
||||
for i,j in zip(date,self.coord_all):
|
||||
for i, j in zip(date, self.coord_all):
|
||||
# print(i)
|
||||
j.set(str(i))
|
||||
else:
|
||||
for i,j in zip(date,self.cont_all):
|
||||
j.set(str(i)+"°")
|
||||
|
||||
for i, j in zip(date, self.cont_all):
|
||||
j.set(str(i) + "°")
|
||||
|
||||
def run(self):
|
||||
while True:
|
||||
try:
|
||||
|
|
@ -348,10 +463,12 @@ class Window:
|
|||
else:
|
||||
raise
|
||||
|
||||
|
||||
def main():
|
||||
window = tk.Tk()
|
||||
window.title('mycobot ros GUI')
|
||||
window.title("mycobot ros GUI")
|
||||
Window(window).run()
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
#!/usr/bin/env python2
|
||||
# from std_msgs.msg import String
|
||||
import time
|
||||
import subprocess
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
|
@ -13,7 +12,7 @@ mc = None
|
|||
|
||||
|
||||
def callback(data):
|
||||
#rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
||||
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
||||
print(data.position)
|
||||
data_list = []
|
||||
for index, value in enumerate(data.position):
|
||||
|
|
@ -25,18 +24,18 @@ def callback(data):
|
|||
|
||||
def listener():
|
||||
global mc
|
||||
rospy.init_node('control_slider', anonymous=True)
|
||||
rospy.init_node("control_slider", anonymous=True)
|
||||
|
||||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
port = rospy.get_param('~port', '/dev/ttyUSB0')
|
||||
baud = rospy.get_param('~baud', 115200)
|
||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
print('sping ...')
|
||||
print("spin ...")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
listener()
|
||||
|
|
|
|||
|
|
@ -1,20 +1,19 @@
|
|||
#!/usr/bin/env python
|
||||
from __future__ import print_function
|
||||
from mycobot_ros.srv import (
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
|
||||
from mycobot_ros.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import sys
|
||||
import select
|
||||
import termios
|
||||
import tty
|
||||
import copy
|
||||
import time
|
||||
|
||||
import roslib
|
||||
roslib.load_manifest('mycobot_ros')
|
||||
|
||||
roslib.load_manifest("mycobot_ros")
|
||||
|
||||
|
||||
msg = """
|
||||
msg = """\
|
||||
Mycobot Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
||||
|
|
@ -44,7 +43,7 @@ def vels(speed, turn):
|
|||
|
||||
|
||||
class Raw(object):
|
||||
def __init__(self,stream):
|
||||
def __init__(self, stream):
|
||||
self.stream = stream
|
||||
self.fd = self.stream.fileno()
|
||||
|
||||
|
|
@ -57,29 +56,28 @@ class Raw(object):
|
|||
|
||||
|
||||
def teleop_keyboard():
|
||||
rospy.init_node('teleop_keyboard')
|
||||
rospy.init_node("teleop_keyboard")
|
||||
|
||||
model = 0
|
||||
speed = rospy.get_param("~speed", 50)
|
||||
change_percent= rospy.get_param("~change_percent", 5)
|
||||
change_percent = rospy.get_param("~change_percent", 5)
|
||||
|
||||
change_angle = 180 * change_percent / 100
|
||||
change_angle = 180 * change_percent / 100
|
||||
change_len = 250 * change_percent / 100
|
||||
|
||||
rospy.wait_for_service('get_joint_angles')
|
||||
rospy.wait_for_service('set_joint_angles')
|
||||
rospy.wait_for_service('get_joint_coords')
|
||||
rospy.wait_for_service('set_joint_coords')
|
||||
rospy.wait_for_service('switch_gripper_status')
|
||||
rospy.wait_for_service("get_joint_angles")
|
||||
rospy.wait_for_service("set_joint_angles")
|
||||
rospy.wait_for_service("get_joint_coords")
|
||||
rospy.wait_for_service("set_joint_coords")
|
||||
rospy.wait_for_service("switch_gripper_status")
|
||||
try:
|
||||
get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
||||
set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
||||
set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
|
||||
switch_gripper = rospy.ServiceProxy(
|
||||
'switch_gripper_status', GripperStatus)
|
||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
||||
switch_gripper = rospy.ServiceProxy("switch_gripper_status", GripperStatus)
|
||||
except:
|
||||
print('start error ...')
|
||||
print("start error ...")
|
||||
exit(1)
|
||||
|
||||
init_pose = [0, 0, 0, 0, 0, 0, speed]
|
||||
|
|
@ -91,68 +89,66 @@ def teleop_keyboard():
|
|||
res = get_coords()
|
||||
if res.x > 1:
|
||||
break
|
||||
time.sleep(.1)
|
||||
time.sleep(0.1)
|
||||
|
||||
record_coords = [
|
||||
res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model
|
||||
]
|
||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
||||
print(record_coords)
|
||||
|
||||
try:
|
||||
print(msg)
|
||||
print(vels(speed, change_percent))
|
||||
while(1):
|
||||
while 1:
|
||||
try:
|
||||
# print("\r current coords: %s" % record_coords, end="")
|
||||
with Raw(sys.stdin):
|
||||
key = sys.stdin.read(1)
|
||||
if key == 'q':
|
||||
if key == "q":
|
||||
break
|
||||
elif key in ['w', 'W']:
|
||||
elif key in ["w", "W"]:
|
||||
record_coords[0] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['s', 'S']:
|
||||
elif key in ["s", "S"]:
|
||||
record_coords[0] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['a', 'A']:
|
||||
elif key in ["a", "A"]:
|
||||
record_coords[1] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['d', 'D']:
|
||||
elif key in ["d", "D"]:
|
||||
record_coords[1] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['z', 'Z']:
|
||||
elif key in ["z", "Z"]:
|
||||
record_coords[2] -= change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['x', 'X']:
|
||||
elif key in ["x", "X"]:
|
||||
record_coords[2] += change_len
|
||||
set_coords(*record_coords)
|
||||
elif key in ['u', 'U']:
|
||||
elif key in ["u", "U"]:
|
||||
record_coords[3] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['j', 'J']:
|
||||
elif key in ["j", "J"]:
|
||||
record_coords[3] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['i', 'I']:
|
||||
elif key in ["i", "I"]:
|
||||
record_coords[4] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['k', 'K']:
|
||||
elif key in ["k", "K"]:
|
||||
record_coords[4] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['o', 'O']:
|
||||
elif key in ["o", "O"]:
|
||||
record_coords[5] += change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['l', 'L']:
|
||||
elif key in ["l", "L"]:
|
||||
record_coords[5] -= change_angle
|
||||
set_coords(*record_coords)
|
||||
elif key in ['g', 'G']:
|
||||
elif key in ["g", "G"]:
|
||||
switch_gripper(True)
|
||||
elif key in ['h', 'H']:
|
||||
elif key in ["h", "H"]:
|
||||
switch_gripper(False)
|
||||
elif key == '1':
|
||||
elif key == "1":
|
||||
rsp = set_angles(*init_pose)
|
||||
elif key in '2':
|
||||
elif key in "2":
|
||||
rsp = set_angles(*home_pose)
|
||||
elif key in '3':
|
||||
elif key in "3":
|
||||
rep = get_angles()
|
||||
home_pose[0] = rep.joint_1
|
||||
home_pose[1] = rep.joint_2
|
||||
|
|
|
|||
|
|
@ -18,13 +18,14 @@ def callback(data):
|
|||
data_list.append(value)
|
||||
|
||||
mc.send_radians(data_list, 80)
|
||||
|
||||
|
||||
|
||||
def listener():
|
||||
global mc
|
||||
rospy.init_node('mycobot_reciver', anonymous=True)
|
||||
rospy.init_node("mycobot_reciver", anonymous=True)
|
||||
|
||||
port = rospy.get_param('~port', '/dev/ttyUSB0')
|
||||
baud = rospy.get_param('~baud', 115200)
|
||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
|
||||
|
|
@ -33,5 +34,6 @@ def listener():
|
|||
# spin() simply keeps python from exiting until this node is stopped
|
||||
rospy.spin()
|
||||
|
||||
if __name__ == '__main__':
|
||||
listener()
|
||||
|
||||
if __name__ == "__main__":
|
||||
listener()
|
||||
|
|
|
|||
|
|
@ -10,22 +10,22 @@ mc = None
|
|||
|
||||
def create_handle():
|
||||
global mc
|
||||
rospy.init_node('mycobot_services')
|
||||
rospy.loginfo('start ...')
|
||||
port = rospy.get_param('~port')
|
||||
baud = rospy.get_param('~baud')
|
||||
rospy.init_node("mycobot_services")
|
||||
rospy.loginfo("start ...")
|
||||
port = rospy.get_param("~port")
|
||||
baud = rospy.get_param("~baud")
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
mc = MyCobot(port, baud)
|
||||
|
||||
|
||||
def create_services():
|
||||
rospy.Service('set_joint_angles', SetAngles, set_angles)
|
||||
rospy.Service('get_joint_angles', GetAngles, get_angles)
|
||||
rospy.Service('set_joint_coords', SetCoords, set_coords)
|
||||
rospy.Service('get_joint_coords', GetCoords, get_coords)
|
||||
rospy.Service('switch_gripper_status', GripperStatus, switch_status)
|
||||
rospy.Service('switch_pump_status', PumpStatus, toggle_pump)
|
||||
rospy.loginfo('ready')
|
||||
rospy.Service("set_joint_angles", SetAngles, set_angles)
|
||||
rospy.Service("get_joint_angles", GetAngles, get_angles)
|
||||
rospy.Service("set_joint_coords", SetCoords, set_coords)
|
||||
rospy.Service("get_joint_coords", GetCoords, get_coords)
|
||||
rospy.Service("switch_gripper_status", GripperStatus, switch_status)
|
||||
rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
|
||||
rospy.loginfo("ready")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
|
|
@ -66,7 +66,7 @@ def set_coords(req):
|
|||
|
||||
if mc:
|
||||
mc.send_coords(coords, sp, mod)
|
||||
|
||||
|
||||
return SetCoordsResponse(True)
|
||||
|
||||
|
||||
|
|
@ -98,7 +98,7 @@ def toggle_pump(req):
|
|||
return PumpStatusResponse(True)
|
||||
|
||||
|
||||
robot_msg = '''
|
||||
robot_msg = """
|
||||
MyCobot Status
|
||||
--------------------------------
|
||||
Joint Limit:
|
||||
|
|
@ -116,29 +116,30 @@ Servo Infomation: %s
|
|||
Servo Temperature: %s
|
||||
|
||||
Atom Version: %s
|
||||
'''
|
||||
"""
|
||||
|
||||
|
||||
def output_robot_message():
|
||||
connect_status = False
|
||||
servo_infomation = 'unknown'
|
||||
servo_temperature = 'unknown'
|
||||
atom_version = 'unknown'
|
||||
servo_infomation = "unknown"
|
||||
servo_temperature = "unknown"
|
||||
atom_version = "unknown"
|
||||
|
||||
if mc:
|
||||
cn = mc.is_controller_connected()
|
||||
if cn == 1:
|
||||
connect_status = True
|
||||
time.sleep(.1)
|
||||
time.sleep(0.1)
|
||||
si = mc.is_all_servo_enable()
|
||||
if si == 1:
|
||||
servo_infomation = 'all connected'
|
||||
servo_infomation = "all connected"
|
||||
|
||||
print(robot_msg % (connect_status, servo_infomation,
|
||||
servo_temperature, atom_version))
|
||||
print(
|
||||
robot_msg % (connect_status, servo_infomation, servo_temperature, atom_version)
|
||||
)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
# print(MyCobot.__dict__)
|
||||
create_handle()
|
||||
output_robot_message()
|
||||
|
|
|
|||
|
|
@ -7,64 +7,71 @@ import threading
|
|||
|
||||
import rospy
|
||||
|
||||
from mycobot_ros.msg import (MycobotAngles, MycobotCoords, MycobotSetAngles, MycobotSetCoords, MycobotGripperStatus, MycobotPumpStatus)
|
||||
from mycobot_ros.msg import (
|
||||
MycobotAngles,
|
||||
MycobotCoords,
|
||||
MycobotSetAngles,
|
||||
MycobotSetCoords,
|
||||
MycobotGripperStatus,
|
||||
MycobotPumpStatus,
|
||||
)
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
|
||||
|
||||
class Watcher:
|
||||
"""this class solves two problems with multithreaded
|
||||
programs in Python, (1) a signal might be delivered
|
||||
to any thread (which is just a malfeature) and (2) if
|
||||
the thread that gets the signal is waiting, the signal
|
||||
is ignored (which is a bug).
|
||||
|
||||
The watcher is a concurrent process (not thread) that
|
||||
waits for a signal and the process that contains the
|
||||
threads. See Appendix A of The Little Book of Semaphores.
|
||||
http://greenteapress.com/semaphores/
|
||||
|
||||
I have only tested this on Linux. I would expect it to
|
||||
work on the Macintosh and not work on Windows.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
""" Creates a child thread, which returns. The parent
|
||||
thread waits for a KeyboardInterrupt and then kills
|
||||
the child thread.
|
||||
"""
|
||||
self.child = os.fork()
|
||||
if self.child == 0:
|
||||
return
|
||||
else:
|
||||
self.watch()
|
||||
|
||||
def watch(self):
|
||||
try:
|
||||
os.wait()
|
||||
except KeyboardInterrupt:
|
||||
# I put the capital B in KeyBoardInterrupt so I can
|
||||
# tell when the Watcher gets the SIGINT
|
||||
print 'KeyBoardInterrupt'
|
||||
self.kill()
|
||||
sys.exit()
|
||||
|
||||
def kill(self):
|
||||
try:
|
||||
os.kill(self.child, signal.SIGKILL)
|
||||
except OSError: pass
|
||||
class Watcher:
|
||||
"""this class solves two problems with multithreaded
|
||||
programs in Python, (1) a signal might be delivered
|
||||
to any thread (which is just a malfeature) and (2) if
|
||||
the thread that gets the signal is waiting, the signal
|
||||
is ignored (which is a bug).
|
||||
|
||||
The watcher is a concurrent process (not thread) that
|
||||
waits for a signal and the process that contains the
|
||||
threads. See Appendix A of The Little Book of Semaphores.
|
||||
http://greenteapress.com/semaphores/
|
||||
|
||||
I have only tested this on Linux. I would expect it to
|
||||
work on the Macintosh and not work on Windows.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Creates a child thread, which returns. The parent
|
||||
thread waits for a KeyboardInterrupt and then kills
|
||||
the child thread.
|
||||
"""
|
||||
self.child = os.fork()
|
||||
if self.child == 0:
|
||||
return
|
||||
else:
|
||||
self.watch()
|
||||
|
||||
def watch(self):
|
||||
try:
|
||||
os.wait()
|
||||
except KeyboardInterrupt:
|
||||
# I put the capital B in KeyBoardInterrupt so I can
|
||||
# tell when the Watcher gets the SIGINT
|
||||
print("KeyBoardInterrupt")
|
||||
self.kill()
|
||||
sys.exit()
|
||||
|
||||
def kill(self):
|
||||
try:
|
||||
os.kill(self.child, signal.SIGKILL)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
class MycobotTopics(object):
|
||||
|
||||
def __init__(self):
|
||||
super(MycobotTopics, self).__init__()
|
||||
|
||||
rospy.init_node('mycobot_topics')
|
||||
rospy.loginfo('start ...')
|
||||
port = rospy.get_param('~port', '/dev/ttyUSB0')
|
||||
baud = rospy.get_param('~baud', 115200)
|
||||
rospy.init_node("mycobot_topics")
|
||||
rospy.loginfo("start ...")
|
||||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
self.mc = MyCobot(port, baud)
|
||||
self.lock = threading.Lock()
|
||||
|
|
@ -89,16 +96,16 @@ class MycobotTopics(object):
|
|||
sg.start()
|
||||
sp.setDaemon(True)
|
||||
sp.start()
|
||||
|
||||
|
||||
pa.join()
|
||||
pb.join()
|
||||
sa.join()
|
||||
sb.join()
|
||||
sg.join()
|
||||
sp.join()
|
||||
|
||||
|
||||
def pub_real_angles(self):
|
||||
pub = rospy.Publisher('mycobot/angles_real', MycobotAngles, queue_size=5)
|
||||
pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5)
|
||||
ma = MycobotAngles()
|
||||
while not rospy.is_shutdown():
|
||||
self.lock.acquire()
|
||||
|
|
@ -112,10 +119,10 @@ class MycobotTopics(object):
|
|||
ma.joint_5 = angles[4]
|
||||
ma.joint_6 = angles[5]
|
||||
pub.publish(ma)
|
||||
time.sleep(.25)
|
||||
time.sleep(0.25)
|
||||
|
||||
def pub_real_coords(self):
|
||||
pub = rospy.Publisher('mycobot/coords_real', MycobotCoords, queue_size=5)
|
||||
pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5)
|
||||
ma = MycobotCoords()
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
|
|
@ -130,15 +137,24 @@ class MycobotTopics(object):
|
|||
ma.ry = coords[4]
|
||||
ma.rz = coords[5]
|
||||
pub.publish(ma)
|
||||
time.sleep(.25)
|
||||
time.sleep(0.25)
|
||||
|
||||
def sub_set_angles(self):
|
||||
def callback(data):
|
||||
angles = [data.joint_1, data.joint_2, data.joint_3, data.joint_4, data.joint_5, data.joint_6]
|
||||
angles = [
|
||||
data.joint_1,
|
||||
data.joint_2,
|
||||
data.joint_3,
|
||||
data.joint_4,
|
||||
data.joint_5,
|
||||
data.joint_6,
|
||||
]
|
||||
sp = int(data.speed)
|
||||
self.mc.send_angles(angles, sp)
|
||||
|
||||
sub = rospy.Subscriber('mycobot/angles_goal', MycobotSetAngles, callback=callback)
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/angles_goal", MycobotSetAngles, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_set_coords(self):
|
||||
|
|
@ -148,7 +164,9 @@ class MycobotTopics(object):
|
|||
model = int(data.model)
|
||||
self.mc.send_coords(angles, sp, model)
|
||||
|
||||
sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback)
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/coords_goal", MycobotSetCoords, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_gripper_status(self):
|
||||
|
|
@ -158,7 +176,9 @@ class MycobotTopics(object):
|
|||
else:
|
||||
self.mc.set_gripper_state(1, 80)
|
||||
|
||||
sub = rospy.Subscriber('mycobot/gripper_status', MycobotGripperStatus, callback=callback)
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/gripper_status", MycobotGripperStatus, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
def sub_pump_status(self):
|
||||
|
|
@ -170,15 +190,17 @@ class MycobotTopics(object):
|
|||
self.mc.set_basic_output(2, 1)
|
||||
self.mc.set_basic_output(5, 1)
|
||||
|
||||
sub = rospy.Subscriber('mycobot/pump_status', MycobotPumpStatus, callback=callback)
|
||||
sub = rospy.Subscriber(
|
||||
"mycobot/pump_status", MycobotPumpStatus, callback=callback
|
||||
)
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
Watcher()
|
||||
mc_topics = MycobotTopics()
|
||||
mc_topics.start()
|
||||
# while True:
|
||||
# mc_topics.pub_real_coords()
|
||||
# mc_topics.sub_set_angles()
|
||||
pass
|
||||
pass
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ mutex = Lock()
|
|||
|
||||
|
||||
class ElephantRobot(object):
|
||||
def __init__(self,host,port):
|
||||
def __init__(self, host, port):
|
||||
# setup connection
|
||||
self.BUFFSIZE = 2048
|
||||
self.ADDR = (host, port)
|
||||
|
|
@ -23,28 +23,28 @@ class ElephantRobot(object):
|
|||
def start_client(self):
|
||||
try:
|
||||
self.tcp_client.connect(self.ADDR)
|
||||
return ''
|
||||
return ""
|
||||
except error, e:
|
||||
return e
|
||||
|
||||
def stop_client(self):
|
||||
self.tcp_client.close()
|
||||
|
||||
def send_command(self,command):
|
||||
def send_command(self, command):
|
||||
with mutex:
|
||||
self.tcp_client.send(command.encode())
|
||||
recv_data = self.tcp_client.recv(self.BUFFSIZE).decode()
|
||||
res_str = str(recv_data)
|
||||
print 'recv = ' + res_str
|
||||
print "recv = " + res_str
|
||||
res_arr = res_str.split(":")
|
||||
if len(res_arr) == 2:
|
||||
return res_arr[1]
|
||||
else:
|
||||
return ''
|
||||
return ""
|
||||
|
||||
def string_to_coords(self, data):
|
||||
data = data.replace('[','')
|
||||
data = data.replace(']','')
|
||||
data = data.replace("[", "")
|
||||
data = data.replace("]", "")
|
||||
data_arr = data.split(",")
|
||||
if len(data_arr) == 6:
|
||||
try:
|
||||
|
|
@ -54,8 +54,7 @@ class ElephantRobot(object):
|
|||
coords_4 = float(data_arr[3])
|
||||
coords_5 = float(data_arr[4])
|
||||
coords_6 = float(data_arr[5])
|
||||
coords = [coords_1, coords_2, coords_3,
|
||||
coords_4, coords_5, coords_6]
|
||||
coords = [coords_1, coords_2, coords_3, coords_4, coords_5, coords_6]
|
||||
return coords
|
||||
except:
|
||||
return invalid_coords()
|
||||
|
|
@ -80,80 +79,80 @@ class ElephantRobot(object):
|
|||
return coords
|
||||
|
||||
def get_angles(self):
|
||||
command = 'get_angles()\n'
|
||||
command = "get_angles()\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_coords(res)
|
||||
|
||||
def get_coords(self):
|
||||
command = 'get_coords()\n'
|
||||
command = "get_coords()\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_coords(res)
|
||||
|
||||
def get_speed(self):
|
||||
command = 'get_speed()\n'
|
||||
command = "get_speed()\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_double(res)
|
||||
|
||||
def power_on(self):
|
||||
command = 'power_on()\n'
|
||||
command = "power_on()\n"
|
||||
res = self.send_command(command)
|
||||
return True
|
||||
|
||||
def power_off(self):
|
||||
command = 'power_off()\n'
|
||||
command = "power_off()\n"
|
||||
res = self.send_command(command)
|
||||
return True
|
||||
|
||||
def check_running(self):
|
||||
command = 'check_running()\n'
|
||||
command = "check_running()\n"
|
||||
res = self.send_command(command)
|
||||
return res == '1'
|
||||
return res == "1"
|
||||
|
||||
def state_check(self):
|
||||
command = 'state_check()\n'
|
||||
command = "state_check()\n"
|
||||
res = self.send_command(command)
|
||||
return res == '1'
|
||||
return res == "1"
|
||||
|
||||
def program_open(self, file_path):
|
||||
command = 'program_open(' + file_path + ")\n"
|
||||
command = "program_open(" + file_path + ")\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_int(res)
|
||||
|
||||
def program_run(self, start_line):
|
||||
command = 'program_run(' + str(start_line) + ")\n"
|
||||
command = "program_run(" + str(start_line) + ")\n"
|
||||
res = self.send_command(command)
|
||||
return self.string_to_int(res)
|
||||
|
||||
def read_next_error(self):
|
||||
command = 'read_next_error()\n'
|
||||
command = "read_next_error()\n"
|
||||
res = self.send_command(command)
|
||||
return res
|
||||
|
||||
def write_coords(self,coords,speed):
|
||||
def write_coords(self, coords, speed):
|
||||
command = "set_coords("
|
||||
for item in coords:
|
||||
command += str(item) + ','
|
||||
command += str(speed) + ')\n'
|
||||
command += str(item) + ","
|
||||
command += str(speed) + ")\n"
|
||||
self.send_command(command)
|
||||
|
||||
def write_coord(self, axis, value, speed):
|
||||
coords = self.get_coords()
|
||||
if coords != self.invalid_coords():
|
||||
coords[axis] = value
|
||||
self.write_coords(coords,speed)
|
||||
self.write_coords(coords, speed)
|
||||
|
||||
def write_angles(self,angles,speed):
|
||||
def write_angles(self, angles, speed):
|
||||
command = "set_angles("
|
||||
for item in angles:
|
||||
command += str(item) + ','
|
||||
command += str(speed) + ')\n'
|
||||
command += str(item) + ","
|
||||
command += str(speed) + ")\n"
|
||||
self.send_command(command)
|
||||
|
||||
def write_angle(self, joint, value, speed):
|
||||
angles = self.get_angles()
|
||||
if angles != self.invalid_coords():
|
||||
angles[joint] = value
|
||||
self.write_angles(angles,speed)
|
||||
self.write_angles(angles, speed)
|
||||
|
||||
def set_speed(self, percentage):
|
||||
command = "set_speed(" + str(percentage) + ")\n"
|
||||
|
|
@ -187,11 +186,15 @@ class ElephantRobot(object):
|
|||
self.send_command(command)
|
||||
|
||||
def jog_angle(self, joint_str, direction, speed):
|
||||
command = "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
|
||||
command = (
|
||||
"jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
|
||||
)
|
||||
self.send_command(command)
|
||||
|
||||
def jog_coord(self, axis_str, direction, speed):
|
||||
command = "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
|
||||
command = (
|
||||
"jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
|
||||
)
|
||||
self.send_command(command)
|
||||
|
||||
def get_digital_in(self, pin_number):
|
||||
|
|
@ -228,7 +231,7 @@ class ElephantRobot(object):
|
|||
self.send_command(command)
|
||||
|
||||
def assign_variable(self, var_name, var_value):
|
||||
command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ')\n'
|
||||
command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ")\n"
|
||||
self.send_command(command)
|
||||
|
||||
def get_variable(self, var_name):
|
||||
|
|
@ -236,41 +239,41 @@ class ElephantRobot(object):
|
|||
return self.send_command(command)
|
||||
|
||||
|
||||
|
||||
old_list = []
|
||||
|
||||
|
||||
def callback(data):
|
||||
global old_list
|
||||
#rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
||||
print('position', data.position)
|
||||
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
||||
print ("position", data.position)
|
||||
data_list = []
|
||||
for index, value in enumerate(data.position):
|
||||
value = value * 180 / math.pi
|
||||
data_list.append(value)
|
||||
print('data', data_list)
|
||||
print ("data", data_list)
|
||||
|
||||
if not old_list:
|
||||
old_list = data_list
|
||||
mc.write_angles(data_list, 1999)
|
||||
elif old_list != data_list:
|
||||
old_list = data_list
|
||||
if (mc.check_running()):
|
||||
if mc.check_running():
|
||||
mc.task_stop()
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
mc.write_angles(data_list, 1999)
|
||||
|
||||
|
||||
def listener():
|
||||
global mc
|
||||
rospy.init_node('control_slider', anonymous=True)
|
||||
rospy.init_node("control_slider", anonymous=True)
|
||||
|
||||
ip = rospy.get_param('~ip', '192.168.10.169')
|
||||
print(ip)
|
||||
ip = rospy.get_param("~ip", "192.168.10.169")
|
||||
print (ip)
|
||||
mc = ElephantRobot(ip, 5001)
|
||||
# START CLIENT
|
||||
res = mc.start_client()
|
||||
if res != '':
|
||||
if res != "":
|
||||
print res
|
||||
sys.exit(1)
|
||||
print ep.wait(5)
|
||||
|
|
@ -282,9 +285,9 @@ def listener():
|
|||
rospy.Subscriber("joint_states", JointState, callback)
|
||||
|
||||
# spin() simply keeps python from exiting until this node is stopped
|
||||
print('sping ...')
|
||||
print ("sping ...")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
listener()
|
||||
|
|
|
|||
|
|
@ -1,71 +1,69 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
'''
|
||||
"""
|
||||
This package need `pymycobot`.
|
||||
This file for test the API if right.
|
||||
Just can run in Linux.
|
||||
'''
|
||||
"""
|
||||
|
||||
import time, random, subprocess
|
||||
from pymycobot.mycobot import MyCobot
|
||||
from pymycobot.genre import Angle, Coord
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys_ = subprocess.check_output(['uname'],
|
||||
shell=True).decode()
|
||||
if not sys_ == 'Linux':
|
||||
print('This script just can run on Linux.')
|
||||
if __name__ == "__main__":
|
||||
sys_ = subprocess.check_output(["uname"], shell=True).decode()
|
||||
if not sys_ == "Linux":
|
||||
print("This script just can run on Linux.")
|
||||
exit(0)
|
||||
|
||||
port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
|
||||
shell=True).decode()
|
||||
port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode()
|
||||
mycobot = MyCobot(port)
|
||||
|
||||
print('Start check api\n')
|
||||
print("Start check api\n")
|
||||
|
||||
print('::get_angles()')
|
||||
print('==> degrees: {}\n'.format(mycobot.get_angles()))
|
||||
print("::get_angles()")
|
||||
print("==> degrees: {}\n".format(mycobot.get_angles()))
|
||||
time.sleep(0.5)
|
||||
|
||||
print('::get_radians()')
|
||||
print('==> radians: {}\n'.format(mycobot.get_radians()))
|
||||
print("::get_radians()")
|
||||
print("==> radians: {}\n".format(mycobot.get_radians()))
|
||||
time.sleep(0.5)
|
||||
|
||||
print('::send_angles()')
|
||||
mycobot.send_angles([0,0,0,0,0,0], 80)
|
||||
print('==> set angles [0,0,0,0,0,0], speed 80\n')
|
||||
print('Is moving: {}'.format(mycobot.is_moving()))
|
||||
print("::send_angles()")
|
||||
mycobot.send_angles([0, 0, 0, 0, 0, 0], 80)
|
||||
print("==> set angles [0,0,0,0,0,0], speed 80\n")
|
||||
print("Is moving: {}".format(mycobot.is_moving()))
|
||||
time.sleep(3)
|
||||
|
||||
print('::send_radians')
|
||||
mycobot.send_radians([1,1,1,1,1,1], 70)
|
||||
print('==> set raidans [1,1,1,1,1,1], speed 70\n')
|
||||
print("::send_radians")
|
||||
mycobot.send_radians([1, 1, 1, 1, 1, 1], 70)
|
||||
print("==> set raidans [1,1,1,1,1,1], speed 70\n")
|
||||
time.sleep(1.5)
|
||||
|
||||
print('::send_angle()')
|
||||
print("::send_angle()")
|
||||
mycobot.send_angle(Angle.J2.value, 10, 50)
|
||||
print('==> angle: joint2, degree: 10, speed: 50\n')
|
||||
print("==> angle: joint2, degree: 10, speed: 50\n")
|
||||
time.sleep(1)
|
||||
|
||||
print('::get_coords()')
|
||||
print('==> coords {}\n'.format(mycobot.get_coords()))
|
||||
print("::get_coords()")
|
||||
print("==> coords {}\n".format(mycobot.get_coords()))
|
||||
time.sleep(0.5)
|
||||
|
||||
print('::send_coords()')
|
||||
print("::send_coords()")
|
||||
coord_list = [160, 160, 160, 0, 0, 0]
|
||||
mycobot.send_coords(coord_list, 70, 0)
|
||||
print('==> send coords [160,160,160,0,0,0], speed 70, mode 0\n')
|
||||
print("==> send coords [160,160,160,0,0,0], speed 70, mode 0\n")
|
||||
time.sleep(3.0)
|
||||
|
||||
print(mycobot.is_in_position(coord_list, 1))
|
||||
time.sleep(1)
|
||||
|
||||
print('::send_coord()')
|
||||
print("::send_coord()")
|
||||
mycobot.send_coord(Coord.X.value, -40, 70)
|
||||
print('==> send coord id: X, coord value: -40, speed: 70\n')
|
||||
print("==> send coord id: X, coord value: -40, speed: 70\n")
|
||||
time.sleep(2)
|
||||
|
||||
print('::release_all_servos()')
|
||||
print("::release_all_servos()")
|
||||
mycobot.release_all_servos()
|
||||
print('==> into free moving mode.')
|
||||
print('=== check end <==\n')
|
||||
print("==> into free moving mode.")
|
||||
print("=== check end <==\n")
|
||||
|
|
|
|||
|
|
@ -1,30 +1,29 @@
|
|||
#include <ros/ros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <ros/ros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
|
||||
{
|
||||
try
|
||||
{
|
||||
cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
|
||||
cv::waitKey(30);
|
||||
}
|
||||
catch (cv_bridge::Exception& e)
|
||||
{
|
||||
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
|
||||
}
|
||||
}
|
||||
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
|
||||
{
|
||||
try
|
||||
{
|
||||
cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
|
||||
cv::waitKey(30);
|
||||
}
|
||||
catch (cv_bridge::Exception &e)
|
||||
{
|
||||
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "image_listener");
|
||||
ros::NodeHandle nh;
|
||||
cv::namedWindow("view");
|
||||
cv::startWindowThread();
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Subscriber sub = it.subscribe("camera/image", 1,imageCallback);
|
||||
ros::spin();
|
||||
cv::destroyWindow("view");
|
||||
|
||||
}
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "image_listener");
|
||||
ros::NodeHandle nh;
|
||||
cv::namedWindow("view");
|
||||
cv::startWindowThread();
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
|
||||
ros::spin();
|
||||
cv::destroyWindow("view");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,60 +1,59 @@
|
|||
#include <ros/ros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <sstream> // for converting the command line parameter to integer
|
||||
#include <ros/ros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <sstream> // for converting the command line parameter to integer
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// Check if video source has been passed as a parameter
|
||||
if(argv[1] == NULL)
|
||||
{
|
||||
ROS_INFO("argv[1]=NULL\n");
|
||||
return 1;
|
||||
}
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
// Check if video source has been passed as a parameter
|
||||
if (argv[1] == NULL)
|
||||
{
|
||||
ROS_INFO("argv[1]=NULL\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
ros::init(argc, argv, "image_publisher"); // Initialize node
|
||||
ros::NodeHandle nh;
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic
|
||||
ros::NodeHandle nh;
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic
|
||||
|
||||
ros::Rate loop_rate(200); // refresh Hz.
|
||||
|
||||
// Convert the passed as command line parameter index for the video device to an integer
|
||||
std::istringstream video_sourceCmd(argv[1]);
|
||||
int video_source;
|
||||
// Check if it is indeed a number
|
||||
if(!(video_sourceCmd >> video_source))
|
||||
{
|
||||
ROS_INFO("video_sourceCmd is %d\n",video_source);
|
||||
return 1;
|
||||
}
|
||||
|
||||
cv::VideoCapture cap(video_source);
|
||||
// Check if video device can be opened with the given index
|
||||
if(!cap.isOpened())
|
||||
{
|
||||
ROS_INFO("can not opencv video device\n");
|
||||
return 1;
|
||||
}
|
||||
cv::Mat frame;
|
||||
sensor_msgs::ImagePtr msg;
|
||||
|
||||
while (nh.ok())
|
||||
{
|
||||
cap >> frame;
|
||||
// cv::imshow("veiwer", frame);
|
||||
// Check if grabbed frame is actually full with some content
|
||||
if(!frame.empty())
|
||||
{
|
||||
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
||||
pub.publish(msg);
|
||||
//cv::Wait(1);
|
||||
}
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
// if(cv::waitKey(2) >= 0)
|
||||
// break;
|
||||
// Convert the passed as command line parameter index for the video device to an integer
|
||||
std::istringstream video_sourceCmd(argv[1]);
|
||||
int video_source;
|
||||
// Check if it is indeed a number
|
||||
if (!(video_sourceCmd >> video_source))
|
||||
{
|
||||
ROS_INFO("video_sourceCmd is %d\n", video_source);
|
||||
return 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
cv::VideoCapture cap(video_source);
|
||||
// Check if video device can be opened with the given index
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
ROS_INFO("can not opencv video device\n");
|
||||
return 1;
|
||||
}
|
||||
cv::Mat frame;
|
||||
sensor_msgs::ImagePtr msg;
|
||||
|
||||
while (nh.ok())
|
||||
{
|
||||
cap >> frame;
|
||||
// cv::imshow("veiwer", frame);
|
||||
// Check if grabbed frame is actually full with some content
|
||||
if (!frame.empty())
|
||||
{
|
||||
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
||||
pub.publish(msg);
|
||||
//cv::Wait(1);
|
||||
}
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
// if(cv::waitKey(2) >= 0)
|
||||
// break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue