add 320 gripper slider control

mycobot_320/new_mycobot_320/CMakeLists.txt

@@ -66,6 +66,7 @@ include_directories(include${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
 catkin_install_python(PROGRAMS
   scripts/mycobot_320_follow_display.py
   scripts/mycobot_320_slider.py
+  scripts/mycobot_320_gripper_slider.py
   scripts/mycobot_320_teleop_keyboard.py
   scripts/mycobot_320_listen_real.py
   scripts/mycobot_320_listen_real_of_topic.py

mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch

@@ -0,0 +1,31 @@
+<launch>
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/new_320_urdf/new_mycobot_pro_320_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find new_mycobot_320)/config/new_mycobot_320_gripper.rviz" />
+	<arg name="gui" default="true" />
+
+    <!-- <include file="$(find mycobot_280)/launch/slider_control.launch">
+		<arg name="model" value="$(arg model)" />
+		<arg name="rvizconfig" value="$(arg rvizconfig)" />
+		<arg name="gui" value="$(arg gui)" />
+    </include> -->
+
+	<!-- new -->
+	<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="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python2
+# -*- coding:utf-8 -*-
+"""[summary]
+This file obtains the joint angle of the manipulator in ROS,
+and then sends it directly to the real manipulator using `pymycobot` API.
+This file is [slider_control.launch] related script.
+Passable parameters:
+    port: serial prot string. Defaults is '/dev/ttyUSB0'
+    baud: serial prot baudrate. Defaults is 115200.
+"""
+
+import rospy
+import time
+from sensor_msgs.msg import JointState
+
+from pymycobot.mycobot import MyCobot
+
+
+mc = None
+gripper_value = []
+
+def callback(data):
+    global mc
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+    data_list = []
+    for index, value in enumerate(data.position):
+        data_list.append(round(value, 3))
+
+    data_list = data_list[:7]
+    print("radians:%s"%data_list[:6])
+    mc.send_radians(data_list[:6], 80)
+    gripper_value = int(abs(-0.7-data_list[6])* 117)
+    print("gripper_value:%s"%gripper_value)
+    # mc.set_gripper_mode(0)
+    # time.sleep(1)
+    mc.set_gripper_value(gripper_value, 100)
+    
+def listener():
+    global mc
+    global gripper_value
+   
+    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)
+    print(port, baud)
+    mc = MyCobot(port, baud)
+ 
+    # spin() simply keeps python from exiting until this node is stopped
+    print("spin ...")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    listener()

mycobot_320/new_mycobot_320/scripts/test.py

@@ -4,36 +4,36 @@ from pymycobot.genre import Angle
 from pymycobot import PI_PORT, PI_BAUD  # 当使用树莓派版本的mycobot时，可以引用这两个变量进行MyCobot初始化
 import time
 
-mc = MyCobot("/dev/ttyUSB0", 115200)
+mc = MyCobot("/dev/ttyACM0", 115200)
 # 通过传递角度参数，让机械臂每个关节移动到对应[0, 0, 0, 0, 0, 0]的位置
 mc.send_angles([0, 0, 0, 0, 0, 0], 50)
 
 # 设置等待时间，确保机械臂已经到达指定位置
-time.sleep(2.5)
+# time.sleep(2.5)
 
 # 让关节1移动到90这个位置
 # mc.send_angle(Angle.J1.value, 90, 50)
 # 设置等待时间，确保机械臂已经到达指定位置
 
-mc.send_coords([73.2, -85.9, 495.7, -94.26, -30.17, -84.42],20,0)
-time.sleep(2)
+# mc.send_coords([73.2, -85.9, 495.7, -94.26, -30.17, -84.42],20,0)
+# time.sleep(2)
 
-mc.send_angles([2.19, 0.35, -3.42, -0.61, 1.23, -30.32],20)
+# mc.send_angles([2.19, 0.35, -3.42, -0.61, 1.23, -30.32],20)
 
-num=0
-while num<5:
-    print('-------------------->1')
-    print(mc.get_angles())
-    time.sleep(1)
-    print('--------------------->2')
-    print(mc.get_coords())
-    print('----------------->3')
-    time.sleep(1)
-    print(mc.get_radians())
+# num=0
+# while num<5:
+#     print('-------------------->1')
+#     print(mc.get_angles())
+#     time.sleep(1)
+#     print('--------------------->2')
+#     print(mc.get_coords())
+#     print('----------------->3')
+#     time.sleep(1)
+#     print(mc.get_radians())
 
-    num+=1
+#     num+=1
 
-time.sleep(2)
+# time.sleep(2)
 
 # # 以下代码可以让机械臂左右摇摆
 # # 设置循环次数
@@ -51,4 +51,37 @@ time.sleep(2)
 #     # 设置等待时间，确保机械臂已经到达指定位置
 #     time.sleep(1.5)
 
-#     num 
+
+#     num 
+mc.set_gripper_mode(0)
+# print('11')
+# time.sleep(1)
+# mc.set_gripper_value(50, 50)
+# time.sleep(1)
+# print(mc.get_angles())
+
+mc.set_gripper_state(0,100)
+time.sleep(0.05)
+
+angles = [[-27.64, 15.2, -53.34, -52.47, 88.76, 99.05]]
+coords = [189.7, -350.6, 334.0, -170.85, -0.86, -91.24]
+mc.set_color(0, 255, 0)
+for i in range(4):
+    coords[1] += 150
+    # mc.send_coords(coords, 100, 1)
+    time.sleep(2)
+    mc.set_gripper_state(0,100)
+    time.sleep(0.05)
+    coords[2] -= 155
+    # mc.send_coords(coords,100,1)
+    print(mc.get_angles())
+    time.sleep(3)
+    mc.set_gripper_state(1,100)
+    time.sleep(1)
+    coords[2] += 155
+    # mc.send_coords(coords,100,1)
+    time.sleep(2)
+    coords[2] -= 155
+    # mc.send_coords(coords,100,1)
+    time.sleep(2)
+    mc.set_gripper_state(0,100)