Small changes on the color recognition and readme

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

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:  
+
+
+![uf_moveit_rqt_graph](./doc/uf_moveit_rqt_graph.png) 
+
+
+
+# 重要说明:
+
+这篇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`输出的网络结构示意图:  
+
+
+![uf_moveit_rqt_graph](./doc/uf_moveit_rqt_graph.png) 

xarm_description/urdf/lite6_robot_cam.urdf.xacro

@@ -0,0 +1,60 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6">
+  <xacro:arg name="prefix" default=""/>
+  <xacro:arg name="ros_namespace" default="xarm"/>
+  <xacro:arg name="limited" default="false"/>
+  <xacro:arg name="effort_control" default="false"/>
+  <xacro:arg name="velocity_control" default="false"/>
+  <xacro:arg name="add_gripper" default="false"/>
+  <xacro:arg name="add_vacuum_gripper" default="false"/>
+
+  <xacro:arg name="add_other_geometry" default="false"/>
+  <xacro:arg name="geometry_type" default="box"/>
+  <xacro:arg name="geometry_mass" default="0.1"/>
+  <xacro:arg name="geometry_height" default="0.1"/>
+  <xacro:arg name="geometry_radius" default="0.1"/>
+  <xacro:arg name="geometry_length" default="0.1"/>
+  <xacro:arg name="geometry_width" default="0.1"/>
+  <xacro:arg name="geometry_mesh_filename" default=""/>
+  <xacro:arg name="geometry_mesh_origin_xyz" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_origin_rpy" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_xyz" default="0 0 0"/>
+  <xacro:arg name="geometry_mesh_tcp_rpy" default="0 0 0"/>
+
+  <xacro:arg name="add_realsense_d435i" default="false"/>
+  <xacro:arg name="model1300" default="false"/>
+  <xacro:arg name="use_gazebo_camera" default="false"/>
+
+  <!-- load lite6 robot -->
+  <xacro:include filename="$(find xarm_description)/urdf/lite6_robot_macro.xacro" />
+  <xacro:lite6_robot prefix="$(arg prefix)" namespace="$(arg ros_namespace)" limited="$(arg limited)" 
+    effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
+    rs_d435i="$(arg add_realsense_d435i)"
+    add_other_geometry="$(arg add_other_geometry)" 
+    geometry_type="$(arg geometry_type)" geometry_mass="$(arg geometry_mass)"
+    geometry_height="$(arg geometry_height)" geometry_radius="$(arg geometry_radius)"
+    geometry_length="$(arg geometry_length)" geometry_width="$(arg geometry_width)"
+    geometry_mesh_filename="$(arg geometry_mesh_filename)"
+    geometry_mesh_origin_xyz="$(arg geometry_mesh_origin_xyz)" geometry_mesh_origin_rpy="$(arg geometry_mesh_origin_rpy)"
+    geometry_mesh_tcp_xyz="$(arg geometry_mesh_tcp_xyz)" geometry_mesh_tcp_rpy="$(arg geometry_mesh_tcp_rpy)" />
+
+  <xacro:if value="$(arg add_gripper)">
+    <xacro:include filename="$(find xarm_description)/urdf/lite_gripper.urdf.xacro" />
+    <xacro:uflite_gripper_urdf attach_to="$(arg prefix)link_eef" />
+  </xacro:if>
+
+  <xacro:unless value="$(arg add_gripper)">
+    <xacro:if value="$(arg add_vacuum_gripper)" >
+      <xacro:include filename="$(find xarm_description)/urdf/lite_vacuum_gripper.urdf.xacro" />
+      <xacro:uflite_vacuum_gripper_urdf attach_to="$(arg prefix)link_eef" />
+    </xacro:if>
+  </xacro:unless>
+
+  <xacro:if value="$(arg use_gazebo_camera)">
+    <!-- <xacro:include filename="$(find kinect_v2)/urdf/kinect_v2.urdf.xacro" />
+    <xacro:kinect_v2  parent="link_base">
+    </xacro:kinect_v2> -->
+    <xacro:include filename="$(find xarm_description)/urdf/camera.gazebo.xacro" />
+    <xacro:camera_gazebo prefix="$(arg prefix)" />
+  </xacro:if>
+</robot>

xarm_gazebo/launch/lite6_camera_scene.launch

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="paused" default="false" doc="Starts gazebo in paused mode" />
+  <arg name="gui" default="true" doc="Starts gazebo gui" />
+  <arg name="effort_control" default="false"/>
+  <arg name="velocity_control" default="false"/>
+  <arg name="add_gripper" default="false" />
+  <arg name="add_vacuum_gripper" default="false" />
+  <arg name="namespace" default="ufactory"/>
+  <arg name="model1300" default="false" />
+
+  <rosparam file="$(find lite6_moveit_config)/config/lite6_params.yaml" command="load" ns="$(arg namespace)"/>
+  <!-- <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/> -->
+  <!-- startup simulated world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_camera_scene.world"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+
+  <!-- send robot urdf to param server, joint limit may be overwritten if use moveit planner -->
+  <param name="robot_description" command="$(find xacro)/xacro
+    --inorder '$(find xarm_description)/urdf/lite6_robot_cam.urdf.xacro' ros_namespace:=$(arg namespace)
+    add_gripper:=$(arg add_gripper) add_vacuum_gripper:=$(arg add_vacuum_gripper)
+    effort_control:=$(arg effort_control) velocity_control:=$(arg velocity_control)
+    use_gazebo_camera:=true model1300:=$(arg model1300)" />
+
+  <!-- spawn robot model in gazebo, located on the table -->
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
+    respawn="false" output="screen"
+    args="-urdf -model lite6 -x 0 -y 0 -z 0.775 -Y 0 -param robot_description"/>
+    <!-- args="-urdf -model lite6 -x -0.2 -y -0.5 -z 1.021 -Y 1.571 -param robot_description"/> -->
+
+  <!-- load the corresponding controllers -->
+  <include file="$(find xarm_controller)/launch/lite6_control.launch">
+    <arg name="effort_control" value="$(arg effort_control)"/>
+    <arg name="velocity_control" value="$(arg velocity_control)"/>
+    <arg name="add_gripper" value="$(arg add_gripper)" />
+    <arg name="namespace" value="$(arg namespace)" />
+  </include>
+
+</launch>

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)