Skip to content

Commit

Permalink
Merge pull request #110 from neufieldrobotics/dev
Browse files Browse the repository at this point in the history
This merge from dev to master add the following major changes:
Add external triggering
ROI settings for images
Bring Travis CI to master
Adding gain settings to code
Bump up version to 1.1.0
Number of little improvements and bug fixes.
  • Loading branch information
vik748 authored Feb 13, 2021
2 parents 4977ee8 + b799b30 commit f6ab290
Show file tree
Hide file tree
Showing 33 changed files with 928 additions and 916 deletions.
3 changes: 3 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@ $ cat .gitignore
*.[oa]
*~

include/spinnaker_sdk_camera_driver/spinnaker_configure.h

# no .a files
#*.a

Expand All @@ -17,6 +19,7 @@ data/
devel
~devel/lib/datacollection_neu/*.xml
.catkin_workspace
include/spinnaker_sdk_camera_driver/spinnaker_configure.h

# ignore doc/notes.txt, but not doc/server/arch.txt
#doc/*.txt
Expand Down
39 changes: 39 additions & 0 deletions .travis.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
language: minimal

services:
- docker

matrix:
include:
- arch: amd64
env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=2.0.0.147
- arch: amd64
env : DOCKER_IMAGE=ros:kinetic-perception-xenial SPINNAKER_VERSION=2.0.0.147
- arch: amd64
env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=2.2.0.48
- arch: amd64
env : DOCKER_IMAGE=ros:kinetic-perception-xenial SPINNAKER_VERSION=1.24.0.60
- arch: amd64
env : DOCKER_IMAGE=ros:melodic-perception-bionic SPINNAKER_VERSION=1.24.0.60
- arch: arm64
env : DOCKER_IMAGE=arm64v8/ros:kinetic-perception-xenial SPINNAKER_VERSION=2.0.0.147
- arch: arm64
env : DOCKER_IMAGE=arm64v8/ros:melodic-perception-bionic SPINNAKER_VERSION=2.0.0.147

before_install:
- docker pull $DOCKER_IMAGE

script:
- echo TRAVIS_BUILD_DIR is $TRAVIS_BUILD_DIR
- >
docker run
-v $TRAVIS_BUILD_DIR:/ros_ws/src/spinnaker_sdk_camera_driver/
--env SPINNAKER_VERSION=$SPINNAKER_VERSION
--env SPINNAKER_LINUX_ARCH=$TRAVIS_CPU_ARCH
$DOCKER_IMAGE
/bin/bash -c
"
/ros_ws/src/spinnaker_sdk_camera_driver/download_and_install_spinnaker.sh;
cd /ros_ws;
catkin_make
"
37 changes: 20 additions & 17 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,7 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
###
# camera Acquisition specific
###
#SET("OpenCV_DIR" "~/apps/opencv-2.4.13/")
set(PROJECT_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include" CACHE PATH "Project Include Directory")
#set(SPINNAKER_INCLUDE_DIR "/usr/include/spinnaker" CACHE PATH "Spinnaker Include Directory")
#set(SPINNAKER_LIB_DIR "/usr/lib" CACHE PATH "Spinnaker Libs Directory")
# set(yaml-cpp_DIR "~/apps/yaml-cpp" CACHE PATH "yaml-cpp Directory")
set(CUDA_USE_STATIC_CUDA_RUNTIME OFF)

find_package(catkin REQUIRED COMPONENTS
Expand All @@ -25,7 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
dynamic_reconfigure
nodelet
)

find_package(trigger_msgs)
#find_package(PCL REQUIRED)

###
Expand Down Expand Up @@ -53,6 +49,13 @@ elseif(NOT Boost_FOUND)
message("Boost not found!")
endif()

# configure a header file to pass some of the CMake settings
# to the source code
configure_file (
"${PROJECT_SOURCE_DIR}/include/spinnaker_sdk_camera_driver/spinnaker_configure.h.in"
"${PROJECT_SOURCE_DIR}/include/spinnaker_sdk_camera_driver/spinnaker_configure.h"
)

add_message_files(
FILES
SpinnakerImageNames.msg
Expand Down Expand Up @@ -110,31 +113,27 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)

endif(${CMAKE_SYSTEM_PROCESSOR} MATCHES aarch64 OR arm)

# Make package version available in CPP as a defination
add_definitions("-D${PROJECT_NAME}_VERSION=\"${${PROJECT_NAME}_VERSION}\"")

add_library (acquilib SHARED
src/capture.cpp
src/camera.cpp
)

add_dependencies(acquilib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries(acquilib ${LIBS} ${catkin_LIBRARIES})

add_library(capture_nodelet src/capture_nodelet.cpp src/subscriber_nodelet.cpp)
target_link_libraries(capture_nodelet acquilib ${catkin_LIBRARIES})

add_executable (acquisition_node src/acquisition_node.cpp)
add_dependencies(acquisition_node acquilib ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries (acquisition_node acquilib ${LIBS} ${catkin_LIBRARIES})

## subscriber_example for subscribing as nodelet
add_library (subscriber_example examples/subscriber_nodelet.cpp)
add_dependencies(subscriber_example ${catkin_EXPORTED_TARGETS})
target_link_libraries(subscriber_example ${catkin_LIBRARIES})

add_executable (acquisition_nodelet src/acquisition_nodelet.cpp)
add_dependencies(acquisition_nodelet acquilib capture_nodelet ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries (acquisition_nodelet acquilib capture_nodelet ${LIBS} ${catkin_LIBRARIES})

add_executable (nodelet_test src/nodelet_test.cpp)
add_dependencies(nodelet_test ${catkin_EXPORTED_TARGETS})
target_link_libraries (nodelet_test capture_nodelet ${LIBS} ${catkin_LIBRARIES})

install(TARGETS acquilib acquisition_node capture_nodelet acquisition_nodelet nodelet_test
install(TARGETS acquilib acquisition_node subscriber_example
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
Expand All @@ -144,3 +143,7 @@ install(DIRECTORY include
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

install(FILES nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
53 changes: 43 additions & 10 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,7 +1,29 @@
# spinnaker_sdk_camera_driver
master:
[![Build Status](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver.svg?branch=master)](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver)
dev: [![Build Status](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver.svg?branch=dev)](https://travis-ci.org/neufieldrobotics/spinnaker_sdk_camera_driver)

# spinnaker_sdk_camera_driver
These are the ros drivers for running the Pt Grey (FLIR) cameras that use the Spinnaker SDK. This code has been tested with various Point Grey Blackfly S (BFS) cameras.

## Compatibility Matrix
| Spinnaker | Architecture | Ubnuntu 16.04 Xenial +<br>ROS Kinetic | Ubnuntu 18.04 Bionic +<br>ROS Melodic | Ubnuntu 20.04 Focal +<br>ROS Noetic |
|-----------|:------------:|:--------------------------------------:|:-------------------------------------:|:-----------------------------------:|
| 1.17.0.23 | AMD64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: |
| 1.17.0.23 | ARM64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: |
| 1.24.0.60 | AMD64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: |
| 1.24.0.60 | ARM64 | :heavy_check_mark: |:heavy_minus_sign: |:heavy_minus_sign: |
| 2.0.0.147 | AMD64 | :heavy_check_mark: |:white_check_mark: |:heavy_minus_sign: |
| 2.0.0.147 | ARM64 | :heavy_check_mark: | |:heavy_minus_sign: |
| 2.2.0.48 | AMD64 | :heavy_minus_sign: |:heavy_check_mark: | |
| 2.2.0.48 | ARM64 | :heavy_minus_sign: | | |
| 2.3.0.77 | AMD64 | :heavy_minus_sign: | | |
| 2.3.0.77 | ARM64 | :heavy_minus_sign: | | |

:heavy_check_mark: Tested
:heavy_minus_sign: Not Applicable
:white_check_mark: Reported to work
:x: Known compatibility Issue

## Getting Started

These instructions will get you a copy of the project up and running on your local machine for development and testing purposes.
Expand Down Expand Up @@ -48,15 +70,12 @@ source ~/spinnaker_ws/devel/setup.bash
Modify the `params/test_params.yaml` file replacing the cam-ids and master cam serial number to match your camera's serial number. Then run the code as:
```bash
# To launch nodelet verison of driver, use #

roslaunch spinnaker_sdk_camera_driver acquisition.launch

# To launch node version of driver, use #

roslaunch spinnaker_sdk_camera_driver acquisition_node.launch
roslaunch spinnaker_sdk_camera_driver node_acquisition.launch

# Test that the images are being published by running

rqt_image_view
```
## Parmeters
Expand All @@ -69,6 +88,8 @@ All the parameters can be set via the launch file or via the yaml config_file.
Should color images be used (only works on models that support color images)
* ~exposure_time (int, default: 0, 0:auto)
Exposure setting for cameras, also available as dynamic reconfiguarble parameter.
* ~external_trigger (bool, default: false)
Camera triggering setting when using an external trigger. In this mode, none of the cameras would be set as a master camera. All cameras are setup to use external trigger. In this mode the main loop runs at rate set by soft_framerate, so if the external trigger rate is higher than the soft_framerate, the buffer will get filled and images will have a lag. Also in this mode, the getnextimage timeout is set to infinite so that the node dosen't die if a trigger is not received for a while.
* ~target_grey_value (double, default: 0 , 0:Continous/auto)
Setting target_grey_value > 4 (min:4 , max:99) will turn AutoExposureTargetGreyValueAuto to 'off' and set AutoExposureTargetGreyValue to target_grey_value. Also available as dynamic reconfigurable parameter. see below in Dynamic reconfigurable parameter section.
* ~frames (int, default: 50)
Expand All @@ -81,7 +102,7 @@ All the parameters can be set via the launch file or via the yaml config_file.
Flag whether images should be saved or not (via opencv mat objects to disk)
* ~save_path (string, default: "\~/projects/data")
Location to save the image data
* \~save_type (string, default: "bmp")
* ~save_type (string, default: "bmp")
Type of file type to save to when saving images locally: binary, tiff, bmp, jpeg etc.
* ~soft_framerate (int, default: 20)
When hybrid software triggering is used, this controls the FPS, 0=as fast as possible
Expand All @@ -93,6 +114,18 @@ All the parameters can be set via the launch file or via the yaml config_file.
Flag whether each image should have Unique timestamps vs the master cams time stamp for all
* ~max_rate_save (bool, default: false)
Flag for max rate mode which is when the master triggers the slaves and saves images at maximum rate possible. This is the multithreaded mode
* ~flip_horizontal (bool, default: false)
Flag to flip image horizontally on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseX". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour.
* ~flip_vertical (bool, default: false)
Flag to flip image vertically on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseY". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour.
If both horizontal and vertical flags are true, then it is equivalent to rotating 180deg.
* ~region_of_interest (dict, default = { width: 0, height: 0, x_offset: 0, y_offset: 0 }
Specify the region of interest in the camera image as dict with width, height, x_offset and y_offset. Width and height specify size of the final image (should be smaller than sensor size). X and Y offsets specify the image origin. The offset plus image size should be smaller than the sensor size.

### node/nodelet Specific Parameters
* ~tf_prefix (string, default: "")
tf_prefix will be pre-fixed to (existing cam_*_optical_frame) frame id in ros Image msg and cameraInfo msg. default option doesn't add any prefix to frame id. eg: uas1/cam_0_optical_frame if tf_prefix:= uas1


### System configuration parameters
* ~cam_ids (yaml sequence or array)
Expand Down Expand Up @@ -120,11 +153,11 @@ This is the names that would be given to the cameras for filenames and rostopics
When exposure_time is set to 0(zero), the ExposureAuto is set to 'Continuous', enabling auto exposure.

### nodelet details
* ~nodelet_manager_name (string, default: vision_nodelet_manager)
* ~manager (string, default: vision_nodelet_manager)
Specify the name of the nodelet_manager under which nodelet to be launched.
* ~start_nodelet_manager (bool, default: false)
If set True, nodelet_manager of name $(arg nodelet_manager_name) will be launched.
If set False(default), the acquisition/capture_nodelet waits for the nodelet_manager name $(arg nodelet_manager_name).
* ~external_manager (bool, default: false)
If set False(default), nodelet_manager of name $(arg manager) will be launched.
If set True, the acquisition/Capture waits for the nodelet_manager name $(arg manager).

### Camera info message details
* ~image_width (int)
Expand Down
2 changes: 1 addition & 1 deletion cfg/debug_console.conf
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
# Set the default ros output to warning and higher
log4j.logger.ros=INFO
log4j.logger.ros.datacollection_neu=DEBUG
log4j.logger.ros.spinnaker_sdk_camera_driver=DEBUG


30 changes: 30 additions & 0 deletions download_and_install_spinnaker.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
#!/usr/bin/env bash
set -x

CWD=`pwd`

export DEBIAN_FRONTEND=noninteractive

# install basic packages
apt-get update
apt-get install -q -y --no-install-recommends \
build-essential tree wget dirmngr gnupg2 vim nano git debconf-utils libunwind-dev iputils-ping dialog apt-utils

wget https://coe.northeastern.edu/fieldrobotics/spinnaker_sdk_archive/spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH-pkg.tar.gz -nv

tar -zxvf spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH-pkg.tar.gz
cd spinnaker-$SPINNAKER_VERSION-$SPINNAKER_LINUX_ARCH

# fix issue with 'logname' command on docker, required by spinnaker 2.2.0.48
mv /usr/bin/logname /usr/bin/logname_old
echo "echo root" > /usr/bin/logname
chmod +x /usr/bin/logname

# auto accept spinnaker license agreements
echo libspinnaker libspinnaker/present-flir-eula note '' | debconf-set-selections
echo libspinnaker libspinnaker/accepted-flir-eula boolean true | debconf-set-selections

dpkg -i *.deb

cd ..

57 changes: 57 additions & 0 deletions examples/subscriber_nodelet.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
//
// Created by pushyami on 1/15/19.
//
//cpp
#include <iostream>
// ROS
#include <ros/ros.h>
#include <image_transport/image_transport.h>
//#include <cv_bridge/cv_bridge.h>
// msgs
#include "sensor_msgs/Image.h"
//#include "sensor_msgs/CameraInfo.h"

// nodelets
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>



namespace subscriber_nodelet_ns
{
class subscriber_nodelet: public nodelet::Nodelet
{
//This is a test nodelet for measuring nodelet performance
public:
subscriber_nodelet(){}
~subscriber_nodelet()
{
it_.reset();
}

std::shared_ptr<image_transport::ImageTransport> it_;
image_transport::Subscriber image_sub_;

virtual void onInit()
{
NODELET_INFO("Initializing Subscriber nodelet");
ros::NodeHandle& node = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();

it_.reset(new image_transport::ImageTransport(node));
image_sub_ = it_->subscribe("/camera_array/cam0/image_raw",1, &subscriber_nodelet::imgCallback, this);
NODELET_INFO("onInit for Subscriber nodelet Initialized");
}

void imgCallback(const sensor_msgs::Image::ConstPtr& msg)
{
// dont modify the input msg in any way
sensor_msgs::Image tmp_img = *msg;
NODELET_INFO_STREAM("diff time is "<< ros::Time::now().toSec() - tmp_img.header.stamp.toSec());
// copy input img to cv::mat and do any cv stuff
// dont do time intense stuff in callbacks
}
};
}

PLUGINLIB_EXPORT_CLASS(subscriber_nodelet_ns::subscriber_nodelet, nodelet::Nodelet)
7 changes: 6 additions & 1 deletion include/spinnaker_sdk_camera_driver/camera.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,10 +59,15 @@ namespace acquisition {
// void setTrigMode();
// void setTriggerOverlapOff();

string get_id() { return string(pCam_->GetUniqueID()); }
string getTLNodeStringValue(string node_string);
double getFloatValueMax(string node_string);
string get_id();
void make_master() { MASTER_ = true; ROS_DEBUG_STREAM( "camera " << get_id() << " set as master"); }
bool is_master() { return MASTER_; }
void set_color(bool flag) { COLOR_ = flag; }
void setGetNextImageTimeout(uint64_t get_next_image_timeout) { GET_NEXT_IMAGE_TIMEOUT_ = get_next_image_timeout; }
bool verifyBinning(int binningDesired);
void calibrationParamsTest(int calibrationWidth, int calibrationHeight);

private:

Expand Down
Loading

0 comments on commit f6ab290

Please sign in to comment.