Change to Realsense camera. Improve some code.

felixchenfy · Mar 17, 2019 · cbbf77a · cbbf77a
1 parent c2fffbd
commit cbbf77a
Show file tree

Hide file tree

Showing 20 changed files with 295 additions and 192 deletions.
diff --git a/config/T_arm_to_depth.txt b/config/T_arm_to_depth.txt
@@ -1,4 +1,4 @@
-1.015825516564594633e-02 -1.571014577250055833e-01 9.875302232502403132e-01 1.206593650272730356e-01
-2.121286215231535377e-02 -9.873250757289093738e-01 -1.572870284423087961e-01 9.924210186297220682e-02
-9.997233739046502698e-01 2.254610426621582800e-02 -6.696928484046339031e-03 -1.451173908924094369e-02
+1.267144202140898894e-02 -2.484658169291787885e-02 9.996109652936363554e-01 1.518783334311267774e-01
+-8.786349520828576343e-02 -9.958519905095529756e-01 -2.363935718130003028e-02 7.400112596699989775e-02
+9.960519267422165424e-01 -8.752976851528090907e-02 -1.480198827116441414e-02 -1.663280100498204170e-02
 0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00
diff --git a/config/T_baxter_to_chess.txt b/config/T_baxter_to_chess.txt
@@ -1,4 +1,4 @@
-9.999744442655082999e-01 -1.112258856135611490e-03 7.062131131997760082e-03 -5.101391848720326916e-02
-1.074721269560885168e-03 9.999852876337731455e-01 5.316906073418393476e-03 3.947666465622167165e-01
--7.067941007205597558e-03 -5.309180373442531388e-03 9.999609276435165173e-01 -8.951477224263286736e-01
+-9.974627159812129440e-01 -7.115258181063210952e-02 -2.332451299280528887e-03 4.511688057607051577e-02
+7.118989888048504744e-02 -9.970795639155750623e-01 -2.764672709947880655e-02 6.508549185172319840e-01
+-3.585035125990692359e-04 -2.774262647277695240e-02 9.996150349767770615e-01 -1.780773787474331238e-01
 0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00
diff --git a/config/rviz_baxter_and_clouds.rviz b/config/rviz_baxter_and_clouds.rviz
@@ -441,25 +441,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.914466619491577
+      Distance: 2.856978416442871
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.20151878893375397
-        Y: 0.01987076736986637
-        Z: -0.6423836946487427
+        X: -0.4696725010871887
+        Y: 0.6163358688354492
+        Z: -0.5493106245994568
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7253981828689575
+      Pitch: 1.000397801399231
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.9053981304168701
+      Yaw: 0.04540303722023964
     Saved: ~
 Window Geometry:
   Camera:

diff --git a/config/rviz_debug_depth_cam.rviz b/config/rviz_debug_depth_cam.rviz
@@ -5,12 +5,15 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Status1
         - /Camera1/Visibility1
         - /TF1/Frames1
         - /TF1/Tree1
         - /PointCloud21
+        - /PointCloud21/Status1
+        - /Image1
       Splitter Ratio: 0.46100521087646484
-    Tree Height: 490
+    Tree Height: 741
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -54,17 +57,18 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz/Camera
-      Enabled: true
+      Enabled: false
       Image Rendering: background and overlay
       Image Topic: /camera/depth/image
       Name: Camera
       Overlay Alpha: 0.5
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
       Visibility:
         Grid: true
+        Image: true
         PointCloud2: true
         RobotModel: true
         TF: true
@@ -123,18 +127,30 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.009999999776482582
+      Size (m): 0.0020000000949949026
       Style: Flat Squares
-      Topic: /camera/depth_registered/points
+      Topic: /camera/depth/color/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/Image
+      Enabled: false
+      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: raw
+      Unreliable: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: camera_rgb_frame
+    Fixed Frame: base
     Frame Rate: 30
   Name: root
   Tools:
@@ -155,25 +171,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.188600778579712
+      Distance: 3.4192166328430176
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.8121629357337952
-        Y: -0.06746526807546616
-        Z: 0.11710114777088165
+        X: 1.101098895072937
+        Y: -0.17598208785057068
+        Z: 0.32941436767578125
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4603986442089081
+      Pitch: 0.585204541683197
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.4635860919952393
+      Yaw: 3.4085798263549805
     Saved: ~
 Window Geometry:
   Camera:
@@ -183,7 +199,9 @@ Window Geometry:
   Height: 960
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000003a300000322fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000227000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026a000000f50000001600ffffff000000010000010f00000322fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000322000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002c20000032200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f500000322fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000322000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001de000000b70000001600fffffffb0000000a0049006d006100670065000000018f000001d00000001600ffffff000000010000010f00000322fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000322000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004700000032200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/config/rviz_depth_image.rviz b/config/rviz_depth_image.rviz
@@ -4,8 +4,8 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Camera1
         - /TF1/Tree1
+        - /Image1
       Splitter Ratio: 0.46100521087646484
     Tree Height: 371
   - Class: rviz/Selection
@@ -26,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Camera
+    SyncSource: Image
 Preferences:
   PromptSaveOnExit: true
 Visualization Manager:
@@ -51,17 +51,18 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz/Camera
-      Enabled: true
+      Enabled: false
       Image Rendering: background and overlay
       Image Topic: /camera/rgb/image_raw
       Name: Camera
       Overlay Alpha: 0.5
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
-      Value: true
+      Value: false
       Visibility:
         Grid: true
+        Image: true
         PointCloud2: true
         RobotModel: true
         TF: true
@@ -127,6 +128,18 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - 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: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -170,17 +183,19 @@ Visualization Manager:
       Pitch: 0.7452033758163452
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.4385786056518555
+      Yaw: 5.42357873916626
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 548
+  Height: 328
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000017a00000132fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000003d000001320000001600ffffff0000000100000100000001a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002eb00000092fc0100000002fb0000000800540069006d00650100000000000002eb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000016b0000013200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000014f00000115fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000020a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000003d000002ce0000001600fffffffb0000000a0049006d0061006700650100000016000001150000001600ffffff0000000100000100000001a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002eb00000092fc0100000002fb0000000800540069006d00650000000000000002eb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000000200000011500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -189,6 +204,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 747
+  Width: 373
   X: 747
   Y: 27
diff --git a/config/rviz_final_cloud.rviz b/config/rviz_final_cloud.rviz
@@ -9,7 +9,7 @@ Panels:
         - /TF1/Tree1
         - /PointCloud21
       Splitter Ratio: 0.46100521087646484
-    Tree Height: 238
+    Tree Height: 363
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -28,7 +28,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
 Preferences:
   PromptSaveOnExit: true
 Visualization Manager:
@@ -154,35 +154,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.7155091762542725
+      Distance: 1.232211709022522
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.10018530488014221
-        Y: -0.11624740064144135
-        Z: 0.44035017490386963
+        X: -0.09286872297525406
+        Y: 0.02577781490981579
+        Z: 0.5341122150421143
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.39520299434661865
+      Pitch: 0.2052028477191925
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.7854135036468506
+      Yaw: 5.165443420410156
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: true
   Displays:
     collapsed: true
-  Height: 565
+  Height: 600
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001ff00000197fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000179000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001bc000000180000001600ffffff000000010000010f00000229fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000229000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004950000003efc0100000002fb0000000800540069006d0065010000000000000495000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004950000019700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ff00000197fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000179000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001bc000000180000001600ffffff000000010000010f00000229fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000229000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004950000003efc0100000002fb0000000800540069006d0065010000000000000495000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000495000001e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -193,4 +193,4 @@ Window Geometry:
     collapsed: true
   Width: 1173
   X: 747
-  Y: 420
+  Y: 385