├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── Makefile ├── README.md ├── examples ├── decklink.launch ├── gscam_nodelet.launch ├── minoru.launch ├── nodelet_pipeline.launch ├── osx.launch ├── uncalibrated_parameters.ini ├── v4l.launch ├── v4ljpeg.launch └── videofile.launch ├── include └── gscam │ ├── gscam.h │ └── gscam_nodelet.h ├── nodelet_plugins.xml ├── package.xml ├── scripts └── gscam_node.in └── src ├── gscam.cpp ├── gscam_node.cpp └── gscam_nodelet.cpp /.travis.yml: -------------------------------------------------------------------------------- 1 | 2 | # this is .traivs.yml written by ./create_travis_settings.py 3 | 4 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/devel_jobs.rst 5 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/prerelease_jobs.rst 6 | # while this doesn't require sudo we don't want to run within a Docker container 7 | sudo: true 8 | dist: trusty 9 | language: python 10 | python: 11 | - "3.4" 12 | env: 13 | global: 14 | - JOB_PATH=/tmp/devel_job 15 | - ABORT_ON_TEST_FAILURE=1 16 | matrix: 17 | - ROS_DISTRO_NAME=indigo OS_NAME=ubuntu OS_CODE_NAME=trusty ARCH=amd64 18 | - ROS_DISTRO_NAME=kinetic OS_NAME=ubuntu OS_CODE_NAME=xenial ARCH=amd64 19 | - ROS_DISTRO_NAME=lunar OS_NAME=ubuntu OS_CODE_NAME=xenial ARCH=amd64 20 | - ROS_DISTRO_NAME=melodic OS_NAME=ubuntu OS_CODE_NAME=bionic ARCH=amd64 21 | # matrix: 22 | # allow_failures: 23 | # - env: ROS_DISTRO_NAME=kinetic OS_NAME=ubuntu OS_CODE_NAME=xenial ARCH=amd64 24 | install: 25 | # either install the latest released version of ros_buildfarm 26 | - pip install ros_buildfarm 27 | # or checkout a specific branch 28 | #- git clone -b master https://github.com/ros-infrastructure/ros_buildfarm /tmp/ros_buildfarm 29 | #- pip install /tmp/ros_buildfarm 30 | # checkout catkin for catkin_test_results script 31 | - git clone https://github.com/ros/catkin /tmp/catkin 32 | # run devel job for a ROS repository with the same name as this repo 33 | - export REPOSITORY_NAME=`basename $TRAVIS_BUILD_DIR` 34 | # use the code already checked out by Travis 35 | - mkdir -p $JOB_PATH/catkin_workspace/src 36 | - cp -R $TRAVIS_BUILD_DIR $JOB_PATH/catkin_workspace/src/ 37 | # generate the script to run a pre-release job for that target and repo 38 | - generate_prerelease_script.py https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml $ROS_DISTRO_NAME default $OS_NAME $OS_CODE_NAME $ARCH --output-dir $JOB_PATH 39 | # run the actual job which involves Docker 40 | - cd $JOB_PATH; sh ./prerelease.sh -y 41 | script: 42 | # get summary of test results 43 | - /tmp/catkin/bin/catkin_test_results $JOB_PATH/catkin_workspace/test_results --all 44 | notifications: 45 | email: false 46 | 47 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package gscam 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.1 (2018-09-06) 6 | ------------------ 7 | * Merge pull request `#52 `_ from k-okada/add_travis 8 | update travis.yml 9 | * Update README.md 10 | gscam -> GSCam 11 | * update travis.yml 12 | * Merge pull request `#40 `_ from ros-drivers/mikaelarguedas-patch-1 13 | update to use non deprecated pluginlib macro 14 | * update to use non deprecated pluginlib macro 15 | * Contributors: Kei Okada, Mikael Arguedas 16 | 17 | 1.0.0 (2017-06-13) 18 | ------------------ 19 | * Gstreamer 1 0 support (`#36 `_ ) 20 | * delete unused manifest.xml, rosdep.yaml 21 | * use libgstreamer1.0 for lunar 22 | * Fixing preproc switches so that the 1.0 branch still builds against 0.1 without the switch flags 23 | * Preliminary GStreamer-1.0 support see README for more info 24 | * Contributors: Jonathan Bohren, Kei Okada 25 | 26 | 0.2.0 (2017-06-13) 27 | ------------------ 28 | * add ROS Orphaned Package Maintainers to maintainer tag (`#35 `_ ) 29 | * gscam_nodelet.h: include scoped_ptr.hpp to compile with boost 1.57 30 | When compiling gscam with the currently latest boost version 1.57, 31 | it fails with: 32 | In file included from [...]/src/gscam_nodelet.cpp:5:0: 33 | [...]/include/gscam/gscam_nodelet.h:20:12: error: 'scoped_ptr' in namespace 'boost' does not name a template type 34 | boost::scoped_ptr gscam_driver\_; 35 | ^ 36 | [...]/include/gscam/gscam_nodelet.h:21:12: error: 'scoped_ptr' in namespace 'boost' does not name a template type 37 | boost::scoped_ptr stream_thread\_; 38 | ^ 39 | It seems that the dependencies of boost/thread.hpp have changed 40 | and boost/scoped_ptr.hpp is not included anymore with 41 | boost/thread.hpp. Hence, the scoped_ptr is not defined in the 42 | gscam_nodelet header. After scanning quickly through the release 43 | notes of version 1.57, the boost bug tracker and a few changesets, 44 | I could not find not a hint what has changed in the thread library 45 | that gscam must include scoped_ptr itself. 46 | This commit simply addresses the compiler error by explicitly 47 | adding boost's scoped_ptr header in the gscam_nodelet header. 48 | As this commit also compiles with boost version 1.56, the commit 49 | is not expected to cause any problems with other build 50 | configurations. 51 | Signed-off-by: Lukas Bulwahn 52 | * Remove dependency on opencv2 to fix under indigo 53 | Packages can no longer depend on opencv2 as of indigo. 54 | I've updated the package to depend instead on cv_bridge as suggested by http://wiki.ros.org/indigo/Migration#OpenCV. 55 | * Install the parameters file refered to in v4l.launch 56 | * Update package.xml 57 | * Examples: Added example for OSX (`#15 `_) 58 | Add a simple launch configuration for OSX. The camera can be selected by 59 | changing the default="0" to the appropriate integer. 60 | * Adding libraries to gscam target 61 | Fixes `#13 `_, now builds on stricter linkers 62 | * adding proper depends to catkin package call 63 | * Update minoru.launch 64 | * Contributors: CHILI Demo Corner, Jonathan Bohren, Kei Okada, Kenn Sebesta, Lukas Bulwahn, Russell Toris, Lukas Bulwahn 65 | 66 | 0.1.3 (2013-12-19) 67 | ------------------ 68 | * Removed special characters from changelog. 69 | 70 | 0.1.2 (2013-12-19) 71 | ------------------ 72 | * Added install targets for headers 73 | * Adding note on blackmagic decklink cards 74 | * Make sure nodelets are usable 75 | * Add jpeg direct publishing without decoding 76 | * Added system to use GST timestamps 77 | * Valid test file for jpeg-based publisher 78 | * Prepared for jpeg-only subscription 79 | * Install some launch files + added nodelet pipeline demo 80 | * Added missing bits to install the nodelet 81 | * Support for gray (mono8) cameras. 82 | * Remove unused ``bpp_`` member. 83 | * Expose default settings for ``image_encodings`` to the ros master. 84 | * Add in support for mono cameras. 85 | * Contributors: Cedric Pradalier, Holger Rapp, Jonathan Bohren, Russell Toris, Severin Lemaignan 86 | 87 | 0.1.1 (2013-05-30) 88 | ------------------ 89 | * adding missing nodelet dep 90 | * Contributors: Jonathan Bohren 91 | 92 | 0.1.0 (2013-05-28) 93 | ------------------ 94 | * adding maintainer/authors 95 | * making node name backwards compatible 96 | * re-adding package.xml 97 | * more info spam 98 | * removing old camera parameters file 99 | * updating gscam to use ``camera_info_manager`` 100 | * Fixing nodelet, adding example 101 | * Making gscam a node and nodeelt 102 | * adding a note on the videofile player and wrapping readme 103 | * adding option to reopen a stream on EOF and adding a videofile example 104 | * Hybrid catkin-rosbuild buildsystem 105 | * adding minoru example 106 | * putting example nodes into namespaces, adding correct error check in gscam source, making tf frame publishing optional 107 | * rst->md 108 | * making gscam conform to standard ``camera_drivers`` ROS API, note, still need to add polled mode 109 | * fixes for decklink capture, adding another example 110 | * can't have manifest.xml and package.xml in same directory 111 | * removing unneeded find-pkgs 112 | * building in catkin ws 113 | * hybrid rosbuild/catkin buildsystem 114 | * Adding changes that were made to the distribution branch that 115 | should have gone into the exerpeimental branch in r2862. 116 | Added a bunch of enhancements and fixed bugs involving data 117 | missing fromthe image message headers. 118 | Index: src/gscam.cpp 119 | =================================================================== 120 | Added ``camera_name`` and ``camera_parameters_file`` globals for camera 121 | info. 122 | Moved ros init to the top of the main function. 123 | Gets the gstreamer configuration either from environment variable 124 | ``GSCAM_CONFIG`` or ROS param ``~/gscam_config``. 125 | Gets the camera calibration parameters from the file located at ROS 126 | param ``~/camera_parameters_file``, will look at 127 | "../camera_settings.txt" by default. 128 | A bunch of re-indenting for consistency. 129 | Updated a lot of error fprintfs to ``ROS_ERROR`` calls. 130 | Gets the TF ``frame_id`` from the ROS param ``~/frame_id``, can be over- 131 | written by camera parameters. 132 | Now sets the appropriate ROS timestap in the image message header. 133 | Now sets the appropriate TF frame in the image message header. 134 | Added more detailed info/error/warn messages. 135 | Modified the warning / segfault avoidance added to experimental in 136 | r2756. Instead of skipping the frame, it just copies only the 137 | amount of data that it was received, and reports the warning each 138 | time, instead of just once. In a large scale system with lots of 139 | messages, a single warning might easily get lost in the noise. 140 | Index: examples/webcam_parameters.txt 141 | =================================================================== 142 | Added example camera parameters (uncalibrated) for a laptop webcam. 143 | Index: examples/webcam.launch 144 | =================================================================== 145 | Added a launchfile that makes use of the new rosparam options and 146 | TF frame. 147 | * avoid segfault when buffer size is too small 148 | * ROSProcessingjs clean-up 149 | * makefile so rosmake is more reliable 150 | * gscam build tweak for oneiric 151 | * fixes for Natty build per Willow request 152 | * stop node on EOS 153 | * File support courtesy of John Hoare of the University of Tennesse at Knoxville 154 | * more conservative license policy 155 | * fps workaround 156 | * ding gscam 157 | * back to before 158 | * two publishers 159 | * Lots of changes. AR Alpha now expects files in the bin directory, to facilitate roslaunch. Gscam must be started from the bin directory, or, again, using roslaunch. The localizer code now works correctly and has been tested on a Create, but has problems cause by AR alpha's processing delays. 160 | * Bugfix: supply default camera parameters when real ones are unavailable. 161 | * Fully-functional calibration file writing. 162 | * Partial changes for file-writing gscam. 163 | * Gscam now fits into an image processing pipeline with rectified images. TODO: Save camera configuration info. 164 | * Handles built for camera info services, but no testing. 165 | * Changed the name of the GStreamer camera package. probe will henceforth be known as gscam. 166 | * Contributors: Jonathan Bohren, chriscrick, evan.exe@gmail.com, nevernim@gmail.com, trevorjay 167 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(gscam) 4 | 5 | # System Dependencies 6 | find_package(PkgConfig) 7 | 8 | pkg_check_modules(GSTREAMER QUIET gstreamer-0.10) 9 | if(NOT GSTREAMER_FOUND) 10 | set(GSTREAMER_VERSION_1_x TRUE) 11 | endif() 12 | if(GSTREAMER_VERSION_1_x) 13 | message(STATUS "gst 1.0") 14 | pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0) 15 | pkg_check_modules(GST_APP REQUIRED gstreamer-app-1.0) 16 | else() 17 | message(STATUS "gst 0.1") 18 | pkg_check_modules(GSTREAMER REQUIRED gstreamer-0.10) 19 | pkg_check_modules(GST_APP REQUIRED gstreamer-app-0.10) 20 | endif() 21 | 22 | if(USE_ROSBUILD) 23 | # Use rosbuild 24 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 25 | 26 | rosbuild_init() 27 | 28 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 29 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 30 | 31 | include_directories(${GLIB_INCLUDE_DIRS} ${GST_APP_INCLUDE_DIRS}) 32 | 33 | rosbuild_add_library(gscam src/gscam.cpp) 34 | target_link_libraries(gscam 35 | ${GSTREAMER_LIBRARIES} 36 | ${GST_APP_LIBRARIES}) 37 | 38 | rosbuild_add_executable(gscam_node src/gscam_node.cpp) 39 | target_link_libraries(gscam_node gscam 40 | ${GSTREAMER_LIBRARIES} 41 | ${GST_APP_LIBRARIES}) 42 | set_target_properties(gscam_node PROPERTIES OUTPUT_NAME gscam) 43 | 44 | rosbuild_add_library(GSCamNodelet src/gscam_nodelet.cpp) 45 | target_link_libraries(GSCamNodelet gscam 46 | ${GSTREAMER_LIBRARIES} 47 | ${GST_APP_LIBRARIES}) 48 | 49 | else() 50 | # Use Catkin 51 | find_package(catkin REQUIRED 52 | COMPONENTS roscpp image_transport sensor_msgs nodelet 53 | camera_calibration_parsers camera_info_manager 54 | ) 55 | 56 | catkin_package( 57 | INCLUDE_DIRS include 58 | LIBRARIES gscam 59 | CATKIN_DEPENDS roscpp nodelet image_transport sensor_msgs 60 | camera_calibration_parsers camera_info_manager 61 | DEPENDS GSTREAMER GST_APP 62 | ) 63 | 64 | include_directories( 65 | include 66 | ${catkin_INCLUDE_DIRS} 67 | ${GLIB_INCLUDE_DIRS} 68 | ${GST_APP_INCLUDE_DIRS}) 69 | 70 | add_library(gscam src/gscam.cpp) 71 | target_link_libraries(gscam 72 | ${catkin_LIBRARIES} 73 | ${GSTREAMER_LIBRARIES} 74 | ${GST_APP_LIBRARIES}) 75 | 76 | add_executable(gscam_node src/gscam_node.cpp) 77 | target_link_libraries(gscam_node gscam 78 | ${catkin_LIBRARIES} 79 | ${GSTREAMER_LIBRARIES} 80 | ${GST_APP_LIBRARIES}) 81 | set_target_properties(gscam_node PROPERTIES OUTPUT_NAME gscam) 82 | 83 | add_library(GSCamNodelet src/gscam_nodelet.cpp) 84 | target_link_libraries(GSCamNodelet gscam 85 | ${catkin_LIBRARIES} 86 | ${GSTREAMER_LIBRARIES} 87 | ${GST_APP_LIBRARIES}) 88 | 89 | # Install directives 90 | 91 | install(TARGETS gscam gscam_node GSCamNodelet 92 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | ) 96 | 97 | install(DIRECTORY include/${PROJECT_NAME}/ 98 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 99 | PATTERN ".svn" EXCLUDE) 100 | 101 | install(FILES nodelet_plugins.xml 102 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 103 | ) 104 | 105 | install(FILES 106 | examples/v4l.launch 107 | examples/gscam_nodelet.launch 108 | examples/nodelet_pipeline.launch 109 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 110 | ) 111 | 112 | install(FILES examples/uncalibrated_parameters.ini 113 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples 114 | ) 115 | 116 | # Interim compatibility 117 | # Remove this in the next release 118 | install(FILES ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node 119 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 120 | set(EXECUTABLE_OUTPUT_PATH ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}) 121 | endif() 122 | 123 | # Interim compatibility 124 | # Remove this in the next distribution release 125 | configure_file(scripts/gscam_node.in ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node) 126 | file(COPY ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node 127 | DESTINATION ${EXECUTABLE_OUTPUT_PATH} 128 | FILE_PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE) 129 | 130 | 131 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1 2 | include $(shell rospack find mk)/cmake.mk 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | GSCam [![Build Status](https://travis-ci.org/ros-drivers/gscam.svg?branch=master)](https://travis-ci.org/ros-drivers/gscam) 2 | =========================================================================================================================== 3 | 4 | This is a ROS package originally developed by the [Brown Robotics 5 | Lab](http://robotics.cs.brown.edu/) for broadcasting any 6 | [GStreamer](http://gstreamer.freedesktop.org/)-based video stream via the 7 | standard [ROS Camera API](http://ros.org/wiki/camera_drivers). This fork has 8 | several fixes incorporated into it to make it broadcast correct 9 | `sensor_msgs/Image` messages with proper frames and timestamps. It also allows 10 | for more ROS-like configuration and more control over the GStreamer interface. 11 | 12 | Note that this pacakge can be built both in a rosbuild and catkin workspaces. 13 | 14 | GStreamer Library Support 15 | ------------------------- 16 | 17 | gscam supports the following versions of GStreamer 18 | 19 | ### 0.1.x: _Default_ 20 | 21 | Install dependencies via `rosdep`. 22 | 23 | ### 1.0.x: Experimental 24 | 25 | #### Dependencies: 26 | 27 | * gstreamer1.0-tools 28 | * libgstreamer1.0-dev 29 | * libgstreamer-plugins-base1.0-dev 30 | * libgstreamer-plugins-good1.0-dev 31 | 32 | Ubuntu Install: 33 | 34 | ##### 12.04 35 | 36 | ```sh 37 | sudo add-apt-repository ppa:gstreamer-developers/ppa 38 | sudo apt-get install gstreamer1.0-tools libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev 39 | ``` 40 | 41 | ##### 14.04 42 | 43 | ```sh 44 | sudo apt-get install gstreamer1.0-tools libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev 45 | ``` 46 | 47 | #### Usage: 48 | * Use the CMake flag `-DGSTREAMER_VERSION_1_x=On` when building 49 | * See the [Video4Linux2 launchfile example](examples/v4l.launch) for 50 | an example of the differences in the GStreamer config lines 51 | 52 | #### Notes: 53 | * This has been tested with `v4l2src` 54 | 55 | ROS API (stable) 56 | ---------------- 57 | 58 | ### gscam 59 | 60 | This can be run as both a node and a nodelet. 61 | 62 | #### Nodes 63 | * `gscam` 64 | 65 | #### Topics 66 | * `camera/image_raw` 67 | * `camera/camera_info` 68 | 69 | #### Services 70 | * `camera/set_camera_info` 71 | 72 | #### Parameters 73 | * `~camera_name`: The name of the camera (corrsponding to the camera info) 74 | * `~camera_info_url`: A url (`file://path/to/file`, `package://pkg_name/path/to/file`) to the [camera calibration file](http://www.ros.org/wiki/camera_calibration_parsers#File_formats). 75 | * `~gscam_config`: The GStreamer [configuration string](http://wiki.oz9aec.net/index.php?title=Gstreamer_cheat_sheet&oldid=1829). 76 | * `~frame_id`: The [TF](http://www.ros.org/wiki/tf) frame ID. 77 | * `~reopen_on_eof`: Re-open the stream if it ends (EOF). 78 | * `~sync_sink`: Synchronize the app sink (sometimes setting this to `false` can resolve problems with sub-par framerates). 79 | 80 | C++ API (unstable) 81 | ------------------ 82 | 83 | The gscam c++ library can be used, but it is not guaranteed to be stable. 84 | 85 | Examples 86 | -------- 87 | 88 | See example launchfiles and configs in the examples directory. Currently there 89 | are examples for: 90 | 91 | * [Video4Linux2](examples/v4l.launch): Standard 92 | [video4linux](http://en.wikipedia.org/wiki/Video4Linux)-based cameras like 93 | USB webcams. 94 | * ***GST-1.0:*** Use the roslaunch argument `GST10:=True` for GStreamer 1.0 variant 95 | * [Nodelet](examples/gscam_nodelet.launch): Run a V4L-based camera in a nodelet 96 | * [Video File](examples/videofile.launch): Any videofile readable by GStreamer 97 | * [DeckLink](examples/decklink.launch): 98 | [BlackMagic](http://www.blackmagicdesign.com/products/decklink/models) 99 | DeckLink SDI capture cards (note: this requires the `gst-plugins-bad` plugins) 100 | -------------------------------------------------------------------------------- /examples/decklink.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /examples/gscam_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/minoru.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /examples/nodelet_pipeline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /examples/osx.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/uncalibrated_parameters.ini: -------------------------------------------------------------------------------- 1 | # Camera intrinsics 2 | 3 | [image] 4 | 5 | width 6 | 320 7 | 8 | height 9 | 240 10 | 11 | [default] 12 | 13 | camera matrix 14 | 467.14110 0.00000 158.56653 15 | 0.00000 465.42608 131.37432 16 | 0.00000 0.00000 1.00000 17 | 18 | distortion 19 | -0.16999 0.31931 0.00929 0.00040 0.00000 20 | 21 | 22 | rectification 23 | 1.00000 0.00000 0.00000 24 | 0.00000 1.00000 0.00000 25 | 0.00000 0.00000 1.00000 26 | 27 | projection 28 | 458.62505 0.00000 158.11160 0.00000 29 | 0.00000 457.90904 131.91881 0.00000 30 | 0.00000 0.00000 1.00000 0.00000 31 | -------------------------------------------------------------------------------- /examples/v4l.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /examples/v4ljpeg.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/videofile.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /include/gscam/gscam.h: -------------------------------------------------------------------------------- 1 | #ifndef __GSCAM_GSCAM_H 2 | #define __GSCAM_GSCAM_H 3 | 4 | extern "C"{ 5 | #include 6 | #include 7 | } 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace gscam { 21 | 22 | class GSCam { 23 | public: 24 | GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private); 25 | ~GSCam(); 26 | 27 | bool configure(); 28 | bool init_stream(); 29 | void publish_stream(); 30 | void cleanup_stream(); 31 | 32 | void run(); 33 | 34 | private: 35 | // General gstreamer configuration 36 | std::string gsconfig_; 37 | 38 | // Gstreamer structures 39 | GstElement *pipeline_; 40 | GstElement *sink_; 41 | 42 | // Appsink configuration 43 | bool sync_sink_; 44 | bool preroll_; 45 | bool reopen_on_eof_; 46 | bool use_gst_timestamps_; 47 | 48 | // Camera publisher configuration 49 | std::string frame_id_; 50 | int width_, height_; 51 | std::string image_encoding_; 52 | std::string camera_name_; 53 | std::string camera_info_url_; 54 | 55 | // ROS Inteface 56 | // Calibration between ros::Time and gst timestamps 57 | double time_offset_; 58 | ros::NodeHandle nh_, nh_private_; 59 | image_transport::ImageTransport image_transport_; 60 | camera_info_manager::CameraInfoManager camera_info_manager_; 61 | image_transport::CameraPublisher camera_pub_; 62 | // Case of a jpeg only publisher 63 | ros::Publisher jpeg_pub_; 64 | ros::Publisher cinfo_pub_; 65 | }; 66 | 67 | } 68 | 69 | #endif // ifndef __GSCAM_GSCAM_H 70 | -------------------------------------------------------------------------------- /include/gscam/gscam_nodelet.h: -------------------------------------------------------------------------------- 1 | #ifndef __GSCAM_GSCAM_NODELET_H 2 | #define __GSCAM_GSCAM_NODELET_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace gscam { 12 | class GSCamNodelet : public nodelet::Nodelet 13 | { 14 | public: 15 | GSCamNodelet(); 16 | ~GSCamNodelet(); 17 | 18 | virtual void onInit(); 19 | 20 | private: 21 | boost::scoped_ptr gscam_driver_; 22 | boost::scoped_ptr stream_thread_; 23 | }; 24 | } 25 | 26 | #endif // infdef __GSCAM_GSCAM_NODELET_H 27 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | Nodelet for publishing a ROS camera interface from a GStreamer stream. 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | gscam 3 | 1.0.1 4 | 5 | A ROS camera driver that uses gstreamer to connect to 6 | devices such as webcams. 7 | 8 | Jonathan Bohren 9 | ROS Orphaned Package Maintainers 10 | BSD 11 | 12 | Jonathan Bohren 13 | Graylin Trevor Jay 14 | Christopher Crick 15 | 16 | catkin 17 | 18 | libgstreamer1.0-dev 19 | libgstreamer-plugins-base1.0-dev 20 | 21 | nodelet 22 | cv_bridge 23 | roscpp 24 | image_transport 25 | sensor_msgs 26 | camera_calibration_parsers 27 | camera_info_manager 28 | 29 | nodelet 30 | cv_bridge 31 | roscpp 32 | image_transport 33 | sensor_msgs 34 | camera_calibration_parsers 35 | camera_info_manager 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /scripts/gscam_node.in: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo '[WARNING] gscam_node is an interim dependency and will be removed in the next release! Please change all launchfiles back to use "gscam" instead of "gscam_node"' 4 | 5 | ${EXECUTABLE_OUTPUT_PATH}/gscam 6 | -------------------------------------------------------------------------------- /src/gscam.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | #include 9 | extern "C"{ 10 | #include 11 | #include 12 | } 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace gscam { 31 | 32 | GSCam::GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private) : 33 | gsconfig_(""), 34 | pipeline_(NULL), 35 | sink_(NULL), 36 | nh_(nh_camera), 37 | nh_private_(nh_private), 38 | image_transport_(nh_camera), 39 | camera_info_manager_(nh_camera) 40 | { 41 | } 42 | 43 | GSCam::~GSCam() 44 | { 45 | } 46 | 47 | bool GSCam::configure() 48 | { 49 | // Get gstreamer configuration 50 | // (either from environment variable or ROS param) 51 | std::string gsconfig_rosparam = ""; 52 | bool gsconfig_rosparam_defined = false; 53 | char *gsconfig_env = NULL; 54 | 55 | gsconfig_rosparam_defined = nh_private_.getParam("gscam_config",gsconfig_rosparam); 56 | gsconfig_env = getenv("GSCAM_CONFIG"); 57 | 58 | if (!gsconfig_env && !gsconfig_rosparam_defined) { 59 | ROS_FATAL( "Problem getting GSCAM_CONFIG environment variable and 'gscam_config' rosparam is not set. This is needed to set up a gstreamer pipeline." ); 60 | return false; 61 | } else if(gsconfig_env && gsconfig_rosparam_defined) { 62 | ROS_FATAL( "Both GSCAM_CONFIG environment variable and 'gscam_config' rosparam are set. Please only define one." ); 63 | return false; 64 | } else if(gsconfig_env) { 65 | gsconfig_ = gsconfig_env; 66 | ROS_INFO_STREAM("Using gstreamer config from env: \""<message ); 125 | return false; 126 | } 127 | 128 | // Create RGB sink 129 | sink_ = gst_element_factory_make("appsink",NULL); 130 | GstCaps * caps = gst_app_sink_get_caps(GST_APP_SINK(sink_)); 131 | 132 | #if (GST_VERSION_MAJOR == 1) 133 | // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html 134 | if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { 135 | caps = gst_caps_new_simple( "video/x-raw", 136 | "format", G_TYPE_STRING, "RGB", 137 | NULL); 138 | } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) { 139 | caps = gst_caps_new_simple( "video/x-raw", 140 | "format", G_TYPE_STRING, "GRAY8", 141 | NULL); 142 | } else if (image_encoding_ == "jpeg") { 143 | caps = gst_caps_new_simple("image/jpeg", NULL, NULL); 144 | } 145 | #else 146 | if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { 147 | caps = gst_caps_new_simple( "video/x-raw-rgb", NULL,NULL); 148 | } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) { 149 | caps = gst_caps_new_simple("video/x-raw-gray", NULL, NULL); 150 | } else if (image_encoding_ == "jpeg") { 151 | caps = gst_caps_new_simple("image/jpeg", NULL, NULL); 152 | } 153 | #endif 154 | 155 | gst_app_sink_set_caps(GST_APP_SINK(sink_), caps); 156 | gst_caps_unref(caps); 157 | 158 | // Set whether the sink should sync 159 | // Sometimes setting this to true can cause a large number of frames to be 160 | // dropped 161 | gst_base_sink_set_sync( 162 | GST_BASE_SINK(sink_), 163 | (sync_sink_) ? TRUE : FALSE); 164 | 165 | if(GST_IS_PIPELINE(pipeline_)) { 166 | GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC); 167 | g_assert(outpad); 168 | 169 | GstElement *outelement = gst_pad_get_parent_element(outpad); 170 | g_assert(outelement); 171 | gst_object_unref(outpad); 172 | 173 | if(!gst_bin_add(GST_BIN(pipeline_), sink_)) { 174 | ROS_FATAL("gst_bin_add() failed"); 175 | gst_object_unref(outelement); 176 | gst_object_unref(pipeline_); 177 | return false; 178 | } 179 | 180 | if(!gst_element_link(outelement, sink_)) { 181 | ROS_FATAL("GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement)); 182 | gst_object_unref(outelement); 183 | gst_object_unref(pipeline_); 184 | return false; 185 | } 186 | 187 | gst_object_unref(outelement); 188 | } else { 189 | GstElement* launchpipe = pipeline_; 190 | pipeline_ = gst_pipeline_new(NULL); 191 | g_assert(pipeline_); 192 | 193 | gst_object_unparent(GST_OBJECT(launchpipe)); 194 | 195 | gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, NULL); 196 | 197 | if(!gst_element_link(launchpipe, sink_)) { 198 | ROS_FATAL("GStreamer: cannot link launchpipe -> sink"); 199 | gst_object_unref(pipeline_); 200 | return false; 201 | } 202 | } 203 | 204 | // Calibration between ros::Time and gst timestamps 205 | GstClock * clock = gst_system_clock_obtain(); 206 | ros::Time now = ros::Time::now(); 207 | GstClockTime ct = gst_clock_get_time(clock); 208 | gst_object_unref(clock); 209 | time_offset_ = now.toSec() - GST_TIME_AS_USECONDS(ct)/1e6; 210 | ROS_INFO("Time offset: %.3f",time_offset_); 211 | 212 | gst_element_set_state(pipeline_, GST_STATE_PAUSED); 213 | 214 | if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 215 | ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration."); 216 | return false; 217 | } else { 218 | ROS_DEBUG_STREAM("Stream is PAUSED."); 219 | } 220 | 221 | // Create ROS camera interface 222 | if (image_encoding_ == "jpeg") { 223 | jpeg_pub_ = nh_.advertise("camera/image_raw/compressed",1); 224 | cinfo_pub_ = nh_.advertise("camera/camera_info",1); 225 | } else { 226 | camera_pub_ = image_transport_.advertiseCamera("camera/image_raw", 1); 227 | } 228 | 229 | return true; 230 | } 231 | 232 | void GSCam::publish_stream() 233 | { 234 | ROS_INFO_STREAM("Publishing stream..."); 235 | 236 | // Pre-roll camera if needed 237 | if (preroll_) { 238 | ROS_DEBUG("Performing preroll..."); 239 | 240 | //The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll 241 | //I am told this is needed and am erring on the side of caution. 242 | gst_element_set_state(pipeline_, GST_STATE_PLAYING); 243 | if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 244 | ROS_ERROR("Failed to PLAY during preroll."); 245 | return; 246 | } else { 247 | ROS_DEBUG("Stream is PLAYING in preroll."); 248 | } 249 | 250 | gst_element_set_state(pipeline_, GST_STATE_PAUSED); 251 | if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) { 252 | ROS_ERROR("Failed to PAUSE."); 253 | return; 254 | } else { 255 | ROS_INFO("Stream is PAUSED in preroll."); 256 | } 257 | } 258 | 259 | if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { 260 | ROS_ERROR("Could not start stream!"); 261 | return; 262 | } 263 | ROS_INFO("Started stream."); 264 | 265 | // Poll the data as fast a spossible 266 | while(ros::ok()) 267 | { 268 | // This should block until a new frame is awake, this way, we'll run at the 269 | // actual capture framerate of the device. 270 | // ROS_DEBUG("Getting data..."); 271 | #if (GST_VERSION_MAJOR == 1) 272 | GstSample* sample = gst_app_sink_pull_sample(GST_APP_SINK(sink_)); 273 | if(!sample) { 274 | ROS_ERROR("Could not get gstreamer sample."); 275 | break; 276 | } 277 | GstBuffer* buf = gst_sample_get_buffer(sample); 278 | GstMemory *memory = gst_buffer_get_memory(buf, 0); 279 | GstMapInfo info; 280 | 281 | gst_memory_map(memory, &info, GST_MAP_READ); 282 | gsize &buf_size = info.size; 283 | guint8* &buf_data = info.data; 284 | #else 285 | GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_)); 286 | guint &buf_size = buf->size; 287 | guint8* &buf_data = buf->data; 288 | #endif 289 | GstClockTime bt = gst_element_get_base_time(pipeline_); 290 | // ROS_INFO("New buffer: timestamp %.6f %lu %lu %.3f", 291 | // GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_); 292 | 293 | 294 | #if 0 295 | GstFormat fmt = GST_FORMAT_TIME; 296 | gint64 current = -1; 297 | 298 | Query the current position of the stream 299 | if (gst_element_query_position(pipeline_, &fmt, ¤t)) { 300 | ROS_INFO_STREAM("Position "<header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->pts+bt)/1e6+time_offset_); 330 | #else 331 | cinfo->header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_); 332 | #endif 333 | } else { 334 | cinfo->header.stamp = ros::Time::now(); 335 | } 336 | // ROS_INFO("Image time stamp: %.3f",cinfo->header.stamp.toSec()); 337 | cinfo->header.frame_id = frame_id_; 338 | if (image_encoding_ == "jpeg") { 339 | sensor_msgs::CompressedImagePtr img(new sensor_msgs::CompressedImage()); 340 | img->header = cinfo->header; 341 | img->format = "jpeg"; 342 | img->data.resize(buf_size); 343 | std::copy(buf_data, (buf_data)+(buf_size), 344 | img->data.begin()); 345 | jpeg_pub_.publish(img); 346 | cinfo_pub_.publish(cinfo); 347 | } else { 348 | // Complain if the returned buffer is smaller than we expect 349 | const unsigned int expected_frame_size = 350 | image_encoding_ == sensor_msgs::image_encodings::RGB8 351 | ? width_ * height_ * 3 352 | : width_ * height_; 353 | 354 | if (buf_size < expected_frame_size) { 355 | ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be " 356 | << expected_frame_size << " bytes but got only " 357 | << (buf_size) << " bytes. (make sure frames are correctly encoded)"); 358 | } 359 | 360 | // Construct Image message 361 | sensor_msgs::ImagePtr img(new sensor_msgs::Image()); 362 | 363 | img->header = cinfo->header; 364 | 365 | // Image data and metadata 366 | img->width = width_; 367 | img->height = height_; 368 | img->encoding = image_encoding_; 369 | img->is_bigendian = false; 370 | img->data.resize(expected_frame_size); 371 | 372 | // Copy only the data we received 373 | // Since we're publishing shared pointers, we need to copy the image so 374 | // we can free the buffer allocated by gstreamer 375 | if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { 376 | img->step = width_ * 3; 377 | } else { 378 | img->step = width_; 379 | } 380 | std::copy( 381 | buf_data, 382 | (buf_data)+(buf_size), 383 | img->data.begin()); 384 | 385 | // Publish the image/info 386 | camera_pub_.publish(img, cinfo); 387 | } 388 | 389 | // Release the buffer 390 | if(buf) { 391 | #if (GST_VERSION_MAJOR == 1) 392 | gst_memory_unmap(memory, &info); 393 | gst_memory_unref(memory); 394 | #endif 395 | gst_buffer_unref(buf); 396 | } 397 | 398 | ros::spinOnce(); 399 | } 400 | } 401 | 402 | void GSCam::cleanup_stream() 403 | { 404 | // Clean up 405 | ROS_INFO("Stopping gstreamer pipeline..."); 406 | if(pipeline_) { 407 | gst_element_set_state(pipeline_, GST_STATE_NULL); 408 | gst_object_unref(pipeline_); 409 | pipeline_ = NULL; 410 | } 411 | } 412 | 413 | void GSCam::run() { 414 | while(ros::ok()) { 415 | if(!this->configure()) { 416 | ROS_FATAL("Failed to configure gscam!"); 417 | break; 418 | } 419 | 420 | if(!this->init_stream()) { 421 | ROS_FATAL("Failed to initialize gscam stream!"); 422 | break; 423 | } 424 | 425 | // Block while publishing 426 | this->publish_stream(); 427 | 428 | this->cleanup_stream(); 429 | 430 | ROS_INFO("GStreamer stream stopped!"); 431 | 432 | if(reopen_on_eof_) { 433 | ROS_INFO("Reopening stream..."); 434 | } else { 435 | ROS_INFO("Cleaning up stream and exiting..."); 436 | break; 437 | } 438 | } 439 | 440 | } 441 | 442 | // Example callbacks for appsink 443 | // TODO: enable callback-based capture 444 | void gst_eos_cb(GstAppSink *appsink, gpointer user_data ) { 445 | } 446 | GstFlowReturn gst_new_preroll_cb(GstAppSink *appsink, gpointer user_data ) { 447 | return GST_FLOW_OK; 448 | } 449 | GstFlowReturn gst_new_asample_cb(GstAppSink *appsink, gpointer user_data ) { 450 | return GST_FLOW_OK; 451 | } 452 | 453 | } 454 | -------------------------------------------------------------------------------- /src/gscam_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | ros::init(argc, argv, "gscam_publisher"); 7 | ros::NodeHandle nh, nh_private("~"); 8 | 9 | gscam::GSCam gscam_driver(nh, nh_private); 10 | gscam_driver.run(); 11 | 12 | return 0; 13 | } 14 | 15 | -------------------------------------------------------------------------------- /src/gscam_nodelet.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | PLUGINLIB_EXPORT_CLASS(gscam::GSCamNodelet, nodelet::Nodelet) 8 | 9 | namespace gscam { 10 | GSCamNodelet::GSCamNodelet() : 11 | nodelet::Nodelet(), 12 | gscam_driver_(NULL), 13 | stream_thread_(NULL) 14 | { 15 | } 16 | 17 | GSCamNodelet::~GSCamNodelet() 18 | { 19 | stream_thread_->join(); 20 | } 21 | 22 | void GSCamNodelet::onInit() 23 | { 24 | gscam_driver_.reset(new gscam::GSCam(this->getNodeHandle(), this->getPrivateNodeHandle())); 25 | stream_thread_.reset(new boost::thread(boost::bind(&GSCam::run, gscam_driver_.get()))); 26 | } 27 | } 28 | --------------------------------------------------------------------------------