Add launches and configs for testing

prosilica_camera/config/disparity_params_stereo_down.yaml

@@ -0,0 +1,9 @@
+prefilter_size: 9
+prefilter_cap: 31
+correlation_window_size: 25
+min_disparity: 13
+disparity_range: 144
+uniqueness_ratio: 10
+texture_threshold: 10
+speckle_size: 320
+speckle_range: 26

prosilica_camera/config/stereo_down.yaml

@@ -0,0 +1,55 @@
+/stereo_down:
+  left:
+    trigger_mode: "fixedrate"
+    trig_rate: 10
+    auto_exposure: True
+    exposure: 0.002
+    auto_gain: True
+    gain: 0
+    auto_whitebalance: True
+    whitebalance_red: 100
+    whitebalance_blue: 100
+    binning_x: 1
+    binning_y: 1
+    decimation_x: 2
+    decimation_y: 2
+    x_offset: 8
+    y_offset: 8
+    width: 1920
+    height: 1440
+    frame_id: "/stereo_down/left_optical"
+    trig_timestamp_topic: "trigger"
+    auto_adjust_stream_bytes_per_second: False
+    stream_bytes_per_second: 115000000
+    exposure_auto_max: 0.006
+    exposure_auto_target: 40
+    gain_auto_max: 20
+    gain_auto_target: 40
+    pixel_format: "Bayer8"
+  right:
+    trigger_mode: "syncin1"
+    trig_rate: 10
+    auto_exposure: True
+    exposure: 0.002
+    auto_gain: True
+    gain: 0
+    auto_whitebalance: True
+    whitebalance_red: 100
+    whitebalance_blue: 100
+    binning_x: 1
+    binning_y: 1
+    decimation_x: 2
+    decimation_y: 2
+    x_offset: 8
+    y_offset: 8
+    width: 1920
+    height: 1440
+    frame_id: "/stereo_down/right_optical"
+    trig_timestamp_topic: "trigger"
+    auto_adjust_stream_bytes_per_second: False
+    stream_bytes_per_second: 115000000
+    exposure_auto_max: 0.006
+    exposure_auto_target: 40
+    gain_auto_max: 20
+    gain_auto_target: 40
+    pixel_format: "Bayer8"

prosilica_camera/launch/prosilica_camera_base.launch

@@ -0,0 +1,20 @@
+<launch>
+
+  <!-- Namespace to publish in -->
+  <arg name="camera_ns" default="camera" />
+  <arg name="camera_name" default="camera" />
+  <arg name="guid" default="" />
+  <arg name="ip_address" default="" />
+
+  <!-- tf frame id associated with this camera -->
+  <arg name="frame_id" default="$(arg camera_name)_optical" />
+
+  <!-- The camera node -->
+  <node ns="$(arg camera_ns)" name="$(arg camera_name)" pkg="prosilica_camera" type="prosilica_node" output="screen">
+    <remap from="camera" to="$(arg camera_ns)" />
+    <param name="guid" type="str" value="$(arg guid)" />
+    <param name="ip_address" type="str" value="$(arg ip_address)" />
+    <param name="frame_id" value="$(arg frame_id)" />
+  </node>
+
+</launch>

prosilica_camera/launch/stereo_down.launch

@@ -0,0 +1,23 @@
+<launch>
+    <arg name="processing"  default="true"/>
+    <arg name="stereo"  default="stereo_down" />
+
+    <rosparam command="load" file="$(find prosilica_camera)/config/stereo_down.yaml"/>
+    <include file="$(find prosilica_camera)/launch/prosilica_camera_base.launch">
+        <arg name="camera_ns"     value="$(arg stereo)" />
+        <arg name="camera_name"   value="left" />
+        <arg name="ip_address"    value="192.168.1.80" />
+        <arg name="frame_id"      value="/stereo_down/left_optical" />
+    </include>
+    <include file="$(find prosilica_camera)/launch/prosilica_camera_base.launch">
+        <arg name="camera_ns"     value="$(arg stereo)" />
+        <arg name="camera_name"   value="right" />
+        <arg name="ip_address"    value="192.168.1.81" />
+        <arg name="frame_id"      value="/stereo_down/right_optical" />
+    </include>
+
+    <node if="$(arg processing)" ns="$(arg stereo)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc" respawn="true" output="screen">
+        <rosparam file="$(find prosilica_camera)/config/disparity_params_$(arg stereo).yaml"/>
+    </node>
+
+</launch>

prosilica_camera/launch/stereo_pipeline.launch

@@ -0,0 +1,39 @@
+<launch>
+
+  <arg name="stereo" />
+  <arg name="enable_decimate_x2" default="true"/>
+  <arg name="enable_decimate_x4" default="false"/>
+  <arg name="launch_in_nuc" default="true"/>
+
+  <include file="$(find prosilica_camera)/launch/stereo_image_proc.launch" >
+    <arg name="stereo" value="$(arg stereo)" />
+    <arg name="disparity_params" value="$(find prosilica_camera)/config/disparity_params_stereo.yaml" />
+  </include>
+
+  <!-- x2 decimate -->
+  <group if="$(arg enable_decimate_x2)">
+    <include file="$(find prosilica_camera)/launch/stereo_crop_decimate.launch" >
+      <arg name="stereo" value="$(arg stereo)" />
+      <arg name="decimation" value="2" />
+    </include>
+    <include file="$(find prosilica_camera)/launch/stereo_image_proc.launch" >
+      <arg name="stereo" value="$(arg stereo)/scaled_x2" />
+      <arg name="disparity_params" value="$(find prosilica_camera)/config/disparity_scaled_x2_params_stereo.yaml"/>
+      <arg name="launch_in_nuc" value="$(arg launch_in_nuc)" />
+    </include>
+  </group>
+
+  <!-- x4 decimate -->
+  <group if="$(arg enable_decimate_x4)">
+    <include file="$(find prosilica_camera)/launch/stereo_crop_decimate.launch" >
+      <arg name="stereo" value="$(arg stereo)" />
+      <arg name="decimation" value="4" />
+    </include>
+    <include file="$(find prosilica_camera)/launch/stereo_image_proc.launch" >
+      <arg name="stereo" value="$(arg stereo)/scaled_x4" />
+      <arg name="disparity_params" value="$(find prosilica_camera)/config/disparity_scaled_x4_params_stereo.yaml"/>
+    </include>
+  </group>
+
+
+</launch>

prosilica_camera/src/nodes/prosilica_nodelet.cpp

@@ -48,6 +48,7 @@
 #include <sensor_msgs/SetCameraInfo.h>
 
 #include <boost/thread.hpp>
+#include <boost/format.hpp>
 
 #include <prosilica_camera/ProsilicaCameraConfig.h>
 #include "prosilica/prosilica.h"

prosilica_camera/src/online_histogram.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+import rospy
+import sys, random
+import signal
+import cv2
+import numpy
+import matplotlib
+from matplotlib.backends.backend_qt5agg import (
+    FigureCanvasQTAgg as FigureCanvas,
+    NavigationToolbar2QT as NavigationToolbar)
+from matplotlib.figure import Figure
+from PyQt5 import QtCore, QtGui, QtWidgets
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+##########################################
+## Plot a histogram from a colour image ##
+##########################################
+##
+## From the terminal run:
+##
+##  rosrun utils online_histogram.py image:=/stereo_down/left/image_rect_color
+##
+## or from a launchfile:
+##
+##  <node name="online_histogram" pkg="utils" type="online_histogram.py">
+##    <remap from="image" to="/stereo_down/left/image_rect_color"/>
+##  </node>
+
+def handleIntSignal(signum, frame):
+    '''Ask app to close if Ctrl+C is pressed.'''
+    QtWidgets.QApplication.closeAllWindows()
+
+class PlotWindowWidget(QtWidgets.QMainWindow):
+    def __init__(self, parent=None):
+        QtWidgets.QMainWindow.__init__(self, parent)
+        self.setWindowTitle('Histogramm')
+        self.create_main_frame()
+        self.on_draw()
+
+    def save_plot(self):
+        pass
+
+    def on_about(self):
+        pass
+
+    def on_pick(self, event):
+        pass
+
+    def on_draw(self):
+        self.axes.clear()
+        self.axes.grid(True)
+        self.canvas.draw()
+
+    def create_main_frame(self):
+        self.main_frame = QtWidgets.QWidget()
+        self.dpi = 100
+        self.fig = Figure((5.0, 4.0), dpi=self.dpi)
+        self.canvas = FigureCanvas(self.fig)
+        self.canvas.setParent(self.main_frame)
+        self.axes = self.fig.add_subplot(111)
+        self.canvas.mpl_connect('pick_event', self.on_pick)
+        self.mpl_toolbar = NavigationToolbar(self.canvas, self.main_frame)
+        vbox = QtWidgets.QVBoxLayout()
+        vbox.addWidget(self.canvas)
+        vbox.addWidget(self.mpl_toolbar)
+        self.main_frame.setLayout(vbox)
+        self.setCentralWidget(self.main_frame)
+
+class OnlineHistWidget(PlotWindowWidget):
+  def __init__(self):
+    PlotWindowWidget.__init__(self)
+    rospy.init_node('visualizer', anonymous=True)
+    self.subscriber = rospy.Subscriber("image", Image, self.plotResults, queue_size = 1 )
+    self.bridge = CvBridge()
+
+  def plotResults(self, data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    self.axes.clear()
+    self.axes.set_autoscaley_on(False)
+
+    color = ('b','g','r')
+    for i,col in enumerate(color):
+        histr = cv2.calcHist([cv_image],[i],None,[256],[0,256])
+        (dummy, maxVal, dummy, dummy) = cv2.minMaxLoc(histr)
+        histr = histr / maxVal;
+        self.axes.plot(histr,color = col)
+        self.axes.set_xlim([0,256])
+        self.axes.set_ylim([0,1])
+    self.canvas.draw()
+
+if __name__ == "__main__":
+    signal.signal(signal.SIGINT, handleIntSignal)
+    app = QtWidgets.QApplication(sys.argv)
+    window = OnlineHistWidget()
+    window.show()
+    while not rospy.is_shutdown():
+        app.exec_()