Skip to content

Commit

Permalink
URDF Description, Diagnostics, ISP Enable, and Launch Files (#81)
Browse files Browse the repository at this point in the history
* Changes required to use GigE Blackfly S version

* Added blackfly mesh

* Added URDF of blackflys and CHANGELOG

* Added new_line at end of flir_blackflys.urdf.xacro

* Added DiagnosticAnalyzers and more detailed diagnostic messages

* Added ISP enable and disable config and updated camera launch file to be more descriptive

* Switched order of configuration to put ISP enable next to color encoding

* Updated config to include enumeration for Off, Once, Continuous parameters, and udpated diagnostics.launch

* Handled issue where no namespace prevents diagnostics_agg from loading from analyzer paramaters
  • Loading branch information
luis-camero authored Dec 1, 2021
1 parent da37069 commit a79c509
Show file tree
Hide file tree
Showing 12 changed files with 374 additions and 42 deletions.
9 changes: 9 additions & 0 deletions flir_camera_description/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package pointgrey_camera_description
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

0.1.0 (2021-11-10)
-------------------
* Initial Release
* Contributors: Luis Camero

10 changes: 10 additions & 0 deletions flir_camera_description/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 2.8.3)
project(flir_camera_description)

find_package(catkin REQUIRED)

catkin_package()

install(DIRECTORY launch meshes rviz urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
Binary file not shown.
15 changes: 15 additions & 0 deletions flir_camera_description/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<package format="2">
<name>flir_camera_description</name>
<version>0.1.0</version>
<description>URDF descriptions for Flir cameras</description>

<maintainer email="lcamero@clearpathrobotics.com">Luis Camero</maintainer>

<license>BSD</license>

<buildtool_depend>catkin</buildtool_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>
108 changes: 108 additions & 0 deletions flir_camera_description/urdf/flir_blackflys.urdf.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="flir_blackflys">
<!--
camera_{x,y,z,mass} are physical properties of camera collision model
lens_{r, h, mass} are physical properties of the camera lens
hfov, fps, width, height, format, near, and far are simulation
-->
<xacro:macro name="flir_blackflys"
params="frame:=camera name:=camera camera_x:=0.047758 camera_y:=0.029 camera_z:=0.029 camera_mass:=0.03
lens_r:=0.016 lens_h:=0.04 lens_mass:= 0.05
hfov:=1.0471975512 fps:=30 width:=720 height:=540 format:=R8G8B8 near:=0.5 far:=300">

<link name="${frame}">
<inertial>
<mass value="${camera_mass}" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
<visual>
<origin xyz="0 0 0.01425" rpy="0 0 0" />
<geometry>
<mesh filename="package://flir_camera_description/meshes/flir_blackflys.stl"/>
</geometry>
<material name="dark_grey" />
</visual>
<collision>
<origin xyz="0.003 0 ${camera_z/2}" rpy="0 0 0" />
<geometry>
<box size="${camera_x} ${camera_y} ${camera_z}" />
</geometry>
</collision>
</link>

<link name="${frame}_lens">
<intertial>
<mass values="${lens_mass}" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</intertial>
<visual>
<origin xyz="${lens_h/2} 0 0" rpy="0 1.5707 0" />
<geometry>
<cylinder radius="${lens_r}" length="${lens_h}"/>
</geometry>
<material name="dark_grey" />
</visual>
<collision>
<origin xyz="${lens_h/2} 0 0" rpy="0 1.5707 0" />
<geometry>
<cylinder radius="${lens_r}" length="${lens_h}"/>
</geometry>
</collision>
</link>

<link name="${frame}_optical" />

<joint name="${frame}_lens_joint" type="fixed">
<origin xyz="0.02685 0 0.01425" rpy="0 0 0" />
<parent link="${frame}" />
<child link="${frame}_lens" />
</joint>

<joint name="${frame}_optical_joint" type="fixed">
<origin xyz="${lens_h} 0 0" rpy="-1.570796 0 -1.570796" />
<parent link="${frame}_lens" />
<child link="${frame}_optical" />
</joint>

<!-- Gazebo -->
<gazebo reference="${frame}_optical">
<material>Gazebo/Grey</material>
<sensor type="camera" name="${name}">
<update_rate>${fps}</update_rate>
<camera name="${name}">
<pose>0 0 0 0 -1.5707 1.5707</pose>
<horizontal_fov>${hfov}</horizontal_fov>
<image>
<width>${width}</width>
<height>${height}</height>
<format>${format}</format>
</image>
<clip>
<near>${near}</near>
<far>${far}</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>

<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>${fps}</updateRate>
<cameraName>${name}</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>${frame}_optical</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
14 changes: 9 additions & 5 deletions spinnaker_camera_driver/cfg/Spinnaker.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -78,18 +78,21 @@ gen = ParameterGenerator()
gen.add("acquisition_frame_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, "User controlled acquisition frame rate in Hertz (frames per second).", 10, 0, 120)
gen.add("acquisition_frame_rate_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables manual (True) and automatic (False) control of the aquisition frame rate", False)


# Set Exposure
# Note: For the Auto Exposure feature, gain and/or exposure time must be set to Once or Continuous.
capture_modes = gen.enum([gen.const("AutoOff", str_t, "Off", ""),
gen.const("AutoOnce", str_t, "Once", ""),
gen.const("AutoContinuous", str_t, "Continuous", "")],
"Automatic mode: Off, Once, or Continuous.")
gen.add("exposure_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Sets the operation mode of the Exposure (Timed or TriggerWidth).", "Timed")
gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the automatic exposure mode to: 'Off', 'Once' or 'Continuous'", "Continuous")
gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the automatic exposure mode to: 'Off', 'Once' or 'Continuous'", "Continuous", edit_method=capture_modes)
gen.add("exposure_time", double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposure time in microseconds when Exposure Mode is Timed and Exposure Auto is not Continuous.", 100.0, 0.0, 3000000.0)
gen.add("auto_exposure_time_upper_limit", double_t, SensorLevels.RECONFIGURE_RUNNING, "Upper Limit on Shutter Speed.", 5000, 0.0, 3000000.0)


# Gain Settings
gen.add("gain_selector", str_t, SensorLevels.RECONFIGURE_RUNNING, "Selects which gain to control. The All selection is a total amplification across all channels.", "All")
gen.add("auto_gain", str_t, SensorLevels.RECONFIGURE_RUNNING, "Gain state control. (Off, Once, Continuous)", "Continuous")
gen.add("auto_gain", str_t, SensorLevels.RECONFIGURE_RUNNING, "Gain state control. (Off, Once, Continuous)", "Continuous", edit_method=capture_modes)
gen.add("gain", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the amplification of the video signal in dB.", 0, -10, 30)

# Pan and Tilt not in Spinnaker Driver
Expand All @@ -113,7 +116,7 @@ gen.add("gamma", double_t, SensorLevels.RECON


# White Balance
gen.add("auto_white_balance", str_t, SensorLevels.RECONFIGURE_RUNNING, "White Balance compensates for color shifts caused by different lighting conditions.", "Off")
gen.add("auto_white_balance", str_t, SensorLevels.RECONFIGURE_RUNNING, "White Balance compensates for color shifts caused by different lighting conditions.", "Off", edit_method=capture_modes)
gen.add("white_balance_blue_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance blue component.", 800, 0, 1023)
gen.add("white_balance_red_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance red component.", 550, 0, 1023)

Expand Down Expand Up @@ -174,7 +177,8 @@ codings = gen.enum([gen.const("Mono8", str_t, "Mono8", ""),

"Image Color Coding: Format of the pixel provided by the camera.")

gen.add("image_format_color_coding", str_t, SensorLevels.RECONFIGURE_STOP, "Image Color coding", "Mono8", edit_method = codings)
gen.add("image_format_color_coding", str_t, SensorLevels.RECONFIGURE_STOP, "Image Color coding", "Mono8", edit_method = codings)
gen.add("isp_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "Controls whether the image processing core is used for optional pixel format mode", True)


# Trigger parameters
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "spinnaker_camera_driver/diagnostics.h"
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/AddDiagnostics.h>
#include <ros/ros.h>
#include <bondcpp/bond.h>

#include <utility>
#include <string>
Expand All @@ -56,7 +58,7 @@ namespace spinnaker_camera_driver
class DiagnosticsManager
{
public:
DiagnosticsManager(const std::string name, const std::string serial, std::shared_ptr<ros::Publisher> const& pub);
DiagnosticsManager(const std::string name, const std::string serial, std::shared_ptr<ros::Publisher> const& pub, ros::NodeHandle& nh);
~DiagnosticsManager();

/*!
Expand Down Expand Up @@ -95,6 +97,8 @@ class DiagnosticsManager
std::pair<float, float> operational = std::make_pair(0.0, 0.0), float lower_bound = 0,
float upper_bound = 0);

void addAnalyzers();

private:
/*
* diagnostic_params is aData Structure to represent a parameter and its
Expand Down Expand Up @@ -126,6 +130,8 @@ class DiagnosticsManager
// constuctor parameters
std::string camera_name_;
std::string serial_number_;
ros::NodeHandle nh_;
std::shared_ptr<bond::Bond> bond_ = nullptr;
std::shared_ptr<ros::Publisher> diagnostics_pub_;

// vectors to keep track of the items to publish
Expand Down
55 changes: 51 additions & 4 deletions spinnaker_camera_driver/launch/camera.launch
Original file line number Diff line number Diff line change
Expand Up @@ -29,30 +29,77 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<arg name="camera_name" default="camera" />
<arg name="camera_serial" default="0" />
<arg name="calibrated" default="0" />
<arg name="device_type" default="GigE" /> <!-- USB3 or GigE -->
<arg name="device_type" default="USB3" /> <!-- USB3 or GigE -->

<!-- When unspecified, the driver will use the default framerate as given by the
camera itself. Use the parameter 'control_frame_rate' to enable manual frame
rate control, and 'frame_rate' to set the frame rate value. -->
<arg name="control_frame_rate" default="false" />
<arg name="control_frame_rate" default="True" />
<arg name="frame_rate" default="30" />

<!-- Disabling ISP will dramatically increase frame-rate. However, it can only be
disabled when using Bayer encoding (e.g. BayerRG8)-->
<arg name="isp_enable" default="False" />
<arg name="encoding" default="BayerRG8" />
<arg name="color_balance" default="Continuous" /> <!-- Off, Once, or Continuous -->
<!-- Available Encodings:
Mono: YUV: YCbCr: Other:
- Mono8 - YUV411Packed - YCbCr8 - BGR8
- Mono16 - YUV422Packed - YCbCr422_8 - BGRa8
- Mono12p - YUV444Packed - YCbCr411_8 - RGB8Packed
- Mono12Packed
Bayer:
- BayerGR8 - BayerGR12p
- BayerRG8 - BayerRG12p
- BayerGB8 - BayerGB12p
- BayerBG8 - BayerBG12p
- BayerGR16 - BayerGR12Packed
- BayerRG16 - BayerRG12Packed
- BayerGB16 - BayerGB12Packed
- BayerBG16 - BayerBG12Packed
-->

<group ns="$(arg camera_name)">
<!-- Nodelet manager -->
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" cwd="node" output="screen"/>

->
<!-- Camera nodelet -->
<node pkg="nodelet" type="nodelet" name="spinnaker_camera_nodelet"
args="load spinnaker_camera_driver/SpinnakerCameraNodelet camera_nodelet_manager" >

<param name="frame_id" value="camera" />
<param name="frame_id" value="$(arg camera_name)" />
<param name="serial" value="$(arg camera_serial)" />
<param name="device_type" value="$(arg device_type)" />

<!-- Frame rate -->
<param name="acquisition_frame_rate_enable" value="$(arg control_frame_rate)" />
<param name="acquisition_frame_rate" value="$(arg frame_rate)" />

<!-- Image Processing -->
<param name="isp_enable" value="$(arg isp_enable)" />
<param name="auto_white_balance" value="$(arg color_balance)" />
<param name="image_format_color_coding" value="$(arg encoding)" />

<!-- Image Resolution -->
<!-- Height and width pixel size cannot be set directly. Instead use the
binning, offset, and region of interest options.
- RoI: range of pixels to select from original image
(Note: RoI is defined from image pixel origin (i.e. top left))
- Binning: reduces resolution by a factor of 1, 2, 4, or 8
- Offset: moves the pixel origin
x-offset = max_width/x_binning - roi_width/2
y-offset = max_height/y_binning - roi_height/2
-->
<!--
<param name="image_format_x_binning" value="2" />
<param name="image_format_y_binning" value="2" />
<param name="image_format_x_offset" value="128" />
<param name="image_format_y_offset" value="122" />
<param name="image_format_roi_width" value="1280" />
<param name="image_format_roi_height" value="720" />
-->

<!-- Use the camera_calibration package to create this file -->
<param name="camera_info_url" if="$(arg calibrated)"
value="file://$(env HOME)/.ros/camera_info/$(arg camera_serial).yaml" />
Expand Down
Loading

0 comments on commit a79c509

Please sign in to comment.