diff --git a/CMakeLists.txt b/CMakeLists.txt index 481a435..ea877b8 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,13 +3,32 @@ project(myCobotROS) add_compile_options(-std=c++11) ## Find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg serial) +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + genmsg + message_generation + serial +) + +add_message_files(FILES + Gripper.msg +) + +add_service_files(FILES + GetAngles.srv + SetAngles.srv + GetCoords.srv + SetCoords.srv + GripperStatus.srv +) ## Generate added messages and services generate_messages(DEPENDENCIES std_msgs) ## Declare a catkin package -catkin_package() +catkin_package(CATKIN_DEPENDS message_runtime std_msgs) ## Build talker and listener include_directories(include ${catkin_INCLUDE_DIRS}) @@ -18,6 +37,10 @@ catkin_install_python(PROGRAMS scripts/display.py scripts/control_slider.py scripts/control_marker.py + scripts/mycobot_ros.py + scripts/listen_real.py + scripts/teleop_keyboard.py + scripts/client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/launch/communication_service.launch b/launch/communication_service.launch new file mode 100644 index 0000000..cf47cc4 --- /dev/null +++ b/launch/communication_service.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/control_slider.launch b/launch/control_slider.launch index 9610608..82d03c3 100644 --- a/launch/control_slider.launch +++ b/launch/control_slider.launch @@ -1,16 +1,16 @@ + - - ["joint_states"] + + - diff --git a/launch/mycobot_teleop_keyboard.launch b/launch/mycobot_teleop_keyboard.launch new file mode 100644 index 0000000..4f78f35 --- /dev/null +++ b/launch/mycobot_teleop_keyboard.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/Gripper.msg b/msg/Gripper.msg new file mode 100644 index 0000000..18d6277 --- /dev/null +++ b/msg/Gripper.msg @@ -0,0 +1 @@ +bool Status \ No newline at end of file diff --git a/package.xml b/package.xml index 78fff1c..90171d2 100644 --- a/package.xml +++ b/package.xml @@ -15,11 +15,13 @@ rospy std_msgs serial + message_generation roscpp rospy std_msgs serial + message_runtime diff --git a/scripts/client.py b/scripts/client.py new file mode 100755 index 0000000..da0cc6b --- /dev/null +++ b/scripts/client.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python2 +import sys +import rospy +from myCobotROS.srv import * + + +def test(): + 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) + except: + pass + + +if __name__ == '__main__': + test() diff --git a/scripts/control_marker.py b/scripts/control_marker.py index c5458bf..82e9b18 100755 --- a/scripts/control_marker.py +++ b/scripts/control_marker.py @@ -1,6 +1,7 @@ #!/usr/bin/env python2 # from std_msgs.msg import String -import time, subprocess +import time +import subprocess import rospy from interactive_markers.interactive_marker_server import * @@ -23,8 +24,10 @@ center_x_changed = 0 center_y_changed = 0 center_z_changed = 0 -# marker box -def processFeedback( feedback ):# +# marker box + + +def processFeedback(feedback): global center_x_changed, center_y_changed, center_z_changed current_x = feedback.pose.position.x current_y = feedback.pose.position.y @@ -38,23 +41,24 @@ def processFeedback( feedback ):# center_y_changed = current_y center_z_changed = current_z print(center_x_changed, center_y_changed, center_z_changed, - _x, _y, _z) + _x, _y, _z) coords = [ - current_y * 1000, - current_x * -1000, - current_z * 1000, - _x * 100, - _y * 100, - _z * 100, - ] + current_y * 1000, + current_x * -1000, + current_z * 1000, + _x * 100, + _y * 100, + _z * 100, + ] speed = 80 mode = 0 mycobot.send_coords(coords, speed, mode) server.applyChanges() -def makeBox( msg ):# + +def makeBox(msg): marker = Marker() marker.type = Marker.SPHERE @@ -68,17 +72,19 @@ def makeBox( msg ):# return marker -def makeBoxControl( msg ):# - control = InteractiveMarkerControl() + +def makeBoxControl(msg): + control = InteractiveMarkerControl() control.always_visible = True - control.markers.append( makeBox(msg) ) - msg.controls.append( control ) + control.markers.append(makeBox(msg)) + msg.controls.append(control) return control -def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof = False):# + +def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof=False): int_marker = InteractiveMarker() int_marker.header.frame_id = "/joint1" - int_marker.pose.position = position # Defined the position of the marker + int_marker.pose.position = position # Defined the position of the marker int_marker.pose.orientation = orientation int_marker.scale = 0.1 @@ -89,7 +95,7 @@ def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof = F makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode - if show_6dof: + if show_6dof: control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 1 @@ -111,7 +117,7 @@ def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof = F if fixed: control.orientation_mode = InteractiveMarkerControl.FIXED int_marker.controls.append(control) - + control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 @@ -157,10 +163,11 @@ def make6DofMarker(fixed, interaction_mode, position, orientation, show_6dof = F int_marker.controls.append(control) server.insert(int_marker, processFeedback) - menu_handler.apply( server, int_marker.name ) + menu_handler.apply(server, int_marker.name) # marker box finish + def listener(): global server rospy.init_node('control_marker', anonymous=True) @@ -174,31 +181,31 @@ def listener(): 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') + coords = [0, 0, 0, 0, 0, 0] + 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, + make6DofMarker(True, InteractiveMarkerControl.NONE, position, orientation, True) server.applyChanges() pub = rospy.Publisher('joint_states', JointState, queue_size=10) - rate = rospy.Rate(30) # 10hz + rate = rospy.Rate(30) # 10hz # pub joint state joint_state_send = JointState() 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 = [] @@ -218,7 +225,6 @@ def listener(): value *= -1 data_list.append(value) - joint_state_send.position = data_list pub.publish(joint_state_send) @@ -227,7 +233,7 @@ def listener(): if __name__ == '__main__': - 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) listener() diff --git a/scripts/control_slider.py b/scripts/control_slider.py index 5b83846..f565676 100755 --- a/scripts/control_slider.py +++ b/scripts/control_slider.py @@ -1,6 +1,7 @@ #!/usr/bin/env python2 # from std_msgs.msg import String -import time, subprocess +import time +import subprocess import rospy from sensor_msgs.msg import JointState @@ -18,7 +19,8 @@ def callback(data): mc.send_radians(data_list, 80) # time.sleep(0.5) - + + def listener(): rospy.init_node('control_slider', anonymous=True) @@ -27,10 +29,11 @@ def listener(): # spin() simply keeps python from exiting until this node is stopped rospy.spin() + if __name__ == '__main__': print('sart') - port = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True).decode() + port = subprocess.check_output(['echo -n /dev/ttyUSB*'], + shell=True).decode() print(port) mc = MyCobot(port) print(mc) diff --git a/scripts/display.py b/scripts/display.py index 28a230d..e18c893 100755 --- a/scripts/display.py +++ b/scripts/display.py @@ -1,6 +1,7 @@ #!/usr/bin/env python2 # license removed for brevity -import time, subprocess +import time +import subprocess import rospy from sensor_msgs.msg import JointState @@ -14,20 +15,20 @@ def talker(): pub = rospy.Publisher('joint_states', JointState, queue_size=10) pub_marker = rospy.Publisher('visualization_marker', Marker, queue_size=10) rospy.init_node('display', anonymous=True) - rate = rospy.Rate(30) # 30hz + rate = rospy.Rate(30) # 30hz # pub joint state joint_state_send = JointState() 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 = [] @@ -43,14 +44,14 @@ def talker(): for index, value in enumerate(angles): data_list.append(value) + rospy.loginfo('{}'.format(data_list)) joint_state_send.position = data_list pub.publish(joint_state_send) coords = mycobot.get_coords() - rospy.loginfo('{}'.format(coords)) - #marker + # marker marker_.header.stamp = rospy.Time.now() marker_.type = marker_.SPHERE marker_.action = marker_.ADD @@ -58,15 +59,15 @@ def talker(): marker_.scale.y = 0.04 marker_.scale.z = 0.04 - #marker position initial + # marker position initial # print(coords) if not coords: - coords = [0,0,0,0,0,0] + coords = [0, 0, 0, 0, 0, 0] rospy.loginfo('error [101]: can not get coord values') - - marker_.pose.position.x = coords[1] / 1000 * -1 - marker_.pose.position.y = coords[0] / 1000 - marker_.pose.position.z = coords[2] / 1000 + + marker_.pose.position.x = coords[1] / 1000 * -1 + marker_.pose.position.y = coords[0] / 1000 + marker_.pose.position.z = coords[2] / 1000 marker_.color.a = 1.0 marker_.color.g = 1.0 @@ -74,12 +75,12 @@ def talker(): rate.sleep() + if __name__ == '__main__': - 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) try: talker() except rospy.ROSInterruptException: pass - diff --git a/scripts/listen_real.py b/scripts/listen_real.py new file mode 100755 index 0000000..b9085c6 --- /dev/null +++ b/scripts/listen_real.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python2 +# license removed for brevity +import time +import math + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from myCobotROS.srv import GetAngles + + +def talker(): + 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 + joint_state_send = JointState() + 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' + ] + 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('start loop ...') + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + + res = func() + radians_list = [ + res.joint_1 * (math.pi / 180), + res.joint_2 * (math.pi / 180), + res.joint_3 * (math.pi / 180), + res.joint_4 * (math.pi / 180), + res.joint_5 * (math.pi / 180), + res.joint_6 * (math.pi / 180), + ] + rospy.loginfo('res: {}'.format(radians_list)) + + joint_state_send.position = radians_list + pub.publish(joint_state_send) + rate.sleep() + + +if __name__ == '__main__': + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/scripts/mycobot_ros.py b/scripts/mycobot_ros.py new file mode 100755 index 0000000..8e6c946 --- /dev/null +++ b/scripts/mycobot_ros.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python2 +import rospy +from myCobotROS.srv import * + +from pymycobot.mycobot import MyCobot + +mc = None + + +def mycobot_services(): + global mc + 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) + + 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.loginfo('ready') + rospy.spin() + + +def set_angles(req): + angles = [ + req.joint_1, + req.joint_2, + req.joint_3, + req.joint_4, + req.joint_5, + req.joint_6, + ] + sp = req.speed + + if mc: + mc.send_angles(angles, sp) + + +def get_angles(req): + if mc: + angles = mc.get_angles() + print(angles) + return GetAnglesResponse(*angles) + + +def set_coords(req): + coords = [ + req.x, + req.y, + req.z, + req.rx, + req.ry, + req.rz, + ] + sp = req.speed + mod = req.model + + if mc: + mc.send_coords(coords, sp, mod) + + +def get_coords(req): + if mc: + coords = mc.get_coords() + return GetCoordsResponse(*coords) + + +def switch_status(req): + if mc: + if req.Status: + print(1) + mc.set_gripper_state(0, 80) + else: + print(2) + mc.set_gripper_state(1, 80) + + +if __name__ == '__main__': + # print(MyCobot.__dict__) + mycobot_services() diff --git a/scripts/teleop_keyboard.py b/scripts/teleop_keyboard.py new file mode 100755 index 0000000..7e520ff --- /dev/null +++ b/scripts/teleop_keyboard.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python +from __future__ import print_function +from myCobotROS.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('myCobotROS') + + +msg = """ +Mycobot Teleop Keyboard Controller +--------------------------- +Movimg options(control coord [x,y,z,rx,ry,rz]): + w(x+) + + a(y-) s(x-) d(y+) + +u(rx+) i(ry+) o(rz+) +j(rx-) k(ry-) l(rz-) + +Gripper control: + g - open + h - close + +Other: + 1 - Go to init pose + 2 - + 3 - + q - Quit +""" + + +def getKey(key_timeout): + tty.setraw(sys.stdin.fileno()) + rlist, _, _ = select.select([sys.stdin], [], [], key_timeout) + if rlist: + key = sys.stdin.read(1) + else: + key = '' + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + + +def vels(speed, turn): + return "currently:\tspeed %s\tchange size %s " % (speed, turn) + + +def teleop_keyboard(): + global settings + settings = termios.tcgetattr(sys.stdin) + + rospy.init_node('teleop_keyboard') + + model = 0 + speed = rospy.get_param("~speed", 50) + change_size = rospy.get_param("~change_size", 10) + key_timeout = rospy.get_param("~key_timeout", 0.0) + if key_timeout == 0.0: + key_timeout = None + + 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) + except: + print('start error ...') + exit(1) + + init_pose = [0, 0, 0, 0, 0, 0, speed] + home_pose = [0, 0, 0, 0, 0, 0, speed] + + # rsp = set_angles(*init_pose) + + while True: + res = get_coords() + if res.x > 5: + break + time.sleep(.1) + + 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_size)) + while(1): + try: + key = getKey(key_timeout) + if key == 'q': + break + elif key in ['w', 'W']: + record_coords[0] += change_size + set_coords(*record_coords) + elif key in ['s', 'S']: + record_coords[0] -= change_size + set_coords(*record_coords) + elif key in ['a', 'A']: + record_coords[1] -= change_size + set_coords(*record_coords) + elif key in ['d', 'D']: + record_coords[1] += change_size + set_coords(*record_coords) + elif key in ['z', 'Z']: + record_coords[2] -= change_size + set_coords(*record_coords) + elif key in ['x', 'X']: + record_coords[2] += change_size + set_coords(*record_coords) + elif key in ['u', 'U']: + record_coords[3] += change_size + set_coords(*record_coords) + elif key in ['j', 'J']: + record_coords[3] -= change_size + set_coords(*record_coords) + elif key in ['i', 'I']: + record_coords[4] += change_size + set_coords(*record_coords) + elif key in ['k', 'K']: + record_coords[4] -= change_size + set_coords(*record_coords) + elif key in ['o', 'O']: + record_coords[5] += change_size + set_coords(*record_coords) + elif key in ['l', 'L']: + record_coords[5] -= change_size + set_coords(*record_coords) + elif key in ['g', 'G']: + switch_gripper(True) + elif key in ['h', 'H']: + switch_gripper(False) + elif key == '1': + rsp = set_angles(*init_pose) + elif key in '2': + rsp = set_angles(*home_pose) + elif key in '3': + rep = get_angles() + home_pose[0] = rep.joint_1 + home_pose[1] = rep.joint_2 + home_pose[2] = rep.joint_3 + home_pose[3] = rep.joint_4 + home_pose[5] = rep.joint_5 + else: + continue + except Exception as e: + # print(e) + continue + + except Exception as e: + print(e) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + + +if __name__ == "__main__": + try: + teleop_keyboard() + except rospy.ROSInterruptException: + pass diff --git a/srv/GetAngles.srv b/srv/GetAngles.srv new file mode 100644 index 0000000..6992699 --- /dev/null +++ b/srv/GetAngles.srv @@ -0,0 +1,9 @@ + +--- + +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 diff --git a/srv/GetCoords.srv b/srv/GetCoords.srv new file mode 100644 index 0000000..c1f9bbc --- /dev/null +++ b/srv/GetCoords.srv @@ -0,0 +1,9 @@ + +--- + +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz \ No newline at end of file diff --git a/srv/GripperStatus.srv b/srv/GripperStatus.srv new file mode 100644 index 0000000..424572f --- /dev/null +++ b/srv/GripperStatus.srv @@ -0,0 +1,3 @@ +bool Status + +--- \ No newline at end of file diff --git a/srv/SetAngles.srv b/srv/SetAngles.srv new file mode 100644 index 0000000..1ad93cd --- /dev/null +++ b/srv/SetAngles.srv @@ -0,0 +1,10 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 + +int8 speed + +--- \ No newline at end of file diff --git a/srv/SetCoords.srv b/srv/SetCoords.srv new file mode 100644 index 0000000..b264760 --- /dev/null +++ b/srv/SetCoords.srv @@ -0,0 +1,11 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz + +int8 speed +int8 model + +--- \ No newline at end of file