working on visualizing point cloud

mdkennedy3 · Jul 26, 2018 · e4f5262 · e4f5262
1 parent f9b6050
commit e4f5262
Show file tree

Hide file tree

Showing 5 changed files with 98 additions and 48 deletions.
diff --git a/description/elp_standalone.urdf.xacro b/description/elp_standalone.urdf.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<robot name="elp_standalone" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- include the elp_stereo_camera macro -->
+  <xacro:include filename="$(find elp_stereo_synchronized_ros_pkg)/description/elp_stereo_camera.urdf.xacro"/>
+
+  <!-- use the elp_stereo_camera macro to create a camera with name "elp" -->
+  <elp_stereo_camera name="elp"/>
+</robot>
diff --git a/extra/elp_stereo_camera.rviz b/extra/elp_stereo_camera.rviz
@@ -5,10 +5,10 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Image1/Status1
+        - /PointCloud21
         - /TF1/Frames1
-      Splitter Ratio: 0.639535
-    Tree Height: 546
+      Splitter Ratio: 0.63953501
+    Tree Height: 595
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -17,12 +17,12 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
+    Splitter Ratio: 0.588679016
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
-    Splitter Ratio: 0.67658
+    Splitter Ratio: 0.676580012
   - Class: rviz/Time
     Experimental: false
     Name: Time
@@ -37,7 +37,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -72,14 +72,15 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.01
-      Style: Flat Squares
+      Size (m): 0.0799999982
+      Style: Spheres
       Topic: /elp/points2
+      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
     - Class: rviz/Image
-      Enabled: true
+      Enabled: false
       Image Topic: /elp/left/image_rect_color
       Max Value: 1
       Median window: 5
@@ -88,7 +89,8 @@ Visualization Manager:
       Normalize Range: true
       Queue Size: 2
       Transport Hint: compressed
-      Value: true
+      Unreliable: false
+      Value: false
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -143,7 +145,7 @@ Visualization Manager:
           Value: true
         elp_right_optical_frame:
           Value: true
-      Marker Scale: 0.1
+      Marker Scale: 0.100000001
       Name: TF
       Show Arrows: true
       Show Axes: true
@@ -161,6 +163,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: elp
     Frame Rate: 30
   Name: root
@@ -182,80 +185,90 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 3.65183
+      Distance: 1.7603991
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.934373
-        Y: 0.000158046
-        Z: 1.19209e-07
+        X: 0.934373021
+        Y: 0.000158045994
+        Z: 1.19208998e-07
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 0.235398
+      Near Clip Distance: 0.00999999978
+      Pitch: -0.139602125
       Target Frame: <Fixed Frame>
       Value: ThirdPersonFollower (rviz)
-      Yaw: 3.14359
+      Yaw: 3.52359223
     Saved:
       - Class: rviz/Orbit
-        Distance: 0.821458
+        Distance: 0.821457982
         Enable Stereo Rendering:
-          Stereo Eye Separation: 0.06
+          Stereo Eye Separation: 0.0599999987
           Stereo Focal Distance: 1
           Swap Stereo Eyes: false
           Value: false
         Focal Point:
-          X: -0.244971
-          Y: 0.227049
-          Z: -0.169792
+          X: -0.244971007
+          Y: 0.227048993
+          Z: -0.169791996
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.0500000007
+        Invert Z Axis: false
         Name: Camera
-        Near Clip Distance: 0.01
-        Pitch: 0.405203
+        Near Clip Distance: 0.00999999978
+        Pitch: 0.405203015
         Target Frame: <Fixed Frame>
         Value: Orbit (rviz)
-        Yaw: 5.54859
+        Yaw: 5.54859018
       - Class: rviz/ThirdPersonFollower
-        Distance: 3.65183
+        Distance: 3.65182996
         Enable Stereo Rendering:
-          Stereo Eye Separation: 0.06
+          Stereo Eye Separation: 0.0599999987
           Stereo Focal Distance: 1
           Swap Stereo Eyes: false
           Value: false
         Focal Point:
-          X: 0.934373
-          Y: 0.000158046
-          Z: 1.19209e-07
+          X: 0.934373021
+          Y: 0.000158045994
+          Z: 1.19208998e-07
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.0500000007
+        Invert Z Axis: false
         Name: Cloud, Behind Camera
-        Near Clip Distance: 0.01
-        Pitch: 0.235398
+        Near Clip Distance: 0.00999999978
+        Pitch: 0.235397995
         Target Frame: <Fixed Frame>
         Value: ThirdPersonFollower (rviz)
-        Yaw: 3.14359
-      - Angle: -1.57
+        Yaw: 3.14358997
+      - Angle: -1.57000005
         Class: rviz/TopDownOrtho
         Enable Stereo Rendering:
-          Stereo Eye Separation: 0.06
+          Stereo Eye Separation: 0.0599999987
           Stereo Focal Distance: 1
           Swap Stereo Eyes: false
           Value: false
+        Invert Z Axis: false
         Name: Cloud, Above Camera
-        Near Clip Distance: 0.01
-        Scale: 245.93
+        Near Clip Distance: 0.00999999978
+        Scale: 245.929993
         Target Frame: <Fixed Frame>
         Value: TopDownOrtho (rviz)
-        X: 1.80639
+        X: 1.80639005
         Y: -0.00118545
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1123
+  Height: 876
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b0000003c0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002b1000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f8000001090000001600ffffff000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006ff0000003efc0100000002fb0000000800540069006d00650100000000000006ff000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000434000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000023f000000cb0000001600ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055f0000003efc0100000002fb0000000800540069006d006501000000000000055f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003ef000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -264,6 +277,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1791
-  X: -7
-  Y: 17
+  Width: 1375
+  X: 65
+  Y: 24
diff --git a/launch/calibrate.launch b/launch/calibrate.launch
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="CAMERA_NAME" default="elp" />
+
+  <arg name="sync_split_node" default="split_sync_image_node" />
+
+  <!-- start camera -->
+  <include file="$(find elp_stereo_synchronized_ros_pkg)/launch/elp_stereo_camera.launch">
+    <arg name="CAMERA_NAME" value="$(arg CAMERA_NAME)" />
+  </include>
+
+  <!-- start calibration -->
+  <node pkg="camera_calibration" type="cameracalibrator.py" name="cameracalibrator" args=" --size 8x6 --square 0.025 --approximate=0.3 left:=/$(arg CAMERA_NAME)/$(arg sync_split_node)/left/image_raw right:=/$(arg CAMERA_NAME)/$(arg sync_split_node)/right/image_raw left_camera:=/$(arg CAMERA_NAME)/$(arg sync_split_node)/left right_camera:=/$(arg CAMERA_NAME)/$(arg sync_split_node)/right" required="true">
+  </node>
+</launch>
diff --git a/launch/rviz.launch b/launch/rviz.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<launch>
+  <!-- start publishing transforms -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(find elp_stereo_synchronized_ros_pkg)/description/elp_standalone.urdf.xacro'" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+
+  <!-- start rviz with custom vcg -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find elp_stereo_synchronized_ros_pkg)/extra/elp_stereo_camera.rviz" required="true">
+  </node>
+</launch>
diff --git a/src/split_sync_images.cpp b/src/split_sync_images.cpp
@@ -39,8 +39,8 @@ SplitImage::SplitImage() //: nh_(ros::NodeHandle("~")), it_(std::shared_ptr<imag
 {
   nh_ = ros::NodeHandle("~");
   it_.reset(new image_transport::ImageTransport(nh_)); //connect image transport to node
-  cinfo_left_= std::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nh_, "left"));  //Setup camera info managers in respective ns
-  cinfo_right_= std::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nh_, "right"));  
+  cinfo_left_= std::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nh_, "elp_left"));  //Setup camera info managers in respective ns
+  cinfo_right_= std::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nh_, "elp_right"));  
   //Load calibration files
   cinfo_left_->loadCameraInfo("package://elp_stereo_synchronized_ros_pkg/calibration/elp_left.yaml");
   cinfo_right_->loadCameraInfo("package://elp_stereo_synchronized_ros_pkg/calibration/elp_right.yaml");
@@ -98,4 +98,4 @@ int main(int argc, char **argv)
   SplitImage splt_img_obj;
   ros::spin();
   return 0;
-}
+}