diff --git a/.github/workflows/industrialci.yml b/.github/workflows/industrialci.yml new file mode 100644 index 0000000..2196f9b --- /dev/null +++ b/.github/workflows/industrialci.yml @@ -0,0 +1,30 @@ +name: industrial_ci + +on: + push: + paths-ignore: + - '**.md' + - '**.jpg' + - '**.pdf' + - '**.png' + pull_request: + paths-ignore: + - '**.md' + - '**.jpg' + - '**.pdf' + - '**.png' + schedule: + - cron: "0 1 * * 2" # Weekly on Tuesdays at 01:00(GMT) + +jobs: + industrial_ci: + strategy: + matrix: + env: + - { ROS_DISTRO: melodic, ROS_REPO: main } + # - { ROS_DISTRO: noetic, ROS_REPO: main } + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: "ros-industrial/industrial_ci@master" + env: ${{ matrix.env }} diff --git a/CHANGELOG.md b/CHANGELOG.md index 843a5da..3dfe6ab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,4 +6,8 @@ - Specification file nmae. - Added teleop keyboard control. - Intergrated MoveIt. -- Update sync moveit plan script. \ No newline at end of file +- Update sync moveit plan script. + +## 05.21 + +- Feature: simple gui. \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f87f48..98ed77b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ catkin_install_python(PROGRAMS scripts/client.py scripts/detect_marker.py scripts/following_marker.py + scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -62,4 +63,7 @@ target_link_libraries(opencv_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_executable(camera_display src/camera_display) target_link_libraries(camera_display ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) - +if (CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(launch) +endif() diff --git a/README.md b/README.md index 60b81e6..de01368 100644 --- a/README.md +++ b/README.md @@ -86,8 +86,15 @@ python scripts/test.py ![Demo](./Screenshot-3.png) ![Demo](./Screenshot-4.png) -## Q & A + +![Demo](./Screenshot-5.png) + +--- + +~*END*~ + + diff --git a/Screenshot-1.png b/Screenshot-1.png index 4a46889..f273441 100644 Binary files a/Screenshot-1.png and b/Screenshot-1.png differ diff --git a/Screenshot-2.png b/Screenshot-2.png index 355339e..4e73cea 100644 Binary files a/Screenshot-2.png and b/Screenshot-2.png differ diff --git a/Screenshot-3.png b/Screenshot-3.png index 045fca4..a595a0b 100644 Binary files a/Screenshot-3.png and b/Screenshot-3.png differ diff --git a/Screenshot-5.png b/Screenshot-5.png new file mode 100644 index 0000000..045fca4 Binary files /dev/null and b/Screenshot-5.png differ diff --git a/docker-compose.yml b/docker-compose.yml index 4ee3dea..9b053d1 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,7 +1,7 @@ version: '3.4' x-app: &common - command: [ "roslaunch mycobot_ros control_slider.launch" ] + command: [ "roslaunch mycobot_ros mycobot_slider.launch" ] privileged: true environment: PYTHONUNBUFFERED: 1 diff --git a/launch/mycobot_simple_gui.launch b/launch/mycobot_simple_gui.launch new file mode 100644 index 0000000..9db16e6 --- /dev/null +++ b/launch/mycobot_simple_gui.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/run_benchmark_ompl.launch b/launch/run_benchmark_ompl.launch index 066ed63..18d2446 100644 --- a/launch/run_benchmark_ompl.launch +++ b/launch/run_benchmark_ompl.launch @@ -1,7 +1,7 @@ - + diff --git a/launch/warehouse.launch b/launch/warehouse.launch index fbae8fb..aff99ab 100644 --- a/launch/warehouse.launch +++ b/launch/warehouse.launch @@ -1,7 +1,7 @@ - + diff --git a/package.xml b/package.xml index ac79a27..200781a 100644 --- a/package.xml +++ b/package.xml @@ -13,31 +13,37 @@ catkin - moveit_ros_move_group - moveit_fake_controller_manager - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - joint_state_publisher - robot_state_publisher - xacro - roscpp rospy std_msgs message_generation + actionlib + moveit_msgs roscpp rospy std_msgs message_runtime - - actionlib - moveit_msgs - actionlib moveit_msgs + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_benchmarks + moveit_ros_visualization + moveit_ros_move_group + moveit_ros_warehouse + moveit_setup_assistant + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + xacro + joy + rviz + warehouse_ros_mongo + gazebo_ros + controller_manager + python-tk diff --git a/scripts/detect_marker.py b/scripts/detect_marker.py index b61710d..867a500 100755 --- a/scripts/detect_marker.py +++ b/scripts/detect_marker.py @@ -13,7 +13,7 @@ from mycobot_ros.srv import ( GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) -class image_converter: +class ImageConverter: def __init__(self): self.br = TransformBroadcaster() self.bridge = CvBridge() @@ -35,9 +35,13 @@ class image_converter: def callback(self, data): + '''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") - # sucess, cv_image = self.cap.read() except CvBridgeError as e: print(e) size = cv_image.shape @@ -63,24 +67,14 @@ class image_converter: cv.aruco.drawDetectedMarkers(cv_image, corners) 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, :] - euler = rvec[0, 0, :] 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' ) - - # res = self.get_coords() - # if res.x == res.y == 0.0: - # return - # record_coords = [ - # res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1 - # ] - # print(record_coords) - # [x, y, z, -172, 3, -46.8] cv.imshow("Image", cv_image) @@ -93,7 +87,7 @@ if __name__ == '__main__': try: rospy.init_node("detect_marker") rospy.loginfo("Starting cv_bridge_test node") - image_converter() + ImageConverter() rospy.spin() except KeyboardInterrupt: print "Shutting down cv_bridge_test node." diff --git a/scripts/following_marker.py b/scripts/following_marker.py index 8f6be50..3a882b4 100755 --- a/scripts/following_marker.py +++ b/scripts/following_marker.py @@ -1,5 +1,4 @@ #!/usr/bin/env python2 -# license removed for brevity import time import rospy @@ -25,8 +24,6 @@ def talker(): print('publishing ...') while not rospy.is_shutdown(): now = rospy.Time.now() - rospy.Duration(0.1) - # now = rospy.Time.now() - # print(now) try: trans, rot = listener.lookupTransform('joint1', 'basic_shapes', now) diff --git a/scripts/simple_gui.py b/scripts/simple_gui.py new file mode 100755 index 0000000..2c67feb --- /dev/null +++ b/scripts/simple_gui.py @@ -0,0 +1,349 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +import Tkinter as tk +from mycobot_ros.srv import ( + GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) +import rospy +import time +from rospy import ServiceException + +class Window: + def __init__(self,handle): + self.win = handle + 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.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 + # 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)) + # 布局 + self.set_layout() + # 输入部分 + self.need_input() + # 展示部分 + 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) + + # 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.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) + + 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.switch_gripper = rospy.ServiceProxy( + 'switch_gripper_status', GripperStatus) + except: + print('start error ...') + exit(1) + + 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) + + 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) + + # 设置输入框的默认值 + self.j1_default = tk.StringVar() + self.j1_default.set(self.res_angles[0]) + self.j2_default = tk.StringVar() + self.j2_default.set(self.res_angles[1]) + self.j3_default = tk.StringVar() + self.j3_default.set(self.res_angles[2]) + self.j4_default = tk.StringVar() + self.j4_default.set(self.res_angles[3]) + self.j5_default = tk.StringVar() + 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() + self.y_default.set(self.record_coords[1]) + self.z_default = tk.StringVar() + self.z_default.set(self.record_coords[2]) + self.rx_default = tk.StringVar() + self.rx_default.set(self.record_coords[3]) + self.ry_default = tk.StringVar() + 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_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) + 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_4 = tk.Entry(self.frmLT, textvariable=self.j4_default) + 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_6 = tk.Entry(self.frmLT, textvariable=self.j6_default) + 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.y = tk.Entry(self.frmRT, textvariable=self.y_default) + 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.rx = tk.Entry(self.frmRT, textvariable=self.rx_default) + 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.rz = tk.Entry(self.frmRT, textvariable=self.rz_default) + 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] + + # 速度输入框 + 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) + + # get数据 + + # ，展示出来 + self.cont_1 = tk.StringVar(self.frmLC) + 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_3 = tk.StringVar(self.frmLC) + 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_5 = tk.StringVar(self.frmLC) + 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] + + # 显示 + 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() + self.coord_y.set(str(self.record_coords[1])) + self.coord_z = tk.StringVar() + self.coord_z.set(str(self.record_coords[2])) + self.coord_rx = tk.StringVar() + self.coord_rx.set(str(self.record_coords[3])) + self.coord_ry = tk.StringVar() + 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) + + # mm 单位展示 + self.unit = tk.StringVar() + self.unit.set('mm') + for i in range(6): + 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 + c_value.append(self.speed) + c_value.append(self.model) + # print(c_value) + try: + self.set_coords(*c_value) + except ServiceException: + pass + 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 + j_value.append(self.speed) + + try: + self.set_angles(*j_value) + except ServiceException: + pass + self.show_j_date(j_value[:-1]) + # return j_value,c_value,speed + + + def get_date(self): + # 拿机械臂的数据，用于展示 + t = time.time() + while time.time() - t < 2: + self.res = self.get_coords() + if self.res.x > 1: + break + time.sleep(.1) + + t = time.time() + while time.time() - t < 2: + self.angles = self.get_angles() + if self.angles.joint_1 > 1: + break + time.sleep(.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 + ] + 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=""): + # 展示数据 + if way == "coord": + 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)+"°") + + def run(self): + self.win.mainloop() + +def main(): + window = tk.Tk() + window.title('mycobot ros GUI') + Window(window).run() + +if __name__ == "__main__": + main() diff --git a/scripts/test.py b/scripts/test.py index 7be8093..0b438bc 100644 --- a/scripts/test.py +++ b/scripts/test.py @@ -52,7 +52,7 @@ if __name__ == '__main__': print('==> send coord id: X, coord value: -40, speed: 70\n') time.sleep(2) - print('::set_free_mode()') - mycobot.set_free_mode() + print('::release_all_servos()') + mycobot.release_all_servos() print('==> into free moving mode.') print('=== check end <==\n')