add match src

2022-05-09 10:42:47 +08:00 · 2022-05-09 10:42:47 +08:00 · a16a096a3c
parent cca937128f
commit a16a096a3c
16 changed files with 1760 additions and 142 deletions
--- a/onekey.sh
+++ b/onekey.sh
@ -9,7 +9,7 @@ export PATH
 #    SPARK技术讨论与反馈QQ群：6646169  8346256
 #=================================================

-GAME_ENABLE="no"
+GAME_ENABLE="yes"
 sh_ver="2.0"
 filepath=$(cd "$(dirname "$0")"; pwd)
 Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
@ -1091,6 +1091,9 @@ case "$num" in
 	12)
 	calibrate_camera
 	;;
+	20)
+	spark_carry_game
+	;;
 	100)
 	tell_us
 	;;
--- a/src/3rd_app/move2grasp/launch/move2grasp.launch
+++ b/src/3rd_app/move2grasp/launch/move2grasp.launch
@ -5,34 +5,26 @@
  	<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
  	<arg name="configuration_basename" default="spark_lds_2d.lua"/>
  	<arg name="open_rviz" default="true"/>
+	<arg name="lidar_type_tel" default="3iroboticslidar2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
+	<!-- 摄像机类型 -->
+	<arg name="camera_type_tel" default="astrapro" doc="camera type [astrapro, astra, d435...]"/>

+	<!--spark底盘驱动，机器人描述,底盘,相机-->
+	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
+		<arg name="camera_type_tel"	value="$(arg camera_type_tel)"/>
+		<arg name="lidar_type_tel"	value="$(arg lidar_type_tel)"/>
+	</include>

-	<include file="$(find spark_description)/launch/spark_description_norviz.launch"/>
- 	<!--include file="$(find spark_description)/launch/spark_description.launch"/-->
-
-
-   	<include file="$(find spark_base)/launch/spark_base.launch">
-		<arg name="serialport"                  value="/dev/sparkBase"/>
- 	</include> 
-
-  <!--spark相机-->
-  <!--include file="$(find spark_bringup)/launch/driver_bringup.launch"/-->
-  <include file="$(find astra_launch)/launch/astra.launch">
-	<arg name="rgb_processing"                  value="true"/> 
-	<arg name="depth_processing"                value="true"/>
-	<arg name="depth_registered_processing"     value="false"/>
-	<arg name="depth_registration"              value="false"/>
-	<arg name="disparity_processing"            value="false"/>
-	<arg name="disparity_registered_processing" value="false"/>
-  </include>
    
  <!--UARM机械臂-->
  <include file="$(find swiftpro)/launch/pro_control_nomoveit.launch"/>

 	<!-- 3D sensor -->
  	<arg name="3d_sensor" default="3i_lidar"/>  
-    		<include file="$(find iiiroboticslidar2)/launch/3iroboticslidar2.launch">
-  	</include>
+ 	<!-- 启动雷达 -->
+	<include file="$(find lidar_driver_transfer)/launch/$(arg lidar_type_tel).launch">
+	</include>
+

  	<!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration-->
  	<include file="$(find spark_slam)/launch/spark_$(arg slam_methods).launch">
@ -48,11 +40,18 @@

  	<!-- rviz -->
  	<group if="$(arg open_rviz)"> 
-    		<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find move2grasp)/rviz/spark_$(arg slam_methods).rviz"/>
+    		<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find move2grasp)/rviz/spark_$(arg slam_methods)_$(arg camera_type_tel).rviz"/>
  	</group>


        <node pkg="move2grasp" type="move.py" name="move" />
-        <node pkg="move2grasp" type="grasp.py" name="grasp" />
+	<group if="$(eval arg('camera_type_tel')=='d435')">
+		<node pkg="move2grasp" type="grasp_d435.py" name="grasp" output="screen">
+			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
+		</node>
+	</group>
+	<group if="$(eval arg('camera_type_tel')=='astrapro')">
+		<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
+	</group>

 </launch>
--- a/src/3rd_app/move2grasp/launch/teleop2grasp.launch
+++ b/src/3rd_app/move2grasp/launch/teleop2grasp.launch
@ -5,34 +5,28 @@
  	<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
  	<arg name="configuration_basename" default="spark_lds_2d.lua"/>
  	<arg name="open_rviz" default="true"/>
+	<arg name="lidar_type_tel" default="3iroboticslidar2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
+	<!-- 摄像机类型 -->
+	<arg name="camera_type_tel" default="astrapro" doc="camera type [astrapro, astra, d435...]"/>


-	<include file="$(find spark_description)/launch/spark_description_norviz.launch"/>
- 	<!--include file="$(find spark_description)/launch/spark_description.launch"/-->
+	<!--spark底盘驱动，机器人描述,底盘,相机-->
+	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
+
+		<arg name="camera_type_tel"	value="$(arg camera_type_tel)"/>
+		<arg name="lidar_type_tel"	value="$(arg lidar_type_tel)"/>
+	</include>


-   	<include file="$(find spark_base)/launch/spark_base.launch">
-		<arg name="serialport"                  value="/dev/sparkBase"/>
- 	</include> 
-
-  <!--spark相机-->
-  <!--include file="$(find spark_bringup)/launch/driver_bringup.launch"/-->
-  <include file="$(find astra_launch)/launch/astra.launch">
-	<arg name="rgb_processing"                  value="true"/> 
-	<arg name="depth_processing"                value="true"/>
-	<arg name="depth_registered_processing"     value="false"/>
-	<arg name="depth_registration"              value="false"/>
-	<arg name="disparity_processing"            value="false"/>
-	<arg name="disparity_registered_processing" value="false"/>
-  </include>
    
  <!--UARM机械臂-->
  <include file="$(find swiftpro)/launch/pro_control_nomoveit.launch"/>

 	<!-- 3D sensor -->
  	<arg name="3d_sensor" default="3i_lidar"/>  
-    		<include file="$(find iiiroboticslidar2)/launch/3iroboticslidar2.launch">
-  	</include>
+ 	<!-- 启动雷达 -->
+	<include file="$(find lidar_driver_transfer)/launch/$(arg lidar_type_tel).launch">
+	</include>

  	<!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration-->
  	<include file="$(find spark_slam)/launch/spark_$(arg slam_methods).launch">
@ -48,11 +42,18 @@

  	<!-- rviz -->
  	<group if="$(arg open_rviz)"> 
-    		<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find move2grasp)/rviz/spark_$(arg slam_methods).rviz"/>
+    		<node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find move2grasp)/rviz/spark_$(arg slam_methods)_$(arg camera_type_tel).rviz"/>
  	</group>


-        <node pkg="move2grasp" type="teleop.py" name="teleop" launch-prefix="xterm -e" />
-        <node pkg="move2grasp" type="grasp.py" name="grasp" />
+	<node pkg="move2grasp" type="teleop.py" name="teleop" launch-prefix="xterm -e" />
+	<group if="$(eval arg('camera_type_tel')=='d435')">
+		<node pkg="move2grasp" type="grasp_d435.py" name="grasp" output="screen">
+			<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
+		</node>
+	</group>
+	<group if="$(eval arg('camera_type_tel')=='astrapro')">
+		<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
+	</group>

 </launch>
--- a/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz
+++ b/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz
@ -7,7 +7,7 @@ Panels:
        - /TF1/Frames1
        - /PointStamped1
      Splitter Ratio: 0.594406009
-    Tree Height: 195
+    Tree Height: 112
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@ -27,6 +27,8 @@ Panels:
    Name: Time
    SyncMode: 0
    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
  Class: ""
  Displays:
@ -55,6 +57,8 @@ Visualization Manager:
        All Enabled: false
        Base:
          Value: true
+        IMU_link:
+          Value: true
        base_footprint:
          Value: false
        base_gyro_link:
@ -125,6 +129,8 @@ Visualization Manager:
              {}
            base_footprint:
              base_link:
+                IMU_link:
+                  {}
                base_gyro_link:
                  {}
                camera_link:
@ -235,6 +241,10 @@ Visualization Manager:
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
+        IMU_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
        Link Tree Style: Links in Alphabetic Order
        base_footprint:
          Alpha: 1
@ -443,12 +453,12 @@ Visualization Manager:
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 716
+  Height: 576
  Hide Left Dock: false
  Hide Right Dock: false
  Image:
    collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000242fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000152000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000180000000ea0000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f0000024200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b6fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000012d000000b10000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000001b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
--- a/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz
+++ b/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz
@ -0,0 +1,472 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /PointStamped1
+      Splitter Ratio: 0.594406009
+    Tree Height: 112
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: map
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        Base:
+          Value: true
+        IMU_link:
+          Value: true
+        base_footprint:
+          Value: false
+        base_gyro_link:
+          Value: false
+        base_link:
+          Value: true
+        camera_depth_frame:
+          Value: false
+        camera_depth_optical_frame:
+          Value: false
+        camera_link:
+          Value: true
+        camera_rgb_frame:
+          Value: false
+        camera_rgb_optical_frame:
+          Value: false
+        cliff_back_left_link:
+          Value: false
+        cliff_back_right_link:
+          Value: false
+        cliff_front_left_link:
+          Value: false
+        cliff_front_right_link:
+          Value: false
+        cliff_left_link:
+          Value: false
+        cliff_right_link:
+          Value: false
+        ir_bumper_back_left_link:
+          Value: false
+        ir_bumper_back_right_link:
+          Value: false
+        ir_bumper_front_left_link:
+          Value: false
+        ir_bumper_front_link:
+          Value: false
+        ir_bumper_front_right_link:
+          Value: false
+        ir_bumper_left_link:
+          Value: false
+        ir_bumper_rear_link:
+          Value: false
+        ir_bumper_right_link:
+          Value: false
+        left_wheel_link:
+          Value: false
+        lidar_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        right_wheel_link:
+          Value: false
+        room_base_link:
+          Value: false
+        spark_stack:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            Base:
+              {}
+            base_footprint:
+              base_link:
+                IMU_link:
+                  {}
+                base_gyro_link:
+                  {}
+                camera_link:
+                  camera_depth_frame:
+                    camera_depth_optical_frame:
+                      {}
+                  camera_rgb_frame:
+                    camera_rgb_optical_frame:
+                      {}
+                cliff_back_left_link:
+                  {}
+                cliff_back_right_link:
+                  {}
+                cliff_front_left_link:
+                  {}
+                cliff_front_right_link:
+                  {}
+                cliff_left_link:
+                  {}
+                cliff_right_link:
+                  {}
+                ir_bumper_back_left_link:
+                  {}
+                ir_bumper_back_right_link:
+                  {}
+                ir_bumper_front_left_link:
+                  {}
+                ir_bumper_front_link:
+                  {}
+                ir_bumper_front_right_link:
+                  {}
+                ir_bumper_left_link:
+                  {}
+                ir_bumper_rear_link:
+                  {}
+                ir_bumper_right_link:
+                  {}
+                left_wheel_link:
+                  {}
+                lidar_link:
+                  {}
+                right_wheel_link:
+                  {}
+                room_base_link:
+                  {}
+                spark_stack:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 11799
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0299999993
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        IMU_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_rgb_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rgb_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_back_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_back_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_front_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_front_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_back_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_back_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_rear_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        room_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        spark_stack:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.33737922
+        Min Value: 0.179654211
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /camera/depth/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Class: rviz/PointStamped
+      Color: 204; 41; 204
+      Enabled: true
+      History Length: 1
+      Name: PointStamped
+      Radius: 0.200000003
+      Topic: /clicked_point
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/ThirdPersonFollower
+      Distance: 11.2426996
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 9.53673975e-07
+        Y: 4.76836988e-07
+        Z: -4.76836988e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.480399042
+      Target Frame: map
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 1.88540006
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 576
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b6fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000012d000000b10000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000001b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 959
+  X: 65
+  Y: 24
--- a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz
+++ b/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz
@ -0,0 +1,524 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /PointStamped1
+      Splitter Ratio: 0.594406009
+    Tree Height: 112
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: map
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        Base:
+          Value: true
+        IMU_link:
+          Value: true
+        base_footprint:
+          Value: false
+        base_gyro_link:
+          Value: false
+        base_link:
+          Value: true
+        camera_aligned_depth_to_color_frame:
+          Value: true
+        camera_aligned_depth_to_infra1_frame:
+          Value: true
+        camera_bottom_screw_frame:
+          Value: true
+        camera_color_frame:
+          Value: true
+        camera_color_optical_frame:
+          Value: true
+        camera_depth_frame:
+          Value: false
+        camera_depth_optical_frame:
+          Value: false
+        camera_infra1_frame:
+          Value: true
+        camera_infra1_optical_frame:
+          Value: true
+        camera_left_ir_frame:
+          Value: true
+        camera_left_ir_optical_frame:
+          Value: true
+        camera_link:
+          Value: true
+        camera_right_ir_frame:
+          Value: true
+        camera_right_ir_optical_frame:
+          Value: true
+        cliff_back_left_link:
+          Value: false
+        cliff_back_right_link:
+          Value: false
+        cliff_front_left_link:
+          Value: false
+        cliff_front_right_link:
+          Value: false
+        cliff_left_link:
+          Value: false
+        cliff_right_link:
+          Value: false
+        ir_bumper_back_left_link:
+          Value: false
+        ir_bumper_back_right_link:
+          Value: false
+        ir_bumper_front_left_link:
+          Value: false
+        ir_bumper_front_link:
+          Value: false
+        ir_bumper_front_right_link:
+          Value: false
+        ir_bumper_left_link:
+          Value: false
+        ir_bumper_rear_link:
+          Value: false
+        ir_bumper_right_link:
+          Value: false
+        left_wheel_link:
+          Value: false
+        lidar_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        right_wheel_link:
+          Value: false
+        room_base_link:
+          Value: false
+        spark_stack:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            Base:
+              {}
+            base_footprint:
+              base_link:
+                IMU_link:
+                  {}
+                base_gyro_link:
+                  {}
+                camera_bottom_screw_frame:
+                  camera_link:
+                    camera_aligned_depth_to_color_frame:
+                      camera_color_optical_frame:
+                        {}
+                    camera_aligned_depth_to_infra1_frame:
+                      camera_infra1_optical_frame:
+                        {}
+                    camera_color_frame:
+                      {}
+                    camera_depth_frame:
+                      camera_depth_optical_frame:
+                        {}
+                      camera_left_ir_frame:
+                        camera_left_ir_optical_frame:
+                          {}
+                      camera_right_ir_frame:
+                        camera_right_ir_optical_frame:
+                          {}
+                    camera_infra1_frame:
+                      {}
+                cliff_back_left_link:
+                  {}
+                cliff_back_right_link:
+                  {}
+                cliff_front_left_link:
+                  {}
+                cliff_front_right_link:
+                  {}
+                cliff_left_link:
+                  {}
+                cliff_right_link:
+                  {}
+                ir_bumper_back_left_link:
+                  {}
+                ir_bumper_back_right_link:
+                  {}
+                ir_bumper_front_left_link:
+                  {}
+                ir_bumper_front_link:
+                  {}
+                ir_bumper_front_right_link:
+                  {}
+                ir_bumper_left_link:
+                  {}
+                ir_bumper_rear_link:
+                  {}
+                ir_bumper_right_link:
+                  {}
+                left_wheel_link:
+                  {}
+                lidar_link:
+                  {}
+                right_wheel_link:
+                  {}
+                room_base_link:
+                  {}
+                spark_stack:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 11799
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.0299999993
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/color/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        IMU_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_left_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_left_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_right_ir_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_right_ir_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_back_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_back_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_front_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_front_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_back_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_back_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_front_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_rear_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ir_bumper_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        room_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        spark_stack:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.33737922
+        Min Value: 0.179654211
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /camera/depth/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Class: rviz/PointStamped
+      Color: 204; 41; 204
+      Enabled: true
+      History Length: 1
+      Name: PointStamped
+      Radius: 0.200000003
+      Topic: /clicked_point
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/ThirdPersonFollower
+      Distance: 11.2426996
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 9.53673975e-07
+        Y: 4.76836988e-07
+        Z: -4.76836988e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.480399042
+      Target Frame: map
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 1.88540006
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 576
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b6fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000012d000000b10000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000001b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 959
+  X: 65
+  Y: 24
--- a/src/3rd_app/move2grasp/scripts/.idea/.gitignore
+++ b/src/3rd_app/move2grasp/scripts/.idea/.gitignore
@ -0,0 +1,2 @@
+# Default ignored files
+/workspace.xml
--- a/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml
+++ b/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml
@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>
--- a/src/3rd_app/move2grasp/scripts/.idea/modules.xml
+++ b/src/3rd_app/move2grasp/scripts/.idea/modules.xml
@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/scripts.iml" filepath="$PROJECT_DIR$/.idea/scripts.iml" />
+    </modules>
+  </component>
+</project>
--- a/src/3rd_app/move2grasp/scripts/.idea/scripts.iml
+++ b/src/3rd_app/move2grasp/scripts/.idea/scripts.iml
@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
--- a/src/3rd_app/move2grasp/scripts/.idea/vcs.xml
+++ b/src/3rd_app/move2grasp/scripts/.idea/vcs.xml
@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="W:/" vcs="Git" />
+  </component>
+</project>
--- a/src/3rd_app/move2grasp/scripts/grasp.py
+++ b/src/3rd_app/move2grasp/scripts/grasp.py
@ -10,7 +10,6 @@ import rospy
 import smach
 import smach_ros
 import threading
-import thread
 import string
 import math
 import cv2
@ -22,7 +21,6 @@ from sensor_msgs.msg import Image
 from cv_bridge import CvBridge, CvBridgeError
 from spark_carry_object.msg import *

-
 class GraspObject():
    '''
    监听主控，用于物品抓取功能
@ -33,14 +31,18 @@ class GraspObject():
        初始化
        '''

-        global xc, yc, xc_prev, yc_prev, found_count
+        global xc, yc, xc_prev, yc_prev, found_count, detect_color_blue
        xc = 0
        yc = 0
        xc_prev = xc
        yc_prev = yc
        found_count = 0
+        detect_color_blue=False
        self.is_found_object = False
        # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+        thread1 = threading.Thread(target=self.image_cb,)
+        thread1.setDaemon(True)
+        thread1.start()
        # 订阅机械臂抓取指令
        self.sub2 = rospy.Subscriber(
            '/grasp', String, self.grasp_cp, queue_size=1)
@ -54,16 +56,21 @@ class GraspObject():
            'grasp_status', String, queue_size=1)
        # 发布TWist消息控制机器人底盘
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-	pos = position()
-	pos.x = 120
-	pos.y = 0
-	pos.z = 35
-	self.pub1.publish(pos)
+        r1 = rospy.Rate(1)
+        r1.sleep()
+        pos = position()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)

    def grasp_cp(self, msg):
+        global detect_color_blue
+        rospy.loginfo("recvice grasp command:%s",msg.data)
        if msg.data == '1':
            # 订阅摄像头话题,对图像信息进行处理
-            self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+            #self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+            detect_color_blue=True
            self.is_found_object = False
            rate = rospy.Rate(10)
            times=0
@ -73,8 +80,9 @@ class GraspObject():
                times+=1
                # 转一圈没有发现可抓取物体,退出抓取
                if steps>=5:
-                    self.sub.unregister()
+                    #self.sub.unregister()
                    print("stop grasp\n")
+                    detect_color_blue=False
                    status=String()
                    status.data='-1'
                    self.grasp_status_pub.publish(status)
@ -86,9 +94,8 @@ class GraspObject():
                    self.turn_body()
                    print("not found\n")
            print("unregisting sub\n")
-            self.sub.unregister()
-            print("unregisted sub\n")
            # 抓取检测到的物体 
+            detect_color_blue=False   
            self.grasp()
            status=String()
            status.data='1'
@ -96,14 +103,16 @@ class GraspObject():
        if msg.data=='0':
            # 放下物体
            self.is_found_object = False
+            detect_color_blue=False
            self.release_object()
            status=String()
            status.data='0'
+            
            self.grasp_status_pub.publish(status)

    # 执行抓取
    def grasp(self):
-        print("start to grasp\n")
+        rospy.loginfo("start to grasp\n")
        global xc, yc, found_count
        # stop function

@ -131,7 +140,7 @@ class GraspObject():
        pos.x = a[0] * yc + a[1]
        pos.y = b[0] * xc + b[1]
        pos.z = 20
-	    # pos.z = 20
+         # pos.z = 20
        print("z = 20\n")
        self.pub1.publish(pos)
        r2.sleep()
@ -146,96 +155,111 @@ class GraspObject():
        r2.sleep()

        # 提起物体
-        pos.x = 250  #160
+        pos.x = 200  #160
        pos.y = 0
-        pos.z = 150  #55
+        pos.z = 155  #55
        self.pub1.publish(pos)
        r1.sleep()
-    # 使用CV检测物体       
-    def image_cb(self, data):
-        global xc, yc, xc_prev, yc_prev, found_count
-        # change to opencv
+
+    def hsv_value(self):
        try:
-            cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
-        except CvBridgeError as e:
-            print('error')
+            filename = os.environ['HOME'] + "/color_block_HSV.txt"
+            with open(filename, "r") as f:
+                for line in f:
+                    split = line.split(':')[1].split(' ')
+                    lower = split[0].split(',')
+                    upper = split[1].split(',')
+                    for i in range(3):
+                        lower[i] = int(lower[i])
+                        upper[i] = int(upper[i])

-        # change rgb to hsv
-        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
+            lower = np.array(lower)
+            upper = np.array(upper)
+        except:
+            raise IOError('could not find hsv_value file : {},please execute #13 command automatically '
+                          'generate this file'.format(filename))

-        # 蓝色物体颜色检测范围
-        LowerBlue = np.array([100, 90, 80])
-        UpperBlue = np.array([130, 255, 255])
-        mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
-        cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
+        return lower, upper

-        # gray process
-        cv_image4 = cv_image3[:, :, 0]
+    # 使用CV检测物体       
+    def image_cb(self):
+        global xc, yc, xc_prev, yc_prev, found_count, detect_color_blue	
+        capture = cv2.VideoCapture(0)
+        LowerBlue, UpperBlue = self.hsv_value()

-        # smooth and clean noise
-        blurred = cv2.blur(cv_image4, (9, 9))
-        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
-        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
-        cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
-        cv_image5 = cv2.erode(cv_image5, None, iterations=4)
-        cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
+        while True:
+            # time.sleep(0.08)
+            ret,frame = capture.read()
+            cv_image2 = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+            mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)		
+            mask = cv2.erode(mask, None, iterations=2)
+            mask = cv2.dilate(mask, None, iterations=2)
+            mask = cv2.GaussianBlur(mask, (9,9), 0)
+            # detect contour
+            cv2.imshow("win2", mask)
+            cv2.waitKey(1)
+            if detect_color_blue :
+                _, contours, hier = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+                if len(contours) > 0:
+                    size = []
+                    size_max = 0
+                    distance_list = []
+                    max_distance = 450
+                    for i, c in enumerate(contours):
+                        rect = cv2.minAreaRect(c)
+                        box = cv2.boxPoints(rect)
+                        box = np.int0(box)
+                        x_mid, y_mid = rect[0]

-        # detect contour
-        # cv2.imshow("win1", cv_image1)
-        # cv2.imshow("win2", cv_image5)
-        # cv2.waitKey(1)
-        _, contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+                        w = math.sqrt((box[0][0] - box[1][0])**2 + (box[0][1] - box[1][1])**2)
+                        h = math.sqrt((box[0][0] - box[3][0])**2 + (box[0][1] - box[3][1])**2)
+
+                        size.append(w * h)
+                        # 所有点到spark的距离
+                        distance_list.append(math.sqrt((320 - x_mid) ** 2 + (300 - y_mid) ** 2))
+
+                        if size[i] > size_max and distance_list[i] < max_distance:
+                            size_max = size[i]
+                            min_distance = distance_list[i]
+                            index = i
+                            xc = x_mid
+                            yc = y_mid
+                            cv2.circle(frame, (np.int32(xc), np.int32(yc)), 2, (255, 0, 0), 2, 8, 0)
+                    if found_count >= 15 and min_distance < 350:
+                        self.is_found_object = True
+                        cmd_vel = Twist()
+                        self.cmd_vel_pub.publish(cmd_vel)
+                    else:
+                        # if box is not moving
+                        if abs(xc - xc_prev) <= 50 and abs(yc - yc_prev) <= 50 and yc > 150 and yc < 370 and xc > 100 and xc < 540:
+                            found_count = found_count + 1
+                        else:
+                            found_count = 0

-        # if find contours, pick the biggest box
-        if len(contours) > 0:
-            size = []
-            size_max = 0
-            for i, c in enumerate(contours):
-                rect = cv2.minAreaRect(c)
-                box = cv2.boxPoints(rect)
-                box = np.int0(box)
-                x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
-                y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
-                w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
-                h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2) 
-                size.append(w * h)
-                if size[i] > size_max:
-                    size_max = size[i]
-                    index = i
-                    xc = x_mid
-                    yc = y_mid
-            # if box is not moving for 20 times
-            # print found_count
-            if found_count >= 30:
-                self.is_found_object = True
-		cmd_vel = Twist()
-		self.cmd_vel_pub.publish(cmd_vel)
-            else:
-                # if box is not moving
-                if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
-                    found_count = found_count + 1
                else:
                    found_count = 0
-        else:
-            found_count = 0
-        xc_prev = xc
-        yc_prev = yc
+            xc_prev = xc
+            yc_prev = yc
+            cv2.imshow("win1", frame)
+            cv2.waitKey(1)
+            #key = cv2.waitKey(10)
+ 
    # 释放物体
    def release_object(self):
-        r1 = rospy.Rate(0.15)  # 5s
-        r2 = rospy.Rate(1)     # 1s
+        r1 = rospy.Rate(1)
+        r2 = rospy.Rate(1)
        pos = position()
        # go forward
        pos.x = 200
        pos.y = 0
-        pos.z = -40  #-80
+        pos.z = -40
        self.pub1.publish(pos)
        r1.sleep()

        # stop pump
        self.pub2.publish(0)
        r2.sleep()
-        #r1.sleep()
+        r1.sleep()
        pos.x = 120
        pos.y = 0
        pos.z = 35
--- a/src/3rd_app/move2grasp/scripts/grasp_d435.py
+++ b/src/3rd_app/move2grasp/scripts/grasp_d435.py
@ -0,0 +1,263 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import os
+import time
+import random
+import ctypes
+import roslib
+import rospy
+import smach
+import smach_ros
+import threading
+import string
+import math
+import cv2
+import numpy as np
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from geometry_msgs.msg import Pose, Point, Quaternion
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from spark_carry_object.msg import *
+
+
+class GraspObject():
+    '''
+    监听主控，用于物品抓取功能
+    '''
+
+    def __init__(self):
+        '''
+        初始化
+        '''
+
+        global xc, yc, xc_prev, yc_prev, found_count
+        xc = 0
+        yc = 0
+        xc_prev = xc
+        yc_prev = yc
+        found_count = 0
+        self.is_found_object = False
+        # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+        # 订阅机械臂抓取指令
+        self.sub2 = rospy.Subscriber(
+            '/grasp', String, self.grasp_cp, queue_size=1)
+        # 发布机械臂位姿
+        self.pub1 = rospy.Publisher(
+            'position_write_topic', position, queue_size=10)
+        # 发布机械臂吸盘
+        self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
+        # 发布机械臂状态
+        self.grasp_status_pub = rospy.Publisher(
+            'grasp_status', String, queue_size=1)
+        # 发布TWist消息控制机器人底盘
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        pos = position()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)
+
+    def grasp_cp(self, msg):
+        if msg.data == '1':
+            # 订阅摄像头话题,对图像信息进行处理
+            self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+            self.is_found_object = False
+            rate = rospy.Rate(10)
+            times=0
+            steps=0
+            while not self.is_found_object:
+                rate.sleep()
+                times+=1
+                # 转一圈没有发现可抓取物体,退出抓取
+                if steps>=5:
+                    self.sub.unregister()
+                    print("stop grasp\n")
+                    status=String()
+                    status.data='-1'
+                    self.grasp_status_pub.publish(status)
+                    return
+                # 旋转一定角度扫描是否有可供抓取的物体
+                if times>=30:
+                    times=0
+                    steps+=1
+                    self.turn_body()
+                    print("not found\n")
+            print("unregisting sub\n")
+            self.sub.unregister()
+            print("unregisted sub\n")
+            # 抓取检测到的物体    
+            self.grasp()
+            status=String()
+            status.data='1'
+            self.grasp_status_pub.publish(status) 
+        if msg.data=='0':
+            # 放下物体
+            self.is_found_object = False
+            self.release_object()
+            status=String()
+            status.data='0'
+            self.grasp_status_pub.publish(status) 
+
+    # 执行抓取
+    def grasp(self):
+        print("start to grasp\n")
+        global xc, yc, found_count
+        # stop function
+
+        filename = os.environ['HOME'] + "/thefile.txt"
+        file_pix = open(filename, 'r')
+        s = file_pix.read()
+        file_pix.close()
+        print(s)
+        arr=s.split()
+        a1=arr[0]
+        a2=arr[1]
+        a3=arr[2]
+        a4=arr[3]
+        a = [0]*2
+        b = [0]*2
+        a[0]=float(a1)
+        a[1]=float(a2)
+        b[0]=float(a3)
+        b[1]=float(a4)
+        print('k and b value:',a[0],a[1],b[0],b[1])
+        r1 = rospy.Rate(0.095)
+        r2 = rospy.Rate(10)
+        pos = position()
+        # 物体所在坐标+标定误差
+        pos.x = a[0] * yc + a[1]
+        pos.y = b[0] * xc + b[1]
+        pos.z = 20
+	    # pos.z = 20
+        print("z = 20\n")
+        self.pub1.publish(pos)
+        r2.sleep()
+        # go down -100
+        pos.z = -50
+        self.pub1.publish(pos)
+        print("z = -83\n")
+        r2.sleep()
+
+        # 开始吸取物体
+        self.pub2.publish(1)
+        r2.sleep()
+
+        # 提起物体
+        pos.x = 250  #160
+        pos.y = 0
+        pos.z = 150  #55
+        self.pub1.publish(pos)
+        r1.sleep()
+    # 使用CV检测物体       
+    def image_cb(self, data):
+        global xc, yc, xc_prev, yc_prev, found_count
+        # change to opencv
+        try:
+            cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
+        except CvBridgeError as e:
+            print('error')
+
+        # change rgb to hsv
+        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
+
+        # 蓝色物体颜色检测范围
+        LowerBlue = np.array([95, 90, 80])
+        UpperBlue = np.array([130, 255, 255])
+        mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
+        cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
+
+        # gray process
+        cv_image4 = cv_image3[:, :, 0]
+
+        # smooth and clean noise
+        blurred = cv2.blur(cv_image4, (9, 9))
+        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
+        cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
+        cv_image5 = cv2.erode(cv_image5, None, iterations=4)
+        cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
+
+        # detect contour
+        # cv2.imshow("win1", cv_image1)
+        # cv2.imshow("win2", cv_image5)
+        # cv2.waitKey(1)
+        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+
+        # if find contours, pick the biggest box
+        if len(contours) > 0:
+            size = []
+            size_max = 0
+            for i, c in enumerate(contours):
+                rect = cv2.minAreaRect(c)
+                box = cv2.boxPoints(rect)
+                box = np.int0(box)
+                x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
+                y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
+                w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
+                h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2) 
+                size.append(w * h)
+                if size[i] > size_max:
+                    size_max = size[i]
+                    index = i
+                    xc = x_mid
+                    yc = y_mid
+            # if box is not moving for 20 times
+            # print found_count
+            if found_count >= 30:
+                self.is_found_object = True
+                cmd_vel = Twist()
+                self.cmd_vel_pub.publish(cmd_vel)
+            else:
+                # if box is not moving
+                if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
+                    found_count = found_count + 1
+                else:
+                    found_count = 0
+        else:
+            found_count = 0
+        xc_prev = xc
+        yc_prev = yc
+    # 释放物体
+    def release_object(self):
+        r1 = rospy.Rate(0.15)  # 5s
+        r2 = rospy.Rate(1)     # 1s
+        pos = position()
+        # go forward
+        pos.x = 200
+        pos.y = 0
+        pos.z = -40  #-80
+        self.pub1.publish(pos)
+        r1.sleep()
+
+        # stop pump
+        self.pub2.publish(0)
+        r2.sleep()
+        #r1.sleep()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)
+        r1.sleep()
+        return 'succeeded'
+    # 转动机器人到一定角度       
+    def turn_body(self):
+        cmd_vel = Twist()
+        cmd_vel.angular.z = 0.25
+        rate = rospy.Rate(10)
+        for i in range(40):            
+            self.cmd_vel_pub.publish(cmd_vel)            
+            rate.sleep()
+       
+
+        
+if __name__ == '__main__':
+    try:
+        rospy.init_node('GraspObject', anonymous=False)
+        rospy.loginfo("Init GraspObject main")   
+        GraspObject()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("End spark GraspObject main")
+
--- a/src/3rd_app/move2grasp/scripts/grasp_pro.py
+++ b/src/3rd_app/move2grasp/scripts/grasp_pro.py
@ -0,0 +1,288 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import os
+import time
+import random
+import ctypes
+import roslib
+import rospy
+import smach
+import smach_ros
+import threading
+import string
+import math
+import cv2
+import numpy as np
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from geometry_msgs.msg import Pose, Point, Quaternion
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from spark_carry_object.msg import *
+import threading
+class GraspObject():
+    '''
+    监听主控，用于物品抓取功能
+    '''
+
+    def __init__(self):
+        '''
+        初始化
+        '''
+
+        global xc, yc, xc_prev, yc_prev, found_count, detect_color_blue
+        xc = 0
+        yc = 0
+        xc_prev = xc
+        yc_prev = yc
+        found_count = 0
+        detect_color_blue=False
+        self.is_found_object = False
+        # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+        thread1 = threading.Thread(target=self.image_cb,)
+        thread1.setDaemon(True)
+        thread1.start()
+        # 订阅机械臂抓取指令
+        self.sub2 = rospy.Subscriber(
+            '/grasp', String, self.grasp_cp, queue_size=1)
+        # 发布机械臂位姿
+        self.pub1 = rospy.Publisher(
+            'position_write_topic', position, queue_size=10)
+        # 发布机械臂吸盘
+        self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
+        # 发布机械臂状态
+        self.grasp_status_pub = rospy.Publisher(
+            'grasp_status', String, queue_size=1)
+        # 发布TWist消息控制机器人底盘
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        r1 = rospy.Rate(1)
+        r1.sleep()
+        pos = position()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)
+
+    def grasp_cp(self, msg):
+        global detect_color_blue
+        rospy.loginfo("recvice grasp command:%s",msg.data)
+        if msg.data == '1':
+            # 订阅摄像头话题,对图像信息进行处理
+            #self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+            detect_color_blue=True
+            self.is_found_object = False
+            rate = rospy.Rate(10)
+            times=0
+            steps=0
+            while not self.is_found_object:
+                rate.sleep()
+                times+=1
+                # 转一圈没有发现可抓取物体,退出抓取
+                if steps>=5:
+                    #self.sub.unregister()
+                    print("stop grasp\n")
+                    detect_color_blue=False
+                    status=String()
+                    status.data='-1'
+                    self.grasp_status_pub.publish(status)
+                    return
+                # 旋转一定角度扫描是否有可供抓取的物体
+                if times>=30:
+                    times=0
+                    steps+=1
+                    self.turn_body()
+                    print("not found\n")
+            print("unregisting sub\n")
+            # 抓取检测到的物体 
+            detect_color_blue=False   
+            self.grasp()
+            status=String()
+            status.data='1'
+            self.grasp_status_pub.publish(status) 
+        if msg.data=='0':
+            # 放下物体
+            self.is_found_object = False
+            detect_color_blue=False
+            self.release_object()
+            status=String()
+            status.data='0'
+            
+            self.grasp_status_pub.publish(status)
+
+    # 执行抓取
+    def grasp(self):
+        rospy.loginfo("start to grasp\n")
+        global xc, yc, found_count
+        # stop function
+
+        filename = os.environ['HOME'] + "/thefile.txt"
+        file_pix = open(filename, 'r')
+        s = file_pix.read()
+        file_pix.close()
+        print(s)
+        arr=s.split()
+        a1=arr[0]
+        a2=arr[1]
+        a3=arr[2]
+        a4=arr[3]
+        a = [0]*2
+        b = [0]*2
+        a[0]=float(a1)
+        a[1]=float(a2)
+        b[0]=float(a3)
+        b[1]=float(a4)
+        print('k and b value:',a[0],a[1],b[0],b[1])
+        r1 = rospy.Rate(0.095)
+        r2 = rospy.Rate(10)
+        pos = position()
+        # 物体所在坐标+标定误差
+        pos.x = a[0] * yc + a[1]
+        pos.y = b[0] * xc + b[1]
+        pos.z = 20
+         # pos.z = 20
+        print("z = 20\n")
+        self.pub1.publish(pos)
+        r2.sleep()
+        # go down -100
+        pos.z = -50
+        self.pub1.publish(pos)
+        print("z = -83\n")
+        r2.sleep()
+
+        # 开始吸取物体
+        self.pub2.publish(1)
+        r2.sleep()
+
+        # 提起物体
+        pos.x = 200  #160
+        pos.y = 0
+        pos.z = 155  #55
+        self.pub1.publish(pos)
+        r1.sleep()
+
+    def hsv_value(self):
+        try:
+            filename = os.environ['HOME'] + "/color_block_HSV.txt"
+            with open(filename, "r") as f:
+                for line in f:
+                    split = line.split(':')[1].split(' ')
+                    lower = split[0].split(',')
+                    upper = split[1].split(',')
+                    for i in range(3):
+                        lower[i] = int(lower[i])
+                        upper[i] = int(upper[i])
+
+            lower = np.array(lower)
+            upper = np.array(upper)
+        except:
+            raise IOError('could not find hsv_value file : {},please execute #13 command automatically '
+                          'generate this file'.format(filename))
+
+        return lower, upper
+
+    # 使用CV检测物体       
+    def image_cb(self):
+        global xc, yc, xc_prev, yc_prev, found_count, detect_color_blue	
+        capture = cv2.VideoCapture(0)
+        LowerBlue, UpperBlue = self.hsv_value()
+
+        while True:
+            # time.sleep(0.08)
+            ret,frame = capture.read()
+            cv_image2 = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+            mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)		
+            mask = cv2.erode(mask, None, iterations=2)
+            mask = cv2.dilate(mask, None, iterations=2)
+            mask = cv2.GaussianBlur(mask, (9,9), 0)
+            # detect contour
+            cv2.imshow("win2", mask)
+            cv2.waitKey(1)
+            if detect_color_blue :
+                contours, hier = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+                if len(contours) > 0:
+                    size = []
+                    size_max = 0
+                    distance_list = []
+                    max_distance = 450
+                    for i, c in enumerate(contours):
+                        rect = cv2.minAreaRect(c)
+                        box = cv2.boxPoints(rect)
+                        box = np.int0(box)
+                        x_mid, y_mid = rect[0]
+
+                        w = math.sqrt((box[0][0] - box[1][0])**2 + (box[0][1] - box[1][1])**2)
+                        h = math.sqrt((box[0][0] - box[3][0])**2 + (box[0][1] - box[3][1])**2)
+
+                        size.append(w * h)
+                        # 所有点到spark的距离
+                        distance_list.append(math.sqrt((320 - x_mid) ** 2 + (300 - y_mid) ** 2))
+
+                        if size[i] > size_max and distance_list[i] < max_distance:
+                            size_max = size[i]
+                            min_distance = distance_list[i]
+                            index = i
+                            xc = x_mid
+                            yc = y_mid
+                            cv2.circle(frame, (np.int32(xc), np.int32(yc)), 2, (255, 0, 0), 2, 8, 0)
+                    if found_count >= 15 and min_distance < 350:
+                        self.is_found_object = True
+                        cmd_vel = Twist()
+                        self.cmd_vel_pub.publish(cmd_vel)
+                    else:
+                        # if box is not moving
+                        if abs(xc - xc_prev) <= 50 and abs(yc - yc_prev) <= 50 and yc > 150 and yc < 370 and xc > 100 and xc < 540:
+                            found_count = found_count + 1
+                        else:
+                            found_count = 0
+
+                else:
+                    found_count = 0
+            xc_prev = xc
+            yc_prev = yc
+            cv2.imshow("win1", frame)
+            cv2.waitKey(1)
+            #key = cv2.waitKey(10)
+ 
+    # 释放物体
+    def release_object(self):
+        r1 = rospy.Rate(1)
+        r2 = rospy.Rate(1)
+        pos = position()
+        # go forward
+        pos.x = 200
+        pos.y = 0
+        pos.z = -40
+        self.pub1.publish(pos)
+        r1.sleep()
+
+        # stop pump
+        self.pub2.publish(0)
+        r2.sleep()
+        r1.sleep()
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)
+        r1.sleep()
+        return 'succeeded'
+    # 转动机器人到一定角度       
+    def turn_body(self):
+        cmd_vel = Twist()
+        cmd_vel.angular.z = 0.25
+        rate = rospy.Rate(10)
+        for i in range(40):            
+            self.cmd_vel_pub.publish(cmd_vel)            
+            rate.sleep()
+       
+
+        
+if __name__ == '__main__':
+    try:
+        rospy.init_node('GraspObject', anonymous=False)
+        rospy.loginfo("Init GraspObject main")   
+        GraspObject()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("End spark GraspObject main")
+
--- a/src/3rd_app/move2grasp/scripts/move.py
+++ b/src/3rd_app/move2grasp/scripts/move.py
@ -17,7 +17,7 @@ class Move2Grasp():
 
        rospy.on_shutdown(self.shutdown)
        #订阅RVIZ上的点击事件
-	rospy.Subscriber('clicked_point', PointStamped, self.cp_callback)
+        rospy.Subscriber('clicked_point', PointStamped, self.cp_callback)
        #订阅机械臂抓取状态
        rospy.Subscriber('/grasp_status', String, self.grasp_status_cp, queue_size=1)
        # Publisher to manually control the robot (e.g. to stop it)  
@ -66,6 +66,7 @@ class Move2Grasp():
            status=self.move(goal)
            # 如果到达指定地点,就发送抓取指令
            if status==True:
+                print('goal reached and start grasp')
                msg=String()
                msg.data='1'
                self.grasp_pub.publish(msg)
@ -76,10 +77,10 @@ class Move2Grasp():
                goal = MoveBaseGoal()
                goal.target_pose.header.frame_id = 'map' 
                goal.target_pose.header.stamp = rospy.Time.now() 
-                pose = Pose(Point(0.0,0.0,0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
+                pose = Pose(Point(0.3,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
                goal.target_pose.pose=pose  
                status=self.move(goal)
-                status=self.move(goal)
+                #status=self.move(goal)
                # 到达起始点,放下物体
                if status==True:
                    msg=String()
--- a/src/3rd_app/move2grasp/scripts/teleop.py
+++ b/src/3rd_app/move2grasp/scripts/teleop.py
@ -17,8 +17,6 @@ grasp_pub = rospy.Publisher('/grasp', String, queue_size=1)
 global can_grasp
 global can_release

-
-
 def grasp_status_cp(msg):
    global can_release,can_grasp
    # 物体抓取成功,让机器人回起始点
@ -29,29 +27,34 @@ def grasp_status_cp(msg):
 grasp_status=rospy.Subscriber('/grasp_status', String, grasp_status_cp, queue_size=1)

 def keyboardLoop():
-    #初始化
    rospy.init_node('teleop')
+    #初始化监听键盘按钮时间间隔
    rate = rospy.Rate(rospy.get_param('~hz', 10))
 
    #速度变量
+    # 慢速
    walk_vel_ = rospy.get_param('walk_vel', 0.3)
+    # 快速
    run_vel_ = rospy.get_param('run_vel', 1.0)
    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
    yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
- 
+    # walk_vel_前后速度
    max_tv = walk_vel_
+    # yaw_rate_旋转速度
    max_rv = yaw_rate_
+    # 参数初始化
    speed=0
    global can_release,can_grasp
    can_grasp=True
    can_release=False
    
-    print "使用[WASD]控制机器人"
-    print "按[G]抓取物体，按[H]放下物体"
-    print "按[Q]退出"
+    print ("使用[WASD]控制机器人")
+    print ("按[G]抓取物体，按[H]放下物体")
+    print ("按[Q]退出" )
 
    #读取按键循环
    while not rospy.is_shutdown():
+        # linux下读取键盘按键
        fd = sys.stdin.fileno()
        turn =0
        old_settings = termios.tcgetattr(fd)
@ -62,7 +65,7 @@ def keyboardLoop():
            ch = sys.stdin.read( 1 )
        finally :
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
- 
+        # ch代表获取的键盘按键
        if ch == 'g':
            if can_grasp:
                msg=String()