diff --git a/onekey.sh b/onekey.sh index 101aa04..66843ee 100755 --- 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 ;; diff --git a/src/3rd_app/move2grasp/launch/move2grasp.launch b/src/3rd_app/move2grasp/launch/move2grasp.launch index 138a0e2..e7d4207 100755 --- a/src/3rd_app/move2grasp/launch/move2grasp.launch +++ b/src/3rd_app/move2grasp/launch/move2grasp.launch @@ -5,34 +5,26 @@ + + + + + + + + - - - - - - - - - - - - - - - - - - - - + + + + @@ -48,11 +40,18 @@ - + - + + + + + + + + diff --git a/src/3rd_app/move2grasp/launch/teleop2grasp.launch b/src/3rd_app/move2grasp/launch/teleop2grasp.launch index 2e09c8b..10234bf 100755 --- a/src/3rd_app/move2grasp/launch/teleop2grasp.launch +++ b/src/3rd_app/move2grasp/launch/teleop2grasp.launch @@ -5,34 +5,28 @@ + + + - - + + + + + + - - - - - - - - - - - - - - - - + + + @@ -48,11 +42,18 @@ - + - - + + + + + + + + + diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping.rviz index cfa6cb1..f8f1964 100755 --- 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: diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping_astrapro.rviz new file mode 100755 index 0000000..f8f1964 --- /dev/null +++ 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 diff --git a/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz b/src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz new file mode 100755 index 0000000..1d3ea79 --- /dev/null +++ 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 diff --git a/src/3rd_app/move2grasp/scripts/.idea/.gitignore b/src/3rd_app/move2grasp/scripts/.idea/.gitignore new file mode 100755 index 0000000..d6fd6f2 --- /dev/null +++ b/src/3rd_app/move2grasp/scripts/.idea/.gitignore @@ -0,0 +1,2 @@ +# Default ignored files +/workspace.xml diff --git a/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml b/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml new file mode 100755 index 0000000..105ce2d --- /dev/null +++ b/src/3rd_app/move2grasp/scripts/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/src/3rd_app/move2grasp/scripts/.idea/modules.xml b/src/3rd_app/move2grasp/scripts/.idea/modules.xml new file mode 100755 index 0000000..875f7d4 --- /dev/null +++ b/src/3rd_app/move2grasp/scripts/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/src/3rd_app/move2grasp/scripts/.idea/scripts.iml b/src/3rd_app/move2grasp/scripts/.idea/scripts.iml new file mode 100755 index 0000000..d9e6024 --- /dev/null +++ b/src/3rd_app/move2grasp/scripts/.idea/scripts.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/src/3rd_app/move2grasp/scripts/.idea/vcs.xml b/src/3rd_app/move2grasp/scripts/.idea/vcs.xml new file mode 100755 index 0000000..c36ffda --- /dev/null +++ b/src/3rd_app/move2grasp/scripts/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/src/3rd_app/move2grasp/scripts/grasp.py b/src/3rd_app/move2grasp/scripts/grasp.py index 02530cf..300dee0 100755 --- 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) + + 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 diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.py b/src/3rd_app/move2grasp/scripts/grasp_d435.py new file mode 100755 index 0000000..a861164 --- /dev/null +++ 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") + diff --git a/src/3rd_app/move2grasp/scripts/grasp_pro.py b/src/3rd_app/move2grasp/scripts/grasp_pro.py new file mode 100755 index 0000000..f08edeb --- /dev/null +++ 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") + diff --git a/src/3rd_app/move2grasp/scripts/move.py b/src/3rd_app/move2grasp/scripts/move.py index f60efb1..be4d89b 100755 --- a/src/3rd_app/move2grasp/scripts/move.py +++ b/src/3rd_app/move2grasp/scripts/move.py @@ -17,14 +17,14 @@ 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) # 发布TWist消息控制机器人 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) # 发布机械臂抓取指令 - self.grasp_pub = rospy.Publisher('/grasp', String, queue_size=1) + self.grasp_pub = rospy.Publisher('/grasp', String, queue_size=1) # Subscribe to the move_base action server # 订阅move_base服务器的消息 @@ -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) @@ -73,13 +74,13 @@ class Move2Grasp(): def grasp_status_cp(self, msg): # 物体抓取成功,让机器人回起始点 if msg.data=='1': - goal = MoveBaseGoal() + 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() diff --git a/src/3rd_app/move2grasp/scripts/teleop.py b/src/3rd_app/move2grasp/scripts/teleop.py index 22c84c9..4b188fc 100755 --- 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()