diff --git a/ReadMe.md b/ReadMe.md
index e4fedcbd..76bf1022 100755
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -761,6 +761,7 @@ For installation with camera stand provided by UFACTORY, the cam model can be at
$ cd ~/catkin_ws/src/
# Download through git (mind to checkout the proper branch):
$ git clone https://github.com/JenniferBuehler/gazebo-pkgs.git
+ $ git clone https://github.com/JenniferBuehler/general-message-pkgs.git
# Compile:
$ cd ..
$ catkin_make
diff --git a/ReadMe_lite6.md b/ReadMe_lite6.md
new file mode 100644
index 00000000..aa0dd2ff
--- /dev/null
+++ b/ReadMe_lite6.md
@@ -0,0 +1,150 @@
+For Chinese version: [简体中文版说明](#重要说明)
+
+# Important Notice:
+This ReadMe is for new(Lite 6) and future UFACTORY product models other than xArm series. Here use "lite6" as example.
+
+If you have used "xarm_ros" for xArm series before, the main differences for new UFACTROY models are:
+
+* Default namespace has changed from `xarm` to `ufactory`;
+
+* Robot full-state report topic name has changed from `xarm_states` to `robot_states`;
+
+
+# 1. Simulations
+
+## 1.1 Simple Rviz visualization:
+```bash
+$ roslaunch xarm_description lite6_rviz_display.launch
+```
+The robot model will appear in Rviz window and "joint state publisher" panel can be used to adjust the posture of that robot.
+
+
+## 1.2 Load robot model in Gazebo environment:
+```bash
+$ roslaunch xarm_gazebo lite6_beside_table.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+Gazebo will be launched and virtual robot will be mounted on a table, `add_gripper` and `add_vacuum_gripper` are optional arguments. However, only one end tool should be installed.
+
+To launch with the overhead camera:
+```bash
+roslaunch xarm_gazebo lite6_camera_scene.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+
+## 1.3 Moveit simulation:
+
+### Without Gazebo:
+```bash
+$ roslaunch lite6_moveit_config demo.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+### With Gazebo:
+In the first terminal, run:
+```bash
+$ roslaunch xarm_gazebo lite6_beside_table.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+Then in another terminal:
+```bash
+$ roslaunch lite6_moveit_config lite6_moveit_gazebo.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+In this way, the simulated robot in gazebo can execute planned trajectory generated by Moveit.
+
+### Color recognition example
+In another terminal:
+```bash
+rosrun xarm_gazebo color_recognition_lite6.py
+```
+
+# 2. Controlling Real Robot
+
+# 2.1 ROS service control:
+First bring up the UFACTORY ros driver:
+```bash
+$ roslaunch xarm_bringup lite6_server.launch robot_ip:=192.168.1.xxx (your robot IP)
+```
+Then, please refer to [xarm_api/xarm_msgs part](https://github.com/xArm-Developer/xarm_ros#57-xarm_apixarm_msgs), the concepts, provided services or messages are all the same except the **default namespace for non-xArm series** is `ufactory` rather than `xarm`. For example, to call the service of enabling all joints:
+```bash
+$ rosservice call /ufactory/motion_ctrl 8 1
+```
+All the xArm services (joint/cartesian motion, velocity motion, servo motions, etc) including [mode operations](https://github.com/xArm-Developer/xarm_ros#6-mode-change) can be directly used on new models like Lite 6, just replace previous namespace `/xarm` with `/ufactory`.
+
+Another difference from xArm version is the topic **`/xarm/xarm_states`** has been changed to **`/ufactory/robot_states`** (with default namespace attached).
+
+# 2.2 Moveit control:
+First make sure the robot and controller box are powered on, then execute:
+```bash
+$ roslaunch lite6_moveit_config realMove_exec.launch robot_ip:=192.168.1.xxx [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+`add_gripper` and `add_vacuum_gripper` are optional available arguments if you have installed UFACTORY provided tool accessory. Only one end tool shall be installed. Below is the network diagram from `rqt_graph` output:
+
+
+
+
+
+
+# 重要说明:
+
+这篇ReadMe适用于UFACTORY xArm系列之外的产品(如Lite 6)，本说明使用 "lite6" 作为例子。
+
+如果您之前已经使用过 "xarm_ros" 开发 xArm 系列产品, 对于UFACTORY其他系列的ros开发，方法和操作大同小异，主要的区别在于:
+
+* 默认的命名空间从 `xarm` 更改为 `ufactory`;
+
+* 机械臂全状态反馈的话题(topic) 名称由 `xarm_states` 更改为 `robot_states`;
+
+
+# 1. 仿真
+
+## 1.1 简单的Rviz可视化:
+```bash
+$ roslaunch xarm_description lite6_rviz_display.launch
+```
+运行之后，机械臂的模型会出现在Rviz界面中，"joint state publisher" 面板可以用来调整手臂的位姿。
+
+
+## 1.2 Gazebo环境中加载仿真模型:
+```bash
+$ roslaunch xarm_gazebo lite6_beside_table.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+Gazebo启动后, 虚拟机械臂会放置在桌子边沿位置，`add_gripper` 和 `add_vacuum_gripper` 是另外的可选参数，可以根据需要给定`true`来加载UFACTORY官方的夹爪或吸头的配件模型，注意只能加载一款末端执行器。
+
+## 1.3 Moveit规划仿真:
+
+### 如果不使用Gazebo:
+```bash
+$ roslaunch lite6_moveit_config demo.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+### 如果使用Gazebo:
+打开一个终端, 执行:
+```bash
+$ roslaunch xarm_gazebo lite6_beside_table.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+然后，在另一个终端中执行:
+```bash
+$ roslaunch lite6_moveit_config lite6_moveit_gazebo.launch [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+这样，Gazebo中的虚拟手臂可以执行Moveit在Rviz界面中的规划路径。
+
+
+# 2. 控制真实机械臂
+
+# 2.1 ROS service 控制:
+首先启动 UFACTORY ROS 驱动:
+```bash
+$ roslaunch xarm_bringup lite6_server.launch robot_ip:=192.168.1.xxx (your robot IP)
+```
+然后，请认真阅读和参考[xarm_api/xarm_msgs 部分](https://github.com/xArm-Developer/xarm_ros/blob/master/ReadMe_cn.md#57-xarm_apixarm_msgs), 其中的概念、提供的服务和定义的消息类型都是和xarm一致的，除了默认的命名空间是 `ufactory` 而不是 `xarm`。举例来说，对于非xArm系列如Lite6手臂，调用全部关节使能服务的方法为:
+```bash
+$ rosservice call /ufactory/motion_ctrl 8 1
+```
+所有xArm适用的服务(关节/笛卡尔运动, 速度模式运动, servo运动等等)以及 [模式操作](https://github.com/xArm-Developer/xarm_ros/blob/master/ReadMe_cn.md#6-%E6%A8%A1%E5%BC%8F%E5%88%87%E6%8D%A2)都可以使用在Lite 6 型号中, 调用方法很简单，将之前说明中的`/xarm`命名空间替换为`/ufactory`就好。
+
+另一个值得注意的变化是xArm系列使用的话题 **`/xarm/xarm_states`** 被修改为 **`/ufactory/robot_states`** (已带上默认命名空间)。
+
+# 2.2 Moveit 规划控制:
+首先确认手臂已经正确上电, 然后执行:
+```bash
+$ roslaunch lite6_moveit_config realMove_exec.launch robot_ip:=192.168.1.xxx [add_gripper:=true] [add_vacuum_gripper:=true]
+```
+`add_gripper` 和 `add_vacuum_gripper` 是另外的可选参数，可以根据需要给定`true`来加载UFACTORY官方的夹爪或吸头的配件模型，注意只能加载一款末端执行器。下图是来自`rqt_graph`输出的网络结构示意图:
+
+
+
\ No newline at end of file
diff --git a/xarm_description/urdf/lite6_robot_cam.urdf.xacro b/xarm_description/urdf/lite6_robot_cam.urdf.xacro
new file mode 100755
index 00000000..102e8bdf
--- /dev/null
+++ b/xarm_description/urdf/lite6_robot_cam.urdf.xacro
@@ -0,0 +1,60 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/xarm_gazebo/launch/lite6_camera_scene.launch b/xarm_gazebo/launch/lite6_camera_scene.launch
new file mode 100755
index 00000000..a0f4982e
--- /dev/null
+++ b/xarm_gazebo/launch/lite6_camera_scene.launch
@@ -0,0 +1,42 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/xarm_gazebo/scripts/color_recognition.py b/xarm_gazebo/scripts/color_recognition.py
index fbefdf6b..1da7fc84 100755
--- a/xarm_gazebo/scripts/color_recognition.py
+++ b/xarm_gazebo/scripts/color_recognition.py
@@ -225,7 +225,7 @@ def get_recognition_rect(frame, lower=COLOR_DICT['red']['lower'], upper=COLOR_DI
if PY3:
box = cv2.boxPoints(rect)
else:
- box = cv2.cv.BoxPoints(rect)
+ box = cv2.boxPoints(rect)
cv2.drawContours(frame, [np.int0(box)], -1, (0, 255, 255), 1)
rects.append(rect)
diff --git a/xarm_gazebo/scripts/color_recognition_lite6.py b/xarm_gazebo/scripts/color_recognition_lite6.py
new file mode 100755
index 00000000..736f667b
--- /dev/null
+++ b/xarm_gazebo/scripts/color_recognition_lite6.py
@@ -0,0 +1,268 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import cv2
+import sys
+import rospy
+import math
+import random
+import threading
+import numpy as np
+import moveit_commander
+from cv_bridge import CvBridge
+from sensor_msgs.msg import CompressedImage
+
+if sys.version_info < (3, 0):
+ PY3 = False
+ import Queue as queue
+else:
+ PY3 = True
+ import queue
+
+
+COLOR_DICT = {
+ 'red': {'lower': np.array([0, 43, 46]), 'upper': np.array([10, 255, 255])},
+ 'blue': {'lower': np.array([90, 100, 100]), 'upper': np.array([130, 255, 255])},
+ 'green': {'lower': np.array([50, 60, 60]), 'upper': np.array([77, 255, 255])},
+ 'yellow': {'lower': np.array([20, 40, 46]), 'upper': np.array([34, 255, 255])},
+}
+
+
+class GripperCtrl(object):
+ def __init__(self):
+ self._commander = moveit_commander.move_group.MoveGroupCommander('tool_end')
+ # self._commander = moveit_commander.move_group.MoveGroupCommander('xarm_gripper')
+ self._init()
+
+ def _init(self):
+ self._commander.set_max_acceleration_scaling_factor(1.0)
+ self._commander.set_max_velocity_scaling_factor(1.0)
+
+ def open(self, wait=True):
+ try:
+ # self._commander.set_named_target('open')
+ ret = self._commander.go(wait=wait)
+ print('gripper_open, ret={}'.format(ret))
+ return ret
+ except Exception as e:
+ print('[Ex] gripper open exception, {}'.format(e))
+ return False
+
+ def close(self, wait=True):
+ try:
+ # self._commander.set_named_target('close')
+ ret = self._commander.go(wait=wait)
+ print('gripper_close, ret={}'.format(ret))
+ return ret
+ except Exception as e:
+ print('[Ex] gripper close exception, {}'.format(e))
+ return False
+
+
+class XArmCtrl(object):
+ def __init__(self, dof):
+ self._commander = moveit_commander.move_group.MoveGroupCommander('lite6')
+ # self._commander = moveit_commander.move_group.MoveGroupCommander('xarm{}'.format(dof))
+ self.dof = int(dof)
+ self._init()
+
+ def _init(self):
+ self._commander.set_max_acceleration_scaling_factor(1.0)
+ self._commander.set_max_velocity_scaling_factor(1.0)
+
+ def set_joints(self, angles, wait=True):
+ try:
+ joint_target = self._commander.get_current_joint_values()
+ for i in range(joint_target):
+ if i >= len(angles):
+ break
+ if angles[i] is not None:
+ joint_target[i] = math.radians(angles[i])
+ print('set_joints, joints={}'.format(joint_target))
+ self._commander.set_joint_value_target(joint_target)
+ ret = self._commander.go(wait=wait)
+ print('move to finish, ret={}'.format(ret))
+ return ret
+ except Exception as e:
+ print('[Ex] set_joints exception, ex={}'.format(e))
+
+ def set_joint(self, angle, inx=-1, wait=True):
+ try:
+ joint_target = self._commander.get_current_joint_values()
+ joint_target[inx] = math.radians(angle)
+ print('set_joints, joints={}'.format(joint_target))
+ self._commander.set_joint_value_target(joint_target)
+ ret = self._commander.go(wait=wait)
+ print('move to finish, ret={}'.format(ret))
+ return ret
+ except Exception as e:
+ print('[Ex] set_joint exception, ex={}'.format(e))
+ return False
+
+ def moveto(self, x=None, y=None, z=None, ox=None, oy=None, oz=None, relative=False, wait=True):
+ if x == 0 and y == 0 and z == 0 and ox == 0 and oy == 0 and oz == 0 and relative:
+ return True
+ try:
+ pose_target = self._commander.get_current_pose().pose
+ if relative:
+ pose_target.position.x += x / 1000.0 if x is not None else 0
+ pose_target.position.y += y / 1000.0 if y is not None else 0
+ pose_target.position.z += z / 1000.0 if z is not None else 0
+ pose_target.orientation.x += ox if ox is not None else 0
+ pose_target.orientation.y += oy if oy is not None else 0
+ pose_target.orientation.z += oz if oz is not None else 0
+ else:
+ pose_target.position.x = x / 1000.0 if x is not None else pose_target.position.x
+ pose_target.position.y = y / 1000.0 if y is not None else pose_target.position.y
+ pose_target.position.z = z / 1000.0 if z is not None else pose_target.position.z
+ pose_target.orientation.x = ox if ox is not None else pose_target.orientation.x
+ pose_target.orientation.y = oy if oy is not None else pose_target.orientation.y
+ pose_target.orientation.z = oz if oz is not None else pose_target.orientation.z
+ print('move to position=[{:.2f}, {:.2f}, {:.2f}], orientation=[{:.6f}, {:.6f}, {:.6f}]'.format(
+ pose_target.position.x * 1000.0, pose_target.position.y * 1000.0, pose_target.position.z * 1000.0,
+ pose_target.orientation.x, pose_target.orientation.y, pose_target.orientation.z
+ ))
+ if self.dof == 7:
+ path, fraction = self._commander.compute_cartesian_path([pose_target], 0.005, 0.0)
+ if fraction < 0.9:
+ ret = False
+ else:
+ ret = self._commander.execute(path, wait=wait)
+ print('move to finish, ret={}, fraction={}'.format(ret, fraction))
+ else:
+ self._commander.set_pose_target(pose_target)
+ ret = self._commander.go(wait=wait)
+ print('move to finish, ret={}'.format(ret))
+ return ret
+ except Exception as e:
+ print('[Ex] moveto exception: {}'.format(e))
+ return False
+
+
+class GazeboMotionThread(threading.Thread):
+ def __init__(self, que, **kwargs):
+ if PY3:
+ super().__init__()
+ else:
+ super(GazeboMotionThread, self).__init__()
+ self.que = que
+ self.daemon = True
+ self.in_motion = True
+ dof = kwargs.get('dof', 6)
+ self._xarm_ctrl = XArmCtrl(dof)
+ self._gripper_ctrl = GripperCtrl()
+ self._grab_z = kwargs.get('grab_z', 10)
+ self._safe_z = kwargs.get('safe_z', 100)
+
+ @staticmethod
+ def _rect_to_move_params(rect):
+ return int((466 - rect[0][1]) * 900.0 / 460.0 + 253.3), int((552 - rect[0][0]) * 900.0 / 460.0 - 450), rect[2] - 90
+
+ def run(self):
+ while True:
+ # self._xarm_ctrl.set_joint(0)
+ # self._gripper_ctrl.open()
+ if not self._xarm_ctrl.moveto(z=self._safe_z):
+ continue
+ if not self._xarm_ctrl.moveto(x=300, y=0, z=self._safe_z):
+ continue
+ self.in_motion = False
+ rects = self.que.get()
+ self.in_motion = True
+
+ rect = rects[random.randint(0, 100) % len(rects)]
+ x, y, angle = self._rect_to_move_params(rect)
+ print('target: x={:.2f}mm, y={:.2f}mm, anlge={:.2f}'.format(x, y, angle))
+ ret = self._xarm_ctrl.moveto(z=self._safe_z)
+ if not ret:
+ continue
+ ret = self._xarm_ctrl.moveto(x=x, y=y, z=self._safe_z)
+ if not ret:
+ continue
+ # ret = self._xarm_ctrl.set_joint(angle)
+ # if not ret:
+ # continue
+ ret = self._xarm_ctrl.moveto(x=x, y=y, z=self._grab_z)
+ if not ret:
+ continue
+ # self._gripper_ctrl.close()
+ ret = self._xarm_ctrl.moveto(x=x, y=y, z=self._safe_z)
+ if not ret:
+ continue
+ ret = self._xarm_ctrl.moveto(x=x, y=y, z=self._grab_z)
+ if not ret:
+ continue
+ self._gripper_ctrl.open()
+ self._xarm_ctrl.moveto(x=x, y=y, z=self._safe_z)
+
+
+class GazeboCamera(object):
+ def __init__(self, topic_name='/camera/image_raw/compressed'):
+ self._frame_que = queue.Queue(10)
+ self._bridge = CvBridge()
+ self._img_sub = rospy.Subscriber(topic_name, CompressedImage, self._img_callback)
+
+ def _img_callback(self, data):
+ if self._frame_que.full():
+ self._frame_que.get()
+ self._frame_que.put(self._bridge.compressed_imgmsg_to_cv2(data))
+
+ def get_frame(self):
+ if self._frame_que.empty():
+ return None
+ return self._frame_que.get()
+
+
+def get_recognition_rect(frame, lower=COLOR_DICT['red']['lower'], upper=COLOR_DICT['red']['upper'], show=True):
+ gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)
+ hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)
+ erode_hsv = cv2.erode(hsv, None, iterations=2)
+ inRange_hsv = cv2.inRange(erode_hsv, lower, upper)
+ contours = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+ rects = []
+ for _, c in enumerate(contours):
+ rect = cv2.minAreaRect(c)
+ if rect[1][0] < 20 or rect[1][1] < 20:
+ continue
+ # print(rect)
+ if PY3:
+ box = cv2.boxPoints(rect)
+ else:
+ box = cv2.boxPoints(rect)
+ cv2.drawContours(frame, [np.int0(box)], -1, (0, 255, 255), 1)
+ rects.append(rect)
+
+ if show:
+ cv2.imshow("Frame", frame)
+
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord('q'):
+ rospy.signal_shutdown('key to exit')
+ return rects
+
+
+if __name__ == '__main__':
+ rospy.init_node('color_recognition_node', anonymous=False)
+ dof = rospy.get_param('/xarm/DOF', default=6)
+ rate = rospy.Rate(10.0)
+
+ motion_que = queue.Queue(1)
+ motion = GazeboMotionThread(motion_que, dof=dof)
+ motion.start()
+
+ color = COLOR_DICT['red']
+
+ cam = GazeboCamera(topic_name='/camera/image_raw/compressed')
+
+ while not rospy.is_shutdown():
+ rate.sleep()
+ frame = cam.get_frame()
+ if frame is None:
+ continue
+ rects = get_recognition_rect(frame, lower=color['lower'], upper=color['upper'])
+ if len(rects) == 0:
+ continue
+ if motion.in_motion or motion_que.qsize() != 0:
+ continue
+ motion_que.put(rects)
+
+
\ No newline at end of file