diff --git a/launch/iri_mvbluefox3_camera.launch b/launch/iri_mvbluefox3_camera.launch index 9653d406069e4c68cd100211ce3783e1a11cb561..863a8997e674f64960217031939f033434e60cba 100644 --- a/launch/iri_mvbluefox3_camera.launch +++ b/launch/iri_mvbluefox3_camera.launch @@ -4,17 +4,18 @@ <arg name="num_cams" default="1" /> <arg name="c1_name" default="cam1" /> - <arg name="c1_serial" default="F0300141" /> + <arg name="c1_serial" default="F0300123" /> <arg name="c1_frame_id" default="$(arg c1_name)" /> <arg name="c1_tf_prefix" default="" /> - <arg name="c1_params_path" default="$(find iri_mvbluefox3_camera)/params/F0300141_params.xml" /> + <arg name="c1_params_path" default="$(find iri_mvbluefox3_camera)/params/F0300123_params.xml" /> + <!--<arg name="c1_params_path" default="$(find iri_mvbluefox3_camera)/params/example_params.xml" />--> <arg name="c1_calib_file" default="file://$(find iri_mvbluefox3_camera)/params/F0300141_calib.yaml" /> <arg name="c2_name" default="cam2" /> - <arg name="c2_serial" default="F0300123" /> + <arg name="c2_serial" default="F0300141" /> <arg name="c2_frame_id" default="$(arg c2_name)" /> <arg name="c2_tf_prefix" default="" /> - <arg name="c2_params_path" default="$(find iri_mvbluefox3_camera)/params/F0300123_params.xml" /> + <arg name="c2_params_path" default="$(find iri_mvbluefox3_camera)/params/F0300141_params.xml" /> <arg name="c2_calib_file" default="file://$(find iri_mvbluefox3_camera)/params/F0300123_calib.yaml" /> <node pkg="iri_mvbluefox3_camera" diff --git a/params/F0300123_params.xml b/params/F0300123_params.xml index b75af7e9ea97ebe079a6d0a89d0e9d3939bf2ad5..e50c22f4ffc83702ed0e0149baf3380994ac7242 100644 --- a/params/F0300123_params.xml +++ b/params/F0300123_params.xml @@ -1,12 +1,12 @@ <?xml version="1.0" encoding="UTF-8" standalone="yes"?> -<PropList contentDescriptor="mvBlueFOX3-M1012bC data(interface layout: GenICam)" name="DriverData" default="0" flags="3" size="7" parent="none" versionMajor="2" versionMinor="18" versionRelease="0"> +<PropList contentDescriptor="mvBlueFOX3-M1012bC data(interface layout: GenICam)" name="DriverData" default="0" flags="3" size="7" parent="none" versionMajor="2" versionMinor="17" versionRelease="0"> <PropList name="ImagingSubsystem" default="0" position="0" flags="3" size="5" parent="none"> <PropList name="Setting" default="0" position="0" flags="3" size="1" parent="none"> <PropList name="Base" default="0" position="0" flags="3" size="6" parent="none"> <PropList name="Camera" default="0" position="1" flags="3" size="3" parent="none"> <Property name="GenICamFeatureBag_91473E04-B76B-422d-8F52-A5D5A768C984" default="0" position="0" flags="7" size="1" formatString="%s" valType="4"> <Value index="0"># {05D8C294-F295-4dfb-9D01-096BD04049F4} -# GenApi persistence file (version 3.0.0) +# GenApi persistence file (version 3.0.1) # Device = MATRIX_VISION::mvBlueFOX3 -- MATRIX VISION USB3Vision camera interface -- Device version = 53.0.0 -- Product GUID = 8A5441D1-A059-11E6-A17E-E9D83A4B713F -- Product version GUID = 8A5468E1-A059-11E6-8532-E9D83A4B713F DeviceClockSelector Sensor mvDeviceClockFrequency kHz_66000 @@ -22,7 +22,7 @@ Width 1280 Height 960 OffsetX 0 OffsetY 0 -PixelFormat BayerGR8 +PixelFormat RGB8 TestPattern Off BinningHorizontal 1 DecimationHorizontal 1 @@ -31,14 +31,8 @@ ReverseX 0 ReverseY 0 AcquisitionMode Continuous ExposureMode Timed -ExposureTime 23399 -ExposureAuto Continuous -mvExposureAutoLowerLimit 74 -mvExposureAutoUpperLimit 23399 -mvExposureAutoAverageGrey 50 -mvExposureAutoHighlightAOI Off -mvExposureAutoAOIMode mvFull -mvExposureAutoMode mvDevice +ExposureTime 20000 +ExposureAuto Off TriggerSelector FrameStart TriggerMode Off TriggerSelector AcquisitionStart @@ -100,9 +94,8 @@ TriggerSelector FrameBurstActive TriggerDelay 0 TriggerSelector FrameStart mvAcquisitionFrameRateLimitMode mvDeviceLinkThroughput -AcquisitionFrameRateEnable 1 -mvAcquisitionFrameRateEnable On -AcquisitionFrameRate 25.3 +AcquisitionFrameRateEnable 0 +mvAcquisitionFrameRateEnable Off AcquisitionFrameCount 1 mvAcquisitionMemoryMode Default CounterSelector Counter1 @@ -166,49 +159,32 @@ TimerSelector Timer2 TimerDelay 0 TimerSelector Timer1 TimerSelector Timer1 -TimerValue 16407 +TimerValue 1128 TimerSelector Timer2 -TimerValue 14452 +TimerValue 12347 TimerSelector Timer1 GainSelector AnalogAll -Gain 6.209 +Gain 0 GainSelector DigitalAll Gain 0 GainSelector AnalogAll GainSelector AnalogAll -GainAuto Continuous -GainSelector AnalogAll -GainSelector AnalogAll -mvGainAutoUpperLimit 32 -GainSelector AnalogAll -GainSelector AnalogAll -mvGainAutoLowerLimit 0 -GainSelector AnalogAll -GainSelector AnalogAll -mvGainAutoAverageGrey 50 -GainSelector AnalogAll -GainSelector AnalogAll -mvGainAutoHighlightAOI Off -GainSelector AnalogAll -GainSelector AnalogAll -mvGainAutoAOIMode mvFull +GainAuto Off GainSelector AnalogAll -mvGainAutoMode mvDevice BlackLevelSelector All BlackLevel 0 BlackLevelSelector DigitalAll BlackLevel 0 -BlackLevelSelector DigitalAll +BlackLevelSelector All BlackLevelSelector All BlackLevelAuto Continuous -BlackLevelSelector DigitalAll +BlackLevelSelector All BalanceRatioSelector Red -BalanceRatio 0.800271 +BalanceRatio 1.21 BalanceRatioSelector Blue -BalanceRatio 3.30967 +BalanceRatio 1.9 BalanceRatioSelector Blue -BalanceWhiteAuto Continuous -mvBalanceWhiteAutoAOIMode mvFull +BalanceWhiteAuto Off mvLogicGateANDSelector mvLogicGateAND1 mvLogicGateANDSource1 Off mvLogicGateANDSelector mvLogicGateAND2 @@ -405,7 +381,7 @@ EventSelector ExposureEnd <Value index="0" val="Mono8_To_RGB888Packed"></Value> </Property> </PropList> -<PropList name="DefectivePixelsFilter" default="0" position="2" flags="3" size="8" parent="none"> +<PropList name="DefectivePixelsFilter" default="0" position="2" flags="3" size="4" parent="none"> <Property name="Mode" default="0" position="0" flags="7" size="1" formatString="%d" valType="1"> <Value index="0" val="Off"></Value> </Property> @@ -415,12 +391,6 @@ EventSelector ExposureEnd <Property name="ColdPixelDeviation_pc" default="0" position="2" flags="7" size="1" formatString="%d %%" valType="1"> <Value index="0" val="15 %"></Value> </Property> -<Property name="DefectivePixelOffsetX" default="0" position="4" flags="3" size="1" formatString="%d" valType="1"> -<Value index="0" val="0"></Value> -</Property> -<Property name="DefectivePixelOffsetY" default="0" position="5" flags="3" size="1" formatString="%d" valType="1"> -<Value index="0" val="0"></Value> -</Property> </PropList> <PropList name="DarkCurrentFilter" default="0" position="3" flags="3" size="3" parent="none"> <Property name="Mode" default="0" position="0" flags="7" size="1" formatString="%d" valType="1"> @@ -1961,7 +1931,7 @@ EventSelector ExposureEnd </PropList> <PropList name="ImageDestination" default="0" position="4" flags="3" size="5" parent="none"> <Property name="PixelFormat" default="0" position="0" flags="7" size="1" formatString="%d" valType="1"> -<Value index="0" val="Mono8"></Value> +<Value index="0" val="Auto"></Value> </Property> <Property name="ScalerMode" default="0" position="1" flags="7" size="1" formatString="%d" valType="1"> <Value index="0" val="Off"></Value> @@ -2034,13 +2004,10 @@ EventSelector ExposureEnd <Value index="0" val=""></Value> </Property> </PropList> -<PropList name="ImageProcessingControl" default="0" position="3" flags="3" size="2" parent="none"> +<PropList name="ImageProcessingControl" default="0" position="3" flags="3" size="1" parent="none"> <Property name="ImageProcessingOptimization" default="0" position="0" flags="7" size="1" formatString="%d" valType="1"> <Value index="0" val="MinimizeMemoryUsage"></Value> </Property> -<Property name="ImageProcessingMode" default="0" position="1" flags="7" size="1" formatString="%d" valType="1"> -<Value index="0" val="ProcessLatestOnly"></Value> -</Property> </PropList> <Property name="FeaturePollingEnable" default="0" position="6" flags="7" size="1" formatString="%d" valType="1"> <Value index="0" val="On"></Value> diff --git a/params/F0300141_calib.yaml b/params/F0300141_calib.yaml index a72f554bb92f159f709f2a6417f8952a01dee7b8..78d36d6720e500783cbf84887b7f017c994ef885 100644 --- a/params/F0300141_calib.yaml +++ b/params/F0300141_calib.yaml @@ -4,17 +4,17 @@ camera_name: camera camera_matrix: rows: 3 cols: 3 - data: [883.682266, 0, 683.433461, 0, 881.321367, 377.728018, 0, 0, 1] + data: [983.3368247116101, 0.0, 663.0390171499093, 0.0, 981.4430850536788, 396.26629958482715, 0.0, 0.0, 1.0] distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 - data: [-0.191329, 0.097859, 0.003095, -0.004325, 0.000000] + data: [-0.1107801575668356, 0.1031381384744468, -0.0020878830811691044, -0.002987672654714989, 0.0] rectification_matrix: rows: 3 cols: 3 - data: [1, 0, 0, 0, 1, 0, 0, 0, 1] + data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] projection_matrix: rows: 3 cols: 4 - data: [814.591980, 0, 678.046283, 0, 0, 832.671570, 371.066868, 0, 0, 0, 1, 0] + data: [958.0409545898438, 0.0, 656.8482799260782, 0.0, 0.0, 961.90283203125, 393.4869532290468, 0.0, 0.0, 0.0, 1.0, 0.0] diff --git a/src/mvbluefox3_camera_driver.cpp b/src/mvbluefox3_camera_driver.cpp index b79f8a2f6c145aae42bf2aae0fc9a38077af7281..903fb883e8dcf8e46542a2ce6ea5b249882850b1 100644 --- a/src/mvbluefox3_camera_driver.cpp +++ b/src/mvbluefox3_camera_driver.cpp @@ -302,9 +302,9 @@ void Mvbluefox3CameraDriver::GetROSImage(const int &ncam, sensor_msgs::Image &im encoding = "8UC1"; switch (bp) { - case 1: encoding = "8UC1"; break; + case 1: encoding = "mono8"; break; case 2: encoding = "8UC2"; break; - case 3: encoding = "8UC3"; break; + case 3: encoding = "rgb8"; break; case 4: encoding = "8UC4"; break; default: ROS_WARN("Invalid depth. Depth not implemented here."); break; }