feat: add teleop_keyboard

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}
 )
 

launch/communication_service.launch

@@ -0,0 +1,10 @@
+<launch>
+	<arg name="PORT" default="/dev/ttyUSB0" />
+	<arg name="BAUD" default="115200" />
+
+	<!-- Open communication service -->
+	<node name="mycobot_services" pkg="myCobotROS" type="mycobot_ros.py">
+		<param name="port" type="string" value="$(arg PORT)" />	
+		<param name="baud" type="int" value="$(arg BAUD)" />	
+    </node> 
+</launch>

launch/control_slider.launch

@@ -1,16 +1,16 @@
 <launch>
 	<arg name="model" default="$(find myCobotROS)/urdf/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find myCobotROS)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
 
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
 
 	<!-- Combinejoin values to TF -->
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 
-		<param name="use_gui" value="true"/>	
+		<param name="use_gui" value="$(arg gui)" />	
-		<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
 	</node>
-
 	<!-- Open control script -->
 	<node name="control_slider" pkg="myCobotROS" type="control_slider.py"/> 
 	<!-- Show in Rviz -->

launch/mycobot_teleop_keyboard.launch

@@ -0,0 +1,21 @@
+<launch>
+	<arg name="PORT" default="/dev/ttyUSB0" />
+	<arg name="BAUD" default="115200" />
+
+	<arg name="model" default="$(find myCobotROS)/urdf/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find myCobotROS)/config/mycobot.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find myCobotROS)/launch/communication_service.launch">
+		<param name="port" type="string" value="$(arg PORT)" />	
+		<param name="baud" type="int" value="$(arg BAUD)" />	
+    </include>
+	<node name="real_listener" pkg="myCobotROS" type="listen_real.py" />
+</launch>

msg/Gripper.msg

@@ -0,0 +1 @@
+bool Status

package.xml

@@ -15,11 +15,13 @@
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>serial</build_depend>
+  <build_depend>message_generation</build_depend>
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>serial</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

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()

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 *
@@ -24,7 +25,9 @@ center_y_changed = 0
 center_z_changed = 0
 
 # marker box
-def processFeedback( feedback ):#
+
+
+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
@@ -54,7 +57,8 @@ def processFeedback( feedback ):#
 
     server.applyChanges()
 
-def makeBox( msg ):#
+
+def makeBox(msg):
     marker = Marker()
 
     marker.type = Marker.SPHERE
@@ -68,14 +72,16 @@ def makeBox( msg ):#
 
     return marker
 
-def makeBoxControl( msg ):#
+
+def makeBoxControl(msg):
     control = InteractiveMarkerControl()
     control.always_visible = True
-    control.markers.append( makeBox(msg) )
+    control.markers.append(makeBox(msg))
-    msg.controls.append( control )
+    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
@@ -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,7 +181,7 @@ def listener():
     menu_handler.insert('Second Entry', callback=processFeedback)
 
     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')
     # initial position
     position = Point(coords[1] / -1000, coords[0] / 1000, coords[2] / 1000)
@@ -218,7 +225,6 @@ def listener():
                     value *= -1
                 data_list.append(value)
 
-            
             joint_state_send.position = data_list
 
             pub.publish(joint_state_send)

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
@@ -19,6 +20,7 @@ def callback(data):
     mc.send_radians(data_list, 80)
     # time.sleep(0.5)
 
+
 def listener():
     rospy.init_node('control_slider', anonymous=True)
 
@@ -27,6 +29,7 @@ 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*'],

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
@@ -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,10 +59,10 @@ 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
@@ -74,6 +75,7 @@ def talker():
 
         rate.sleep()
 
+
 if __name__ == '__main__':
     port = subprocess.check_output(['echo -n /dev/ttyUSB*'],
                                    shell=True).decode()
@@ -82,4 +84,3 @@ if __name__ == '__main__':
         talker()
     except rospy.ROSInterruptException:
         pass
-

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

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()

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

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

srv/GetCoords.srv

@@ -0,0 +1,9 @@
+
+--- 
+
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz

srv/GripperStatus.srv

@@ -0,0 +1,3 @@
+bool Status
+
+---

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
+
+---

srv/SetCoords.srv

@@ -0,0 +1,11 @@
+float32 x
+float32 y
+float32 z
+float32 rx
+float32 ry
+float32 rz
+
+int8 speed
+int8 model
+
+---