caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added rover catkin packages

Browse Source
This commit is contained in:
Corwin Perren
2017-12-03 13:46:55 -08:00
parent 9cbec88ebe
commit 08a7add471
55 changed files with 51834 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
rover/drive_test/CMakeLists.txt Normal file
View File

@@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 2.8.3)
project(drive_test)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
stage_ros
geometry_msgs
roscpp
rospy
)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
)

BIN
rover/drive_test/HW2.pdf Normal file
View File

Binary file not shown.

0
rover/drive_test/ReadMe Normal file
View File

7
rover/drive_test/launch/drive_test.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="stage_ros" type="stageros" name="simulator" args="$(find drive_test)/worlds/manyDots.world"/>
<node pkg="drive_test" type="drive_test.py" name="hw2" output="screen">
<param name="goalX" value="20" />
<param name="goalY" value="-20" />
</node>
</launch>

58
rover/drive_test/package.xml Normal file
View File

@@ -0,0 +1,58 @@
<?xml version="1.0"?>
<package>
<name>drive_test</name>
<version>0.0.0</version>
<description>ROB456 HW2</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="rob456@todo.todo">rob456</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pioneer_delivery</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>stage_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>stage_ros</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

90
rover/drive_test/src/drive_test.py Normal file
View File

@@ -0,0 +1,90 @@
#!/usr/bin/env python
import rospy
import math
import tf
from tf.transformations import euler_from_quaternion
import message_filters
from pprint import pprint
import time
# The laser scan message
from sensor_msgs.msg import Joy
# The odometry message
from nav_msgs.msg import Odometry
# the velocity command message
from geometry_msgs.msg import Twist
# global pi - this may come in handy
pi = math.pi
# method to control the robot
def callback_spacenav(twist_message):
scalar_forward = 100.0
scalar_turn = 2.0
# make a new twist message
command = Twist()
# Fill in the fields. Field values are unspecified
# until they are actually assigned. The Twist message
# holds linear and angular velocities.
command.linear.x = twist_message.linear.x * scalar_forward
command.linear.y = twist_message.linear.y * scalar_forward
command.linear.z = twist_message.linear.z * scalar_forward
command.angular.x = twist_message.angular.x * scalar_forward
command.angular.y = twist_message.angular.y * scalar_forward
command.angular.z = twist_message.angular.z * scalar_turn
# print command
# Send the commands
pub.publish(command)
# method to control the robot
def callback_joy(joy_message):
scalar_forward = 100.0
scalar_turn = 2.0
# make a new twist message
command = Twist()
# Fill in the fields. Field values are unspecified
# until they are actually assigned. The Twist message
# holds linear and angular velocities.
command.linear.x = joy_message.axes[1] * 100
#command.linear.y = twist_message.linear.y * scalar_forward
#command.linear.z = twist_message.linear.z * scalar_forward
#command.angular.x = twist_message.angular.x * scalar_forward
#command.angular.y = twist_message.angular.y * scalar_forward
command.angular.z = joy_message.axes[0] * 1.0
# command.angular.z = joy_message.axes[2] * 1.0
print joy_message
# Send the commands
pub.publish(command)
# main function call
if __name__ == "__main__":
# Initialize the node
rospy.init_node('drive_test', log_level=rospy.DEBUG)
# subscribe to laser scan message
# sub = rospy.Subscriber('/spacenav/twist', Twist, callback_spacenav)
# subscribe to laser scan message
sub = rospy.Subscriber('/joy', Joy, callback_joy)
# synchronize laser scan and odometry data
# ts = message_filters.TimeSynchronizer([sub, sub2], 10)
# ts.registerCallback(callback)
# publish twist message
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Turn control over to ROS
rospy.spin()

48122
rover/drive_test/worlds/manyDots.pgm Normal file
View File

File diff suppressed because it is too large Load Diff

71
rover/drive_test/worlds/manyDots.world Normal file
View File

@@ -0,0 +1,71 @@
define block model
(
size [0.5 0.5 0.5]
gui_nose 0
)
define topurg ranger
(
sensor(
range [ 0.0 30.0 ]
fov 270.25
samples 1081
)
# generic model properties
color "black"
size [ 0.05 0.05 0.1 ]
)
define erratic position
(
size [0.35 0.35 0.25]
origin [-0.05 0 0 0]
gui_nose 0
drive "diff"
topurg(pose [ 0.050 0.000 0 0.000 ])
odom_error [0.00 0.00 0.00 0.00 0.00 0.00]
)
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 1
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
# set the resolution of the underlying raytrace model in meters
resolution 0.005
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 745.000 448.000 ]
rotate [ 0.000 0.000 ]
scale 5
)
# load an environment bitmap
floorplan
(
name "manyDots"
bitmap "manyDots.pgm"
size [54.0 58.7 0.5]
pose [ 0 0.0 0 90.000 ]
)
# throw in a robot
erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])

105
rover/gscam/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,105 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package gscam
^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2013-12-19)
------------------
* Removed special characters from changelog.
0.1.2 (2013-12-19)
------------------
* Added install targets for headers
* Adding note on blackmagic decklink cards
* Make sure nodelets are usable
* Add jpeg direct publishing without decoding
* Added system to use GST timestamps
* Valid test file for jpeg-based publisher
* Prepared for jpeg-only subscription
* Install some launch files + added nodelet pipeline demo
* Added missing bits to install the nodelet
* Support for gray (mono8) cameras.
* Remove unused ``bpp_`` member.
* Expose default settings for ``image_encodings`` to the ros master.
* Add in support for mono cameras.
* Contributors: Cedric Pradalier, Holger Rapp, Jonathan Bohren, Russell Toris, Severin Lemaignan
0.1.1 (2013-05-30)
------------------
* adding missing nodelet dep
* Contributors: Jonathan Bohren
0.1.0 (2013-05-28)
------------------
* adding maintainer/authors
* making node name backwards compatible
* re-adding package.xml
* more info spam
* removing old camera parameters file
* updating gscam to use ``camera_info_manager``
* Fixing nodelet, adding example
* Making gscam a node and nodeelt
* adding a note on the videofile player and wrapping readme
* adding option to reopen a stream on EOF and adding a videofile example
* Hybrid catkin-rosbuild buildsystem
* adding minoru example
* putting example nodes into namespaces, adding correct error check in gscam source, making tf frame publishing optional
* rst->md
* making gscam conform to standard ``camera_drivers`` ROS API, note, still need to add polled mode
* fixes for decklink capture, adding another example
* can't have manifest.xml and package.xml in same directory
* removing unneeded find-pkgs
* building in catkin ws
* hybrid rosbuild/catkin buildsystem
* Adding changes that were made to the distribution branch that
should have gone into the exerpeimental branch in r2862.
Added a bunch of enhancements and fixed bugs involving data
missing fromthe image message headers.
Index: src/gscam.cpp
===================================================================
Added ``camera_name`` and ``camera_parameters_file`` globals for camera
info.
Moved ros init to the top of the main function.
Gets the gstreamer configuration either from environment variable
``GSCAM_CONFIG`` or ROS param ``~/gscam_config``.
Gets the camera calibration parameters from the file located at ROS
param ``~/camera_parameters_file``, will look at
"../camera_settings.txt" by default.
A bunch of re-indenting for consistency.
Updated a lot of error fprintfs to ``ROS_ERROR`` calls.
Gets the TF ``frame_id`` from the ROS param ``~/frame_id``, can be over-
written by camera parameters.
Now sets the appropriate ROS timestap in the image message header.
Now sets the appropriate TF frame in the image message header.
Added more detailed info/error/warn messages.
Modified the warning / segfault avoidance added to experimental in
r2756. Instead of skipping the frame, it just copies only the
amount of data that it was received, and reports the warning each
time, instead of just once. In a large scale system with lots of
messages, a single warning might easily get lost in the noise.
Index: examples/webcam_parameters.txt
===================================================================
Added example camera parameters (uncalibrated) for a laptop webcam.
Index: examples/webcam.launch
===================================================================
Added a launchfile that makes use of the new rosparam options and
TF frame.
* avoid segfault when buffer size is too small
* ROSProcessingjs clean-up
* makefile so rosmake is more reliable
* gscam build tweak for oneiric
* fixes for Natty build per Willow request
* stop node on EOS
* File support courtesy of John Hoare of the University of Tennesse at Knoxville
* more conservative license policy
* fps workaround
* ding gscam
* back to before
* two publishers
* 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.
* Bugfix: supply default camera parameters when real ones are unavailable.
* Fully-functional calibration file writing.
* Partial changes for file-writing gscam.
* Gscam now fits into an image processing pipeline with rectified images. TODO: Save camera configuration info.
* Handles built for camera info services, but no testing.
* Changed the name of the GStreamer camera package. probe will henceforth be known as gscam.
* Contributors: Jonathan Bohren, chriscrick, evan.exe@gmail.com, nevernim@gmail.com, trevorjay

118
rover/gscam/CMakeLists.txt Normal file
View File

@@ -0,0 +1,118 @@
cmake_minimum_required(VERSION 2.8.3)
project(gscam)
# System Dependencies
find_package(PkgConfig)
pkg_check_modules(GSTREAMER REQUIRED gstreamer-0.10)
pkg_check_modules(GST_APP REQUIRED gstreamer-app-0.10)
if(USE_ROSBUILD)
# Use rosbuild
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
include_directories(${GLIB_INCLUDE_DIRS} ${GST_APP_INCLUDE_DIRS})
rosbuild_add_library(gscam src/gscam.cpp)
target_link_libraries(gscam
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
rosbuild_add_executable(gscam_node src/gscam_node.cpp)
target_link_libraries(gscam_node gscam
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
set_target_properties(gscam_node PROPERTIES OUTPUT_NAME gscam)
rosbuild_add_library(GSCamNodelet src/gscam_nodelet.cpp)
target_link_libraries(GSCamNodelet gscam
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
else()
# Use Catkin
find_package(catkin REQUIRED
COMPONENTS roscpp image_transport sensor_msgs nodelet
camera_calibration_parsers camera_info_manager
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES gscam
CATKIN_DEPENDS roscpp nodelet image_transport sensor_msgs
camera_calibration_parsers camera_info_manager
DEPENDS GSTREAMER GST_APP
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${GLIB_INCLUDE_DIRS}
${GST_APP_INCLUDE_DIRS})
add_library(gscam src/gscam.cpp)
target_link_libraries(gscam
${catkin_LIBRARIES}
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
add_executable(gscam_node src/gscam_node.cpp)
target_link_libraries(gscam_node gscam
${catkin_LIBRARIES}
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
set_target_properties(gscam_node PROPERTIES OUTPUT_NAME gscam)
add_library(GSCamNodelet src/gscam_nodelet.cpp)
target_link_libraries(GSCamNodelet gscam
${catkin_LIBRARIES}
${GSTREAMER_LIBRARIES}
${GST_APP_LIBRARIES})
# Install directives
install(TARGETS gscam gscam_node GSCamNodelet
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
PATTERN ".svn" EXCLUDE)
install(FILES nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES
examples/v4l.launch
examples/gscam_nodelet.launch
examples/nodelet_pipeline.launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES examples/uncalibrated_parameters.ini
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples
)
# Interim compatibility
# Remove this in the next release
install(FILES ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
set(EXECUTABLE_OUTPUT_PATH ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME})
endif()
# Interim compatibility
# Remove this in the next distribution release
configure_file(scripts/gscam_node.in ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node)
file(COPY ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/gscam_node
DESTINATION ${EXECUTABLE_OUTPUT_PATH}
FILE_PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE)

2
rover/gscam/Makefile Normal file
View File

@@ -0,0 +1,2 @@
EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1
include $(shell rospack find mk)/cmake.mk

56
rover/gscam/README.md Normal file
View File

@@ -0,0 +1,56 @@
GSCam
=====
This is a ROS package originally developed by the [Brown Robotics
Lab](http://robotics.cs.brown.edu/) for broadcasting any
[GStreamer](http://gstreamer.freedesktop.org/)-based video stream via the
standard [ROS Camera API](http://ros.org/wiki/camera_drivers). This fork has
several fixes incorporated into it to make it broadcast correct
`sensor_msgs/Image` messages with proper frames and timestamps. It also allows
for more ROS-like configuration and more control over the GStreamer interface.
Note that this pacakge can be built both in a rosbuild and catkin workspaces.
ROS API (stable)
----------------
### gscam
This can be run as both a node and a nodelet.
#### Nodes
* `gscam`
#### Topics
* `camera/image_raw`
* `camera/camera_info`
#### Services
* `camera/set_camera_info`
#### Parameters
* `~camera_name`: The name of the camera (corrsponding to the camera info)
* `~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).
* `~gscam_config`: The GStreamer [configuration string](http://wiki.oz9aec.net/index.php?title=Gstreamer_cheat_sheet&oldid=1829).
* `~frame_id`: The [TF](http://www.ros.org/wiki/tf) frame ID.
* `~reopen_on_eof`: Re-open the stream if it ends (EOF).
* `~sync_sink`: Synchronize the app sink (sometimes setting this to `false` can resolve problems with sub-par framerates).
C++ API (unstable)
------------------
The gscam c++ library can be used, but it is not guaranteed to be stable.
Examples
--------
See example launchfiles and configs in the examples directory. Currently there
are examples for:
* [Video4Linux2](examples/v4l.launch): Standard
[video4linux](http://en.wikipedia.org/wiki/Video4Linux)-based cameras like
USB webcams
* [Nodelet](examples/gscam_nodelet.launch): Run a V4L-based camera in a nodelet
* [Video File](examples/videofile.launch): Any videofile readable by GStreamer
* [DeckLink](examples/decklink.launch):
[BlackMagic](http://www.blackmagicdesign.com/products/decklink/models)
DeckLink SDI capture cards (note: this requires the `gst-plugins-bad` plugins)

22
rover/gscam/examples/decklink.launch Normal file
View File

@@ -0,0 +1,22 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /decklink/image_raw from the input channel of a BlackMagic
DeckLink SDI capture card-->
<!-- DeckLink config, run $ gst-inspect decklinksrc to see all the options for your card -->
<arg name="MODE" default="ntsc"/>
<arg name="CONNECTION" default="sdi"/>
<arg name="SUBDEVICE" default="0"/>
<arg name="PUBLISH_FRAME" default="false"/>
<node ns="decklink" name="gscam_driver_decklink" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="decklinksrc mode=$(arg MODE) connection=$(arg CONNECTION) subdevice=$(arg SUBDEVICE) ! ffmpegcolorspace "/>
<param name="frame_id" value="/decklink_frame"/>
<!-- This needs to be set to false to avoid dropping tons of frames -->
<param name="sync_sink" value="false"/>
</node>
<node name="decklink_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /decklink_frame 10"/>
</launch>

20
rover/gscam/examples/gscam_nodelet.launch Normal file
View File

@@ -0,0 +1,20 @@
<launch>
<arg name="DEVICE" default="/dev/video0"/>
<!-- The GStreamer framerate needs to be an integral fraction -->
<arg name="FPS" default="30/1"/>
<node pkg="nodelet" type="nodelet"
name="standalone_nodelet" args="manager"
output="screen"/>
<node pkg="nodelet" type="nodelet"
name="GSCamNodelet"
args="load gscam/GSCamNodelet standalone_nodelet"
output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="v4l2src device=$(arg DEVICE) ! video/x-raw-rgb,framerate=$(arg FPS) ! ffmpegcolorspace"/>
<param name="frame_id" value="/v4l_frame"/>
<param name="sync_sink" value="true"/>
</node>
</launch>

39
rover/gscam/examples/minoru.launch Normal file
View File

@@ -0,0 +1,39 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /webcam/image_raw -->
<arg name="LEFT_DEV" default="/dev/video0"/>
<arg name="RIGHT_DEV" default="/dev/video1"/>
<!-- The GStreamer framerate needs to be an integral fraction -->
<!-- Note: Minoru can't push full 640x480 at 30fps -->
<arg name="WIDTH" default="320"/>
<arg name="HEIGHT" default="240"/>
<arg name="FPS" default="30/1"/>
<arg name="BRIGHTNESS" default="0"/>
<arg name="PUBLISH_FRAME" default="false"/>
<!-- Construct the v4l2src format config -->
<arg name="FORMAT" default="video/x-raw-rgb,width=$(arg WIDTH),height=$(arg HEIGHT),framerate=$(arg FPS)"/>
<group ns="minoru">
<node ns="left" name="gscam_driver_v4l" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config"
value="v4l2src device=$(arg LEFT_DEV) brightness=$(arg BRIGHTNESS) ! $(arg FORMAT) ! ffmpegcolorspace"/>
<param name="frame_id" value="/minoru_left"/>
<param name="sync_sink" value="true"/>
</node>
<node ns="right" name="gscam_driver_v4l" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config"
value="v4l2src device=$(arg RIGHT_DEV) brightness=$(arg BRIGHTNESS) ! $(arg FORMAT) ! ffmpegcolorspace"/>
<param name="frame_id" value="/minoru_right"/>
<param name="sync_sink" value="true"/>
</node>
</group>
<node if="$(arg PUBLISH_FRAME)" name="left_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /minoru_left 10"/>
<node if="$(arg PUBLISH_FRAME)" name="right_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /minoru_right 10"/>
</launch>

41
rover/gscam/examples/nodelet_pipeline.launch Normal file
View File

@@ -0,0 +1,41 @@
<!-- -*- mode: XML -*- -->
<!-- Load Bayer color correction and recification into camera nodelet
manager process.
this is a test script: NOT FOR GENERAL USE
-->
<launch>
<!-- start the driver in a camera_nodelet_manager process -->
<include file="$(find gscam)/gscam_nodelet.launch" >
</include>
<!-- Bayer color decoding -->
<node pkg="nodelet" type="nodelet" name="image_proc_debayer"
args="load image_proc/debayer camera_nodelet_manager">
<remap from="camera_info" to="camera/camera_info" />
<remap from="image_color" to="camera/image_color" />
<remap from="image_mono" to="camera/image_mono" />
<remap from="image_raw" to="camera/image_raw" />
</node>
<!-- mono rectification -->
<node pkg="nodelet" type="nodelet" name="image_proc_rect"
args="load image_proc/rectify camera_nodelet_manager">
<remap from="camera_info" to="camera/camera_info" />
<remap from="image_mono" to="camera/image_mono" />
<remap from="image_rect" to="camera/image_rect" />
</node>
<!-- color rectification -->
<node pkg="nodelet" type="nodelet" name="image_proc_rect_color"
args="load image_proc/rectify camera_nodelet_manager">
<remap from="camera_info" to="camera/camera_info" />
<remap from="image_mono" to="camera/image_color" />
<remap from="image_rect" to="camera/image_rect_color" />
</node>
</launch>

20
rover/gscam/examples/osx.launch Normal file
View File

@@ -0,0 +1,20 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /webcam/image_raw -->
<!-- The GStreamer device-index needs to be an integer -->
<arg name="DEVICE_INDEX" default="0"/>
<!-- The GStreamer framerate needs to be an integral fraction -->
<arg name="FPS" default="30/1"/>
<arg name="PUBLISH_FRAME" default="false"/>
<node ns="qtkit" name="gscam_driver_qtkit" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="qtkitvideosrc device-index=$(arg DEVICE_INDEX) ! video/x-raw-yuv,framerate=$(arg FPS) ! ffmpegcolorspace"/>
<param name="frame_id" value="/qtkit_frame"/>
<param name="sync_sink" value="true"/>
</node>
<node if="$(arg PUBLISH_FRAME)" name="qtkit_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /qtkit_frame 10"/>
</launch>

30
rover/gscam/examples/uncalibrated_parameters.ini Normal file
View File

@@ -0,0 +1,30 @@
# Camera intrinsics
[image]
width
320
height
240
[default]
camera matrix
467.14110 0.00000 158.56653
0.00000 465.42608 131.37432
0.00000 0.00000 1.00000
distortion
-0.16999 0.31931 0.00929 0.00040 0.00000
rectification
1.00000 0.00000 0.00000
0.00000 1.00000 0.00000
0.00000 0.00000 1.00000
projection
458.62505 0.00000 158.11160 0.00000
0.00000 457.90904 131.91881 0.00000
0.00000 0.00000 1.00000 0.00000

19
rover/gscam/examples/v4l.launch Normal file
View File

@@ -0,0 +1,19 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /webcam/image_raw -->
<arg name="DEVICE" default="/dev/video0"/>
<!-- The GStreamer framerate needs to be an integral fraction -->
<arg name="FPS" default="30/1"/>
<arg name="PUBLISH_FRAME" default="false"/>
<node ns="v4l" name="gscam_driver_v4l" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="v4l2src device=$(arg DEVICE) ! video/x-raw-rgb,framerate=$(arg FPS) ! ffmpegcolorspace"/>
<param name="frame_id" value="/v4l_frame"/>
<param name="sync_sink" value="true"/>
</node>
<node if="$(arg PUBLISH_FRAME)" name="v4l_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /v4l_frame 10"/>
</launch>

20
rover/gscam/examples/v4ljpeg.launch Normal file
View File

@@ -0,0 +1,20 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /webcam/image_raw -->
<arg name="DEVICE" default="/dev/video0"/>
<!-- The GStreamer framerate needs to be an integral fraction -->
<arg name="FPS" default="30/1"/>
<arg name="PUBLISH_FRAME" default="false"/>
<node ns="v4l" name="gscam_driver_v4l" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="v4l2src do-timestamp=true is-live=true ! video/x-raw-yuv ! jpegenc ! multipartmux ! multipartdemux ! jpegparse"/>
<param name="use_gst_timestamps" value="true"/>
<param name="image_encoding" value="jpeg"/>
<param name="frame_id" value="/v4l_frame"/>
<param name="sync_sink" value="true"/>
</node>
</launch>

19
rover/gscam/examples/videofile.launch Normal file
View File

@@ -0,0 +1,19 @@
<launch>
<!-- This launchfile should bring up a node that broadcasts a ros image
transport on /videofile/image_raw -->
<arg name="FILENAME"/>
<arg name="LOOP" default="true"/>
<arg name="PUBLISH_FRAME" default="false"/>
<node ns="videofile" name="gscam_driver_v4l" pkg="gscam" type="gscam" output="screen">
<param name="camera_name" value="default"/>
<param name="camera_info_url" value="package://gscam/examples/uncalibrated_parameters.ini"/>
<param name="gscam_config" value="filesrc location=$(arg FILENAME) ! decodebin ! ffmpegcolorspace"/>
<param name="frame_id" value="/videofile_frame"/>
<param name="sync_sink" value="true"/>
<param name="reopen_on_eof" value="$(arg LOOP)"/>
</node>
<node if="$(arg PUBLISH_FRAME)" name="videofile_transform" pkg="tf" type="static_transform_publisher" args="1 2 3 0 -3.141 0 /world /videofile_frame 10"/>
</launch>

69
rover/gscam/include/gscam/gscam.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef __GSCAM_GSCAM_H
#define __GSCAM_GSCAM_H
extern "C"{
#include <gst/gst.h>
#include <gst/app/gstappsink.h>
}
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/SetCameraInfo.h>
#include <stdexcept>
namespace gscam {
class GSCam {
public:
GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private);
~GSCam();
bool configure();
bool init_stream();
void publish_stream();
void cleanup_stream();
void run();
private:
// General gstreamer configuration
std::string gsconfig_;
// Gstreamer structures
GstElement *pipeline_;
GstElement *sink_;
// Appsink configuration
bool sync_sink_;
bool preroll_;
bool reopen_on_eof_;
bool use_gst_timestamps_;
// Camera publisher configuration
std::string frame_id_;
int width_, height_;
std::string image_encoding_;
std::string camera_name_;
std::string camera_info_url_;
// ROS Inteface
// Calibration between ros::Time and gst timestamps
double time_offset_;
ros::NodeHandle nh_, nh_private_;
image_transport::ImageTransport image_transport_;
camera_info_manager::CameraInfoManager camera_info_manager_;
image_transport::CameraPublisher camera_pub_;
// Case of a jpeg only publisher
ros::Publisher jpeg_pub_;
ros::Publisher cinfo_pub_;
};
}
#endif // ifndef __GSCAM_GSCAM_H

26
rover/gscam/include/gscam/gscam_nodelet.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef __GSCAM_GSCAM_NODELET_H
#define __GSCAM_GSCAM_NODELET_H
#include <nodelet/nodelet.h>
#include <gscam/gscam.h>
#include <boost/thread.hpp>
#include <boost/scoped_ptr.hpp>
namespace gscam {
class GSCamNodelet : public nodelet::Nodelet
{
public:
GSCamNodelet();
~GSCamNodelet();
virtual void onInit();
private:
boost::scoped_ptr<GSCam> gscam_driver_;
boost::scoped_ptr<boost::thread> stream_thread_;
};
}
#endif // infdef __GSCAM_GSCAM_NODELET_H

25
rover/gscam/manifest.xml Normal file
View File

@@ -0,0 +1,25 @@
<package>
<description brief="gscam">
A ROS camera driver that uses gstreamer to connect to devices such as webcams.
</description>
<author>Graylin Trevor Jay, Christopher Crick</author>
<email>tjay@cs.brown.edu, chriscrick@cs.brown.edu</email>
<license>LGPL</license>
<review status="unreviewed" notes=""/>
<rosdep name="libgstreamer0.10-dev"/>
<rosdep name="libgstreamer-plugins-base0.10-dev"/>
<depend package="roscpp"/>
<depend package="image_transport"/>
<depend package="sensor_msgs"/>
<depend package="camera_calibration_parsers" />
<depend package="camera_info_manager" />
<depend package="nodelet" />
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
</package>

11
rover/gscam/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,11 @@
<library path="lib/libGSCamNodelet">
<class
name="gscam/GSCamNodelet"
type="gscam::GSCamNodelet"
base_class_type="nodelet::Nodelet">
<description>
Nodelet for publishing a ROS camera interface from a GStreamer stream.
</description>
</class>
</library>

42
rover/gscam/package.xml Normal file
View File

@@ -0,0 +1,42 @@
<package>
<name>gscam</name>
<version>0.1.3</version>
<description>
A ROS camera driver that uses gstreamer to connect to
devices such as webcams.
</description>
<maintainer email="jbo@jhu.edu">Jonathan Bohren</maintainer>
<license>BSD</license>
<author email="jbo@jhu.edu">Jonathan Bohren</author>
<author email="tjay@cs.brown.edu">Graylin Trevor Jay</author>
<author email="chriscrick@cs.brown.edu">Christopher Crick</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>libgstreamer0.10-dev</build_depend>
<build_depend>libgstreamer-plugins-base0.10-dev</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>camera_calibration_parsers</build_depend>
<build_depend>camera_info_manager</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>camera_calibration_parsers</run_depend>
<run_depend>camera_info_manager</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml"/>
</export>
</package>

6
rover/gscam/rosdep.yaml Normal file
View File

@@ -0,0 +1,6 @@
libgstreamer0.10-dev:
ubuntu:
libgstreamer0.10-dev
libgstreamer-plugins-base0.10-dev:
ubuntu:
libgstreamer-plugins-base0.10-dev

5
rover/gscam/scripts/gscam_node.in Normal file
View File

@@ -0,0 +1,5 @@
#!/usr/bin/env bash
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"'
${EXECUTABLE_OUTPUT_PATH}/gscam

403
rover/gscam/src/gscam.cpp Normal file
View File

@@ -0,0 +1,403 @@
#include <stdlib.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <iostream>
extern "C"{
#include <gst/gst.h>
#include <gst/app/gstappsink.h>
}
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/SetCameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <camera_calibration_parsers/parse_ini.h>
#include <gscam/gscam.h>
namespace gscam {
GSCam::GSCam(ros::NodeHandle nh_camera, ros::NodeHandle nh_private) :
gsconfig_(""),
pipeline_(NULL),
sink_(NULL),
nh_(nh_camera),
nh_private_(nh_private),
image_transport_(nh_camera),
camera_info_manager_(nh_camera)
{
}
GSCam::~GSCam()
{
}
bool GSCam::configure()
{
// Get gstreamer configuration
// (either from environment variable or ROS param)
std::string gsconfig_rosparam = "";
bool gsconfig_rosparam_defined = false;
char *gsconfig_env = NULL;
gsconfig_rosparam_defined = nh_private_.getParam("gscam_config",gsconfig_rosparam);
gsconfig_env = getenv("GSCAM_CONFIG");
if (!gsconfig_env && !gsconfig_rosparam_defined) {
ROS_FATAL( "Problem getting GSCAM_CONFIG environment variable and 'gscam_config' rosparam is not set. This is needed to set up a gstreamer pipeline." );
return false;
} else if(gsconfig_env && gsconfig_rosparam_defined) {
ROS_FATAL( "Both GSCAM_CONFIG environment variable and 'gscam_config' rosparam are set. Please only define one." );
return false;
} else if(gsconfig_env) {
gsconfig_ = gsconfig_env;
ROS_INFO_STREAM("Using gstreamer config from env: \""<<gsconfig_env<<"\"");
} else if(gsconfig_rosparam_defined) {
gsconfig_ = gsconfig_rosparam;
ROS_INFO_STREAM("Using gstreamer config from rosparam: \""<<gsconfig_rosparam<<"\"");
}
// Get additional gscam configuration
nh_private_.param("sync_sink", sync_sink_, true);
nh_private_.param("preroll", preroll_, false);
nh_private_.param("use_gst_timestamps", use_gst_timestamps_, false);
nh_private_.param("reopen_on_eof", reopen_on_eof_, false);
// Get the camera parameters file
nh_private_.getParam("camera_info_url", camera_info_url_);
nh_private_.getParam("camera_name", camera_name_);
// Get the image encoding
nh_private_.param("image_encoding", image_encoding_, sensor_msgs::image_encodings::RGB8);
if (image_encoding_ != sensor_msgs::image_encodings::RGB8 &&
image_encoding_ != sensor_msgs::image_encodings::MONO8 &&
image_encoding_ != "jpeg") {
ROS_FATAL_STREAM("Unsupported image encoding: " + image_encoding_);
}
camera_info_manager_.setCameraName(camera_name_);
if(camera_info_manager_.validateURL(camera_info_url_)) {
camera_info_manager_.loadCameraInfo(camera_info_url_);
ROS_INFO_STREAM("Loaded camera calibration from "<<camera_info_url_);
} else {
ROS_WARN_STREAM("Camera info at: "<<camera_info_url_<<" not found. Using an uncalibrated config.");
}
// Get TF Frame
if(!nh_private_.getParam("frame_id",frame_id_)){
frame_id_ = "/camera_frame";
ROS_WARN_STREAM("No camera frame_id set, using frame \""<<frame_id_<<"\".");
nh_private_.setParam("frame_id",frame_id_);
}
return true;
}
bool GSCam::init_stream()
{
if(!gst_is_initialized()) {
// Initialize gstreamer pipeline
ROS_DEBUG_STREAM( "Initializing gstreamer..." );
gst_init(0,0);
}
ROS_DEBUG_STREAM( "Gstreamer Version: " << gst_version_string() );
GError *error = 0; // Assignment to zero is a gst requirement
pipeline_ = gst_parse_launch(gsconfig_.c_str(), &error);
if (pipeline_ == NULL) {
ROS_FATAL_STREAM( error->message );
return false;
}
// Create RGB sink
sink_ = gst_element_factory_make("appsink",NULL);
GstCaps * caps = NULL;
if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
caps = gst_caps_new_simple("video/x-raw-rgb", NULL);
} else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) {
caps = gst_caps_new_simple("video/x-raw-gray", NULL);
} else if (image_encoding_ == "jpeg") {
caps = gst_caps_new_simple("image/jpeg", NULL);
}
gst_app_sink_set_caps(GST_APP_SINK(sink_), caps);
gst_caps_unref(caps);
// Set whether the sink should sync
// Sometimes setting this to true can cause a large number of frames to be
// dropped
gst_base_sink_set_sync(
GST_BASE_SINK(sink_),
(sync_sink_) ? TRUE : FALSE);
if(GST_IS_PIPELINE(pipeline_)) {
GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC);
g_assert(outpad);
GstElement *outelement = gst_pad_get_parent_element(outpad);
g_assert(outelement);
gst_object_unref(outpad);
if(!gst_bin_add(GST_BIN(pipeline_), sink_)) {
ROS_FATAL("gst_bin_add() failed");
gst_object_unref(outelement);
gst_object_unref(pipeline_);
return false;
}
if(!gst_element_link(outelement, sink_)) {
ROS_FATAL("GStreamer: cannot link outelement(\"%s\") -> sink\n", gst_element_get_name(outelement));
gst_object_unref(outelement);
gst_object_unref(pipeline_);
return false;
}
gst_object_unref(outelement);
} else {
GstElement* launchpipe = pipeline_;
pipeline_ = gst_pipeline_new(NULL);
g_assert(pipeline_);
gst_object_unparent(GST_OBJECT(launchpipe));
gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, NULL);
if(!gst_element_link(launchpipe, sink_)) {
ROS_FATAL("GStreamer: cannot link launchpipe -> sink");
gst_object_unref(pipeline_);
return false;
}
}
// Calibration between ros::Time and gst timestamps
GstClock * clock = gst_system_clock_obtain();
ros::Time now = ros::Time::now();
GstClockTime ct = gst_clock_get_time(clock);
gst_object_unref(clock);
time_offset_ = now.toSec() - GST_TIME_AS_USECONDS(ct)/1e6;
ROS_INFO("Time offset: %.3f",time_offset_);
gst_element_set_state(pipeline_, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration.");
return false;
} else {
ROS_DEBUG_STREAM("Stream is PAUSED.");
}
// Create ROS camera interface
if (image_encoding_ == "jpeg") {
jpeg_pub_ = nh_.advertise<sensor_msgs::CompressedImage>("camera/image_raw/compressed",1);
cinfo_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera/camera_info",1);
} else {
camera_pub_ = image_transport_.advertiseCamera("camera/image_raw", 1);
}
return true;
}
void GSCam::publish_stream()
{
ROS_INFO_STREAM("Publishing stream...");
// Pre-roll camera if needed
if (preroll_) {
ROS_DEBUG("Performing preroll...");
//The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
//I am told this is needed and am erring on the side of caution.
gst_element_set_state(pipeline_, GST_STATE_PLAYING);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PLAY during preroll.");
return;
} else {
ROS_DEBUG("Stream is PLAYING in preroll.");
}
gst_element_set_state(pipeline_, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PAUSE.");
return;
} else {
ROS_INFO("Stream is PAUSED in preroll.");
}
}
if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Could not start stream!");
return;
}
ROS_INFO("Started stream.");
// Poll the data as fast a spossible
while(ros::ok()) {
// This should block until a new frame is awake, this way, we'll run at the
// actual capture framerate of the device.
// ROS_DEBUG("Getting data...");
GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));
GstClockTime bt = gst_element_get_base_time(pipeline_);
// ROS_INFO("New buffer: timestamp %.6f %lu %lu %.3f",
// GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_);
#if 0
GstFormat fmt = GST_FORMAT_TIME;
gint64 current = -1;
Query the current position of the stream
if (gst_element_query_position(pipeline_, &fmt, &current)) {
ROS_INFO_STREAM("Position "<<current);
}
#endif
// Stop on end of stream
if (!buf) {
ROS_INFO("Stream ended.");
break;
}
// ROS_DEBUG("Got data.");
// Get the image width and height
GstPad* pad = gst_element_get_static_pad(sink_, "sink");
const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
GstStructure *structure = gst_caps_get_structure(caps,0);
gst_structure_get_int(structure,"width",&width_);
gst_structure_get_int(structure,"height",&height_);
// Update header information
sensor_msgs::CameraInfoPtr cinfo;
cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
if (use_gst_timestamps_) {
cinfo->header.stamp = ros::Time(GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_);
} else {
cinfo->header.stamp = ros::Time::now();
}
// ROS_INFO("Image time stamp: %.3f",cinfo->header.stamp.toSec());
cinfo->header.frame_id = frame_id_;
if (image_encoding_ == "jpeg") {
sensor_msgs::CompressedImagePtr img(new sensor_msgs::CompressedImage());
img->header = cinfo->header;
img->format = "jpeg";
img->data.resize(buf->size);
std::copy(buf->data, (buf->data)+(buf->size),
img->data.begin());
jpeg_pub_.publish(img);
cinfo_pub_.publish(cinfo);
} else {
// Complain if the returned buffer is smaller than we expect
const unsigned int expected_frame_size =
image_encoding_ == sensor_msgs::image_encodings::RGB8
? width_ * height_ * 3
: width_ * height_;
if (buf->size < expected_frame_size) {
ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
<< expected_frame_size << " bytes but got only "
<< (buf->size) << " bytes. (make sure frames are correctly encoded)");
}
// Construct Image message
sensor_msgs::ImagePtr img(new sensor_msgs::Image());
img->header = cinfo->header;
// Image data and metadata
img->width = width_;
img->height = height_;
img->encoding = image_encoding_;
img->is_bigendian = false;
img->data.resize(expected_frame_size);
// Copy only the data we received
// Since we're publishing shared pointers, we need to copy the image so
// we can free the buffer allocated by gstreamer
if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
img->step = width_ * 3;
} else {
img->step = width_;
}
std::copy(
buf->data,
(buf->data)+(buf->size),
img->data.begin());
// Publish the image/info
camera_pub_.publish(img, cinfo);
}
// Release the buffer
gst_buffer_unref(buf);
ros::spinOnce();
}
}
void GSCam::cleanup_stream()
{
// Clean up
ROS_INFO("Stopping gstreamer pipeline...");
if(pipeline_) {
gst_element_set_state(pipeline_, GST_STATE_NULL);
gst_object_unref(pipeline_);
pipeline_ = NULL;
}
}
void GSCam::run() {
while(ros::ok()) {
if(!this->configure()) {
ROS_FATAL("Failed to configure gscam!");
break;
}
if(!this->init_stream()) {
ROS_FATAL("Failed to initialize gscam stream!");
break;
}
// Block while publishing
this->publish_stream();
this->cleanup_stream();
ROS_INFO("GStreamer stream stopped!");
if(reopen_on_eof_) {
ROS_INFO("Reopening stream...");
} else {
ROS_INFO("Cleaning up stream and exiting...");
break;
}
}
}
// Example callbacks for appsink
// TODO: enable callback-based capture
void gst_eos_cb(GstAppSink *appsink, gpointer user_data ) {
}
GstFlowReturn gst_new_preroll_cb(GstAppSink *appsink, gpointer user_data ) {
return GST_FLOW_OK;
}
GstFlowReturn gst_new_asample_cb(GstAppSink *appsink, gpointer user_data ) {
return GST_FLOW_OK;
}
}

14
rover/gscam/src/gscam_node.cpp Normal file
View File

@@ -0,0 +1,14 @@
#include <ros/ros.h>
#include <gscam/gscam.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "gscam_publisher");
ros::NodeHandle nh, nh_private("~");
gscam::GSCam gscam_driver(nh, nh_private);
gscam_driver.run();
return 0;
}

27
rover/gscam/src/gscam_nodelet.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <gscam/gscam_nodelet.h>
PLUGINLIB_DECLARE_CLASS(gscam, GSCamNodelet, gscam::GSCamNodelet, nodelet::Nodelet)
namespace gscam {
GSCamNodelet::GSCamNodelet() :
nodelet::Nodelet(),
gscam_driver_(NULL),
stream_thread_(NULL)
{
}
GSCamNodelet::~GSCamNodelet()
{
stream_thread_->join();
}
void GSCamNodelet::onInit()
{
gscam_driver_.reset(new gscam::GSCam(this->getNodeHandle(), this->getPrivateNodeHandle()));
stream_thread_.reset(new boost::thread(boost::bind(&GSCam::run, gscam_driver_.get())));
}
}

198
rover/rover/CMakeLists.txt Normal file
View File

@@ -0,0 +1,198 @@
cmake_minimum_required(VERSION 2.8.3)
project(rover)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES rover
# CATKIN_DEPENDS rospy std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/rover.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/rover_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_rover.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

65
rover/rover/package.xml Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>rover</name>
<version>0.0.0</version>
<description>The rover package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="caperren@todo.todo">caperren</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rover</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

198
rover/uvc_capture/CMakeLists.txt Normal file
View File

@@ -0,0 +1,198 @@
cmake_minimum_required(VERSION 2.8.3)
project(uvc_capture)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES uvc_capture
# CATKIN_DEPENDS roscpp rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/uvc_capture.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/uvc_capture_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_uvc_capture.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

25
rover/uvc_capture/launch/uvc.launch Normal file
View File

@@ -0,0 +1,25 @@
<launch>
<group ns="cam1">
<node name="usb_cam1" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video1" />
<param name="image_width" value="1280" />
<param name="image_height" value="720" />
<param name="pixel_format" value="mjpeg" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
</group>
<group ns="cam2">
<node name="usb_cam2" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video2" />
<param name="image_width" value="1280" />
<param name="image_height" value="720" />
<param name="pixel_format" value="mjpeg" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
</group>
</launch>

65
rover/uvc_capture/package.xml Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>uvc_capture</name>
<version>0.0.0</version>
<description>The uvc_capture package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="caperren@todo.todo">caperren</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/uvc_capture</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

146
rover/zed_wrapper/CMakeLists.txt Normal file
View File

@@ -0,0 +1,146 @@
cmake_minimum_required(VERSION 2.8.7)
project(zed_wrapper)
# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default
IF(NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE Release)
ENDIF(NOT CMAKE_BUILD_TYPE)
function(checkPackage package customMessage)
set(varName "${package}_FOUND")
if (NOT "${${varName}}")
string(REPLACE "_" "-" aptPackage ${package})
if("${customMessage}" STREQUAL "")
message(FATAL_ERROR "\n\n ${package} is missing, please try to install it with:\n sudo apt-get install ros-$(rosversion -d)-${aptPackage}\n\n")
else()
message(FATAL_ERROR "\n\n ${customMessage} \n\n")
endif()
endif()
endfunction(checkPackage)
find_package(ZED 2.1)
checkPackage("ZED" "ZED SDK not found, install it from:\n https://www.stereolabs.com/developers/")
exec_program(uname ARGS -p OUTPUT_VARIABLE CMAKE_SYSTEM_NAME2)
if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX
SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF)
endif()
find_package(OpenCV 3 COMPONENTS core highgui imgproc)
checkPackage("OPENCV_CORE" "OpenCV core not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
checkPackage("OPENCV_HIGHGUI" "OpenCV highgui not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
checkPackage("OPENCV_IMGPROC" "OpenCV imgproc not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html")
find_package(CUDA)
checkPackage("CUDA" "CUDA not found, install it from:\n https://developer.nvidia.com/cuda-downloads")
find_package(PCL)
checkPackage("PCL" "PCL not found, try to install it with:\n sudo apt-get install libpcl1 ros-$(rosversion -d)-pcl-ros")
find_package(catkin COMPONENTS
image_transport
roscpp
rosconsole
sensor_msgs
dynamic_reconfigure
tf2_ros
pcl_conversions
nodelet
tf2_geometry_msgs
)
checkPackage("image_transport" "")
checkPackage("roscpp" "")
checkPackage("rosconsole" "")
checkPackage("sensor_msgs" "")
checkPackage("dynamic_reconfigure" "")
checkPackage("tf2_ros" "")
checkPackage("pcl_conversions" "")
checkPackage("nodelet" "")
checkPackage("tf2_geometry_msgs" "")
generate_dynamic_reconfigure_options(
cfg/Zed.cfg
)
catkin_package(
CATKIN_DEPENDS
roscpp
rosconsole
sensor_msgs
opencv
image_transport
dynamic_reconfigure
tf2_ros
tf2_geometry_msgs
pcl_conversions
)
###############################################################################
# INCLUDES
# Specify locations of header files.
include_directories(
${catkin_INCLUDE_DIRS}
${CUDA_INCLUDE_DIRS}
${ZED_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
link_directories(${ZED_LIBRARY_DIR})
link_directories(${CUDA_LIBRARY_DIRS})
link_directories(${OpenCV_LIBRARY_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
###############################################################################
###############################################################################
# EXECUTABLE
if(NOT DEFINED CUDA_NPP_LIBRARIES_ZED)
#To deal with cuda 9 nppi libs and previous versions of ZED SDK
set(CUDA_NPP_LIBRARIES_ZED ${CUDA_npp_LIBRARY} ${CUDA_npps_LIBRARY} ${CUDA_nppi_LIBRARY})
endif()
add_definitions(-std=c++11)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") ## if using from pcl package (pcl/vtk bug)
set(LINK_LIBRARIES
${catkin_LIBRARIES}
${ZED_LIBRARIES}
${CUDA_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED}
${OpenCV_LIBS}
${PCL_LIBRARIES})
add_library(ZEDWrapper src/zed_wrapper_nodelet.cpp)
target_link_libraries(ZEDWrapper ${LINK_LIBRARIES})
add_dependencies(ZEDWrapper ${PROJECT_NAME}_gencfg)
add_executable(zed_wrapper_node src/zed_wrapper_node.cpp)
target_link_libraries(zed_wrapper_node ZEDWrapper ${LINK_LIBRARIES})
add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)
###############################################################################
#Add all files in subdirectories of the project in
# a dummy_target so qtcreator have access to all files
FILE(GLOB_RECURSE extra_files ${CMAKE_SOURCE_DIR}/*)
add_custom_target(dummy_${PROJECT_NAME} SOURCES ${extra_files})
###############################################################################
# INSTALL
install(TARGETS
ZEDWrapper
zed_wrapper_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(FILES
nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

28
rover/zed_wrapper/LICENSE Normal file
View File

@@ -0,0 +1,28 @@
Copyright (c) 2017, Stereolabs
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of zed-ros-wrapper nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

51
rover/zed_wrapper/README.md Normal file
View File

@@ -0,0 +1,51 @@
# Stereolabs ZED Camera - ROS Integration
This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, odometry information and supports the use of multiple ZED cameras.
## Getting started
- First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers/)
- Download the ZED ROS wrapper [here](https://github.com/stereolabs/zed-ros-wrapper/archive/master.zip).
- For more information, check out our [ROS documentation](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html), our [ROS wiki](http://wiki.ros.org/zed-ros-wrapper) or our [blog post](https://www.stereolabs.com/blog/index.php/2015/09/07/use-your-zed-camera-with-ros/). If you want to customize the wrapper, check the [ZED API documentation](https://www.stereolabs.com/developers/documentation/API/).
### Prerequisites
- Ubuntu 16.04
- [ZED SDK **≥ 2.1**](https://www.stereolabs.com/developers/) and its dependencies ([OpenCV](http://docs.opencv.org/3.1.0/d7/d9f/tutorial_linux_install.html), [CUDA](https://developer.nvidia.com/cuda-downloads))
- [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu)
- [Point Cloud Library (PCL)](https://github.com/PointCloudLibrary/pcl)
### Build the program
The zed_ros_wrapper is a catkin package. It depends on the following ROS packages:
- tf2_ros
- tf2_geometry_msgs
- nav_msgs
- roscpp
- rosconsole
- sensor_msgs
- opencv
- image_transport
- dynamic_reconfigure
- urdf
Place the package folder `zed_wrapper` in your catkin workspace source folder `~/catkin_ws/src`.
Open a terminal and build the package:
cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
### Run the program
To launch the wrapper along with an Rviz preview, open a terminal and launch:
roslaunch zed_wrapper display.launch
To launch the wrapper without Rviz, use:
roslaunch zed_wrapper zed.launch
[More](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html)

10
rover/zed_wrapper/cfg/Zed.cfg Normal file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env python
PACKAGE = "zed_wrapper"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80, 1, 100)
exit(gen.generate(PACKAGE, "zed_wrapper", "Zed"))

27
rover/zed_wrapper/launch/README.md Normal file
View File

@@ -0,0 +1,27 @@
# Launch Files
Launch files provide a convenient way to start up multiple nodes and a master, as well as setting parameters.
Use **roslaunch** to open ZED launch files.
```
roslaunch zed_wrapper zed.launch
```
### Start the node with one ZED
Use the **zed.launch** file to launch a single camera nodelet. It includes one instance of **zed\_camera.launch**.
```
roslaunch zed_wrapper zed.launch
```
### Start the node with multiple ZED
To use multiple ZED, launch the **zed\_multi\_cam.launch** file which describes the different cameras. This example includes two instances of **zed\_camera.launch**.
```
roslaunch zed_wrapper zed_multi_cam.launch
```
### Start the node with multiple ZED and GPUs
You can configure the wrapper to assign a GPU to a ZED. In that case, it it is not possible to use several instances of **zed\_camera.launch** because different parameters need to be set for each ZED.
A sample **zed\_multi\_gpu.launch** file is available to show how to work with different ZED and GPUs.
```
roslaunch zed_wrapper zed_multi_gpu.launch
```

27
rover/zed_wrapper/launch/display.launch Normal file
View File

@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<!-- Launch ZED camera wrapper -->
<include file="$(find zed_wrapper)/launch/zed.launch" />
<!-- Launch rivz display -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_wrapper)/rviz/zed.rviz" output="screen" />
</launch>

30
rover/zed_wrapper/launch/zed.launch Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<!-- Odometry coordinate frame -->
<arg name="odometry_frame" default="map" />
<group ns="zed">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
</launch>

89
rover/zed_wrapper/launch/zed_camera.launch Normal file
View File

@@ -0,0 +1,89 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<arg name="svo_file" default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
<arg name="zed_id" default="0" />
<!-- GPU ID-->
<arg name="gpu_id" default="-1" />
<!-- Definition coordinate frames -->
<arg name="publish_tf" default="true" />
<arg name="odometry_frame" default="odom" />
<arg name="base_frame" default="zed_center" />
<arg name="camera_frame" default="zed_left_camera" />
<arg name="depth_frame" default="zed_depth_camera" />
<!-- Publish urdf zed -->
<arg name="publish_urdf" default="true" />
<node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen">
<!-- publish odometry frame -->
<param name="publish_tf" value="$(arg publish_tf)" />
<!-- Configuration frame camera -->
<param name="odometry_frame" value="$(arg odometry_frame)" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="camera_frame" value="$(arg camera_frame)" />
<param name="depth_frame" value="$(arg depth_frame)" />
<!-- SVO file path -->
<param name="svo_filepath" value="$(arg svo_file)" />
<!-- ZED parameters -->
<param name="zed_id" value="$(arg zed_id)" />
<param name="resolution" value="3" />
<param name="quality" value="1" />
<param name="sensing_mode" value="0" />
<param name="frame_rate" value="60" />
<param name="odometry_db" value="" />
<param name="openni_depth_mode" value="0" />
<param name="gpu_id" value="$(arg gpu_id)" />
<param name="confidence" value="100" />
<param name="depth_stabilization" value="1" />
<!-- ROS topic names -->
<param name="rgb_topic" value="rgb/image_rect_color" />
<param name="rgb_raw_topic" value="rgb/image_raw_color" />
<param name="rgb_cam_info_topic" value="rgb/camera_info" />
<param name="left_topic" value="left/image_rect_color" />
<param name="left_raw_topic" value="left/image_raw_color" />
<param name="left_cam_info_topic" value="left/camera_info" />
<param name="right_topic" value="right/image_rect_color" />
<param name="right_raw_topic" value="right/image_raw_color" />
<param name="right_cam_info_topic" value="right/camera_info" />
<param name="depth_topic" value="depth/depth_registered" />
<param name="depth_cam_info_topic" value="depth/camera_info" />
<param name="point_cloud_topic" value="point_cloud/cloud_registered" />
<param name="odometry_topic" value="odom" />
</node>
<!-- ROS URDF description of the ZED -->
<group if="$(arg publish_urdf)">
<param name="zed_description" textfile="$(find zed_wrapper)/urdf/zed.urdf" />
<node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<remap from="robot_description" to="zed_description" />
</node>
</group>
</launch>

45
rover/zed_wrapper/launch/zed_multi_cam.launch Normal file
View File

@@ -0,0 +1,45 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<!-- Odometry coordinate frame -->
<arg name="odometry_frame" default="map" />
<arg name="zed_id1" default="0" />
<arg name="zed_id2" default="1" />
<!-- First ZED camera -->
<group ns="zed1">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="zed_id" value="$(arg zed_id1)" />
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
<!-- Second ZED camera -->
<group ns="zed2">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="zed_id" value="$(arg zed_id2)" />
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
</launch>

49
rover/zed_wrapper/launch/zed_multi_gpu.launch Normal file
View File

@@ -0,0 +1,49 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<!-- Odometry coordinate frame -->
<arg name="odometry_frame" default="map" />
<arg name="zed_id1" default="0" />
<arg name="zed_id2" default="1" />
<!-- First ZED camera on GPU 0 -->
<group ns="zed_GPU0">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="zed_id" value="$(arg zed_id1)" />
<!-- Set GPU -->
<arg name="gpu_id" value="0" />
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
<!-- Second ZED camera on GPU 1 -->
<group ns="zed_GPU1">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="zed_id" value="$(arg zed_id2)" />
<!-- Set GPU -->
<arg name="gpu_id" value="1" />
<!-- compliant mode for rviz -->
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
</launch>

7
rover/zed_wrapper/nodelet_plugins.xml Normal file
View File

@@ -0,0 +1,7 @@
<library path="lib/libZEDWrapper">
<class name="zed_wrapper/ZEDWrapperNodelet"
type="zed_wrapper::ZEDWrapperNodelet"
base_class_type="nodelet::Nodelet">
<description>This is the nodelet of ROS interface for Stereolabs ZED Camera.</description>
</class>
</library>

41
rover/zed_wrapper/package.xml Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0"?>
<package>
<name>zed_wrapper</name>
<version>1.0.0</version>
<description>zed_wrapper is a ROS wrapper for the <a href="http://www.stereolabs.com/developers/">ZED library</a>
for interfacing with the ZED camera.
</description>
<maintainer email="developers@stereolabs.com">STEREOLABS</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rosconsole</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>opencv</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>urdf</build_depend>
<build_depend>nodelet</build_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>tf2_geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rosconsole</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>opencv</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>pcl_conversions</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>robot_state_publisher</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
</export>
</package>

2
rover/zed_wrapper/records/record_depth.sh Normal file
View File

@@ -0,0 +1,2 @@
#!/bin/bash
rosbag record /camera/rgb/camera_info /camera/depth_registered/image_raw/compressedDepth /camera/rgb/image_rect_color/compressed /tf

2
rover/zed_wrapper/records/record_stereo.sh Normal file
View File

@@ -0,0 +1,2 @@
#!/bin/bash
rosbag record /camera/left/camera_info /camera/right/camera_info /camera/left/image_raw/compressed /camera/right/image_raw/compressed /tf

301
rover/zed_wrapper/rviz/zed.rviz Normal file
View File

@@ -0,0 +1,301 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /RobotModel1/Links1
- /TF1/Frames1
- /Odometry1/Shape1
Splitter Ratio: 0.5
Tree Height: 281
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Camera
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
zed_center:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
zed_depth_camera:
Alpha: 1
Show Axes: false
Show Trail: false
zed_left_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
zed_right_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: zed/zed_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
map:
Value: false
zed_center:
Value: false
zed_depth_camera:
Value: false
zed_left_camera:
Value: true
zed_right_camera:
Value: false
Marker Scale: 1
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
map:
zed_center:
zed_left_camera:
zed_depth_camera:
{}
zed_right_camera:
{}
Update Interval: 0
Value: true
- Angle Tolerance: 0
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.300000012
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0
Shape:
Alpha: 0
Axes Length: 1
Axes Radius: 0.100000001
Color: 255; 25; 0
Head Length: 0.200000003
Head Radius: 0.100000001
Shaft Length: 0.300000012
Shaft Radius: 0.0199999996
Value: Arrow
Topic: /zed/odom
Unreliable: false
Value: true
- Alpha: 1
Auto Size:
Auto Size Factor: 1
Value: true
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/DepthCloud
Color: 255; 255; 255
Color Image Topic: /zed/rgb/image_raw_color
Color Transformer: RGB8
Color Transport Hint: raw
Decay Time: 0
Depth Map Topic: /zed/depth/depth_registered
Depth Map Transport Hint: raw
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: DepthCloud
Occlusion Compensation:
Occlusion Time-Out: 30
Value: false
Position Transformer: XYZ
Queue Size: 5
Selectable: true
Size (Pixels): 3
Style: Flat Squares
Topic Filter: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /zed/rgb/image_rect_color
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Visibility:
DepthCloud: true
Grid: true
Odometry: true
PointCloud2: true
RobotModel: true
TF: true
Value: false
Zoom Factor: 1
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /zed/point_cloud/cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 3.33887124
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.00562699512
Y: -0.0610139929
Z: -0.0259050336
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.310398251
Target Frame: zed_left_camera
Value: Orbit (rviz)
Yaw: 3.44725299
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
Height: 793
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000028ffc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a8000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000000c00430061006d00650072006101000001d6000000e10000001600ffffff000000010000010f00000327fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000327000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000045d0000003efc0100000002fb0000000800540069006d006501000000000000045d0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002ed0000028f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1117
X: 1474
Y: 432

37
rover/zed_wrapper/src/zed_wrapper_node.cpp Normal file
View File

@@ -0,0 +1,37 @@
///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2017, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
#include <ros/ros.h>
#include <nodelet/loader.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "zed_wrapper_node");
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
nodelet.load(ros::this_node::getName(),
"zed_wrapper/ZEDWrapperNodelet",
remap, nargv);
ros::spin();
return 0;
}

813
rover/zed_wrapper/src/zed_wrapper_nodelet.cpp Normal file
View File

@@ -0,0 +1,813 @@
///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2017, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
/****************************************************************************************************
** This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS. **
** A set of parameters can be specified in the launch file. **
****************************************************************************************************/
#include <csignal>
#include <cstdio>
#include <math.h>
#include <limits>
#include <thread>
#include <chrono>
#include <memory>
#include <sys/stat.h>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/distortion_models.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <zed_wrapper/ZedConfig.h>
#include <nav_msgs/Odometry.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/make_shared.hpp>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sl/Camera.hpp>
using namespace std;
namespace zed_wrapper {
class ZEDWrapperNodelet : public nodelet::Nodelet {
ros::NodeHandle nh;
ros::NodeHandle nh_ns;
boost::shared_ptr<boost::thread> device_poll_thread;
image_transport::Publisher pub_rgb;
image_transport::Publisher pub_raw_rgb;
image_transport::Publisher pub_left;
image_transport::Publisher pub_raw_left;
image_transport::Publisher pub_right;
image_transport::Publisher pub_raw_right;
image_transport::Publisher pub_depth;
ros::Publisher pub_cloud;
ros::Publisher pub_rgb_cam_info;
ros::Publisher pub_left_cam_info;
ros::Publisher pub_right_cam_info;
ros::Publisher pub_depth_cam_info;
ros::Publisher pub_odom;
// tf
tf2_ros::TransformBroadcaster transform_odom_broadcaster;
std::string left_frame_id;
std::string right_frame_id;
std::string rgb_frame_id;
std::string depth_frame_id;
std::string cloud_frame_id;
std::string odometry_frame_id;
std::string base_frame_id;
std::string camera_frame_id;
// initialization Transform listener
boost::shared_ptr<tf2_ros::Buffer> tfBuffer;
boost::shared_ptr<tf2_ros::TransformListener> tf_listener;
bool publish_tf;
// Launch file parameters
int resolution;
int quality;
int sensing_mode;
int rate;
int gpu_id;
int zed_id;
int depth_stabilization;
std::string odometry_DB;
std::string svo_filepath;
//Tracking variables
sl::Pose pose;
tf2::Transform base_transform;
// zed object
sl::InitParameters param;
std::unique_ptr<sl::Camera> zed;
// flags
int confidence;
bool computeDepth;
bool grabbing = false;
int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html
// Point cloud variables
sl::Mat cloud;
string point_cloud_frame_id = "";
ros::Time point_cloud_time;
/* \brief Convert an sl:Mat to a cv::Mat
* \param mat : the sl::Mat to convert
*/
cv::Mat toCVMat(sl::Mat &mat) {
if (mat.getMemoryType() == sl::MEM_GPU)
mat.updateCPUfromGPU();
int cvType;
switch (mat.getDataType()) {
case sl::MAT_TYPE_32F_C1:
cvType = CV_32FC1;
break;
case sl::MAT_TYPE_32F_C2:
cvType = CV_32FC2;
break;
case sl::MAT_TYPE_32F_C3:
cvType = CV_32FC3;
break;
case sl::MAT_TYPE_32F_C4:
cvType = CV_32FC4;
break;
case sl::MAT_TYPE_8U_C1:
cvType = CV_8UC1;
break;
case sl::MAT_TYPE_8U_C2:
cvType = CV_8UC2;
break;
case sl::MAT_TYPE_8U_C3:
cvType = CV_8UC3;
break;
case sl::MAT_TYPE_8U_C4:
cvType = CV_8UC4;
break;
}
return cv::Mat((int) mat.getHeight(), (int) mat.getWidth(), cvType, mat.getPtr<sl::uchar1>(sl::MEM_CPU), mat.getStepBytes(sl::MEM_CPU));
}
/* \brief Test if a file exist
* \param name : the path to the file
*/
bool file_exist(const std::string& name) {
struct stat buffer;
return (stat(name.c_str(), &buffer) == 0);
}
/* \brief Image to ros message conversion
* \param img : the image to publish
* \param encodingType : the sensor_msgs::image_encodings encoding type
* \param frameId : the id of the reference frame of the image
* \param t : the ros::Time to stamp the image
*/
sensor_msgs::ImagePtr imageToROSmsg(cv::Mat img, const std::string encodingType, std::string frameId, ros::Time t) {
sensor_msgs::ImagePtr ptr = boost::make_shared<sensor_msgs::Image>();
sensor_msgs::Image& imgMessage = *ptr;
imgMessage.header.stamp = t;
imgMessage.header.frame_id = frameId;
imgMessage.height = img.rows;
imgMessage.width = img.cols;
imgMessage.encoding = encodingType;
int num = 1; //for endianness detection
imgMessage.is_bigendian = !(*(char *) &num == 1);
imgMessage.step = img.cols * img.elemSize();
size_t size = imgMessage.step * img.rows;
imgMessage.data.resize(size);
if (img.isContinuous())
memcpy((char*) (&imgMessage.data[0]), img.data, size);
else {
uchar* opencvData = img.data;
uchar* rosData = (uchar*) (&imgMessage.data[0]);
for (unsigned int i = 0; i < img.rows; i++) {
memcpy(rosData, opencvData, imgMessage.step);
rosData += imgMessage.step;
opencvData += img.step;
}
}
return ptr;
}
/* \brief Publish the pose of the camera with a ros Publisher
* \param base_transform : Transformation representing the camera pose from base frame
* \param pub_odom : the publisher object to use
* \param odom_frame_id : the id of the reference frame of the pose
* \param t : the ros::Time to stamp the image
*/
void publishOdom(tf2::Transform base_transform, ros::Publisher &pub_odom, string odom_frame_id, ros::Time t) {
nav_msgs::Odometry odom;
odom.header.stamp = t;
odom.header.frame_id = odom_frame_id; // odom_frame
odom.child_frame_id = base_frame_id; // base_frame
// conversion from Tranform to message
geometry_msgs::Transform base2 = tf2::toMsg(base_transform);
// Add all value in odometry message
odom.pose.pose.position.x = base2.translation.x;
odom.pose.pose.position.y = base2.translation.y;
odom.pose.pose.position.z = base2.translation.z;
odom.pose.pose.orientation.x = base2.rotation.x;
odom.pose.pose.orientation.y = base2.rotation.y;
odom.pose.pose.orientation.z = base2.rotation.z;
odom.pose.pose.orientation.w = base2.rotation.w;
// Publish odometry message
pub_odom.publish(odom);
}
/* \brief Publish the pose of the camera as a transformation
* \param base_transform : Transformation representing the camera pose from base frame
* \param trans_br : the TransformBroadcaster object to use
* \param odometry_transform_frame_id : the id of the transformation
* \param t : the ros::Time to stamp the image
*/
void publishTrackedFrame(tf2::Transform base_transform, tf2_ros::TransformBroadcaster &trans_br, string odometry_transform_frame_id, ros::Time t) {
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = odometry_frame_id;
transformStamped.child_frame_id = odometry_transform_frame_id;
// conversion from Tranform to message
transformStamped.transform = tf2::toMsg(base_transform);
// Publish transformation
trans_br.sendTransform(transformStamped);
}
/* \brief Publish a cv::Mat image with a ros Publisher
* \param img : the image to publish
* \param pub_img : the publisher object to use
* \param img_frame_id : the id of the reference frame of the image
* \param t : the ros::Time to stamp the image
*/
void publishImage(cv::Mat img, image_transport::Publisher &pub_img, string img_frame_id, ros::Time t) {
pub_img.publish(imageToROSmsg(img, sensor_msgs::image_encodings::BGR8, img_frame_id, t));
}
/* \brief Publish a cv::Mat depth image with a ros Publisher
* \param depth : the depth image to publish
* \param pub_depth : the publisher object to use
* \param depth_frame_id : the id of the reference frame of the depth image
* \param t : the ros::Time to stamp the depth image
*/
void publishDepth(cv::Mat depth, image_transport::Publisher &pub_depth, string depth_frame_id, ros::Time t) {
string encoding;
if (openniDepthMode) {
depth *= 1000.0f;
depth.convertTo(depth, CV_16UC1); // in mm, rounded
encoding = sensor_msgs::image_encodings::TYPE_16UC1;
} else {
encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
pub_depth.publish(imageToROSmsg(depth, encoding, depth_frame_id, t));
}
/* \brief Publish a pointCloud with a ros Publisher
* \param width : the width of the point cloud
* \param height : the height of the point cloud
* \param pub_cloud : the publisher object to use
*/
void publishPointCloud(int width, int height, ros::Publisher &pub_cloud) {
pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
point_cloud.width = width;
point_cloud.height = height;
int size = width*height;
point_cloud.points.resize(size);
sl::Vector4<float>* cpu_cloud = cloud.getPtr<sl::float4>();
for (int i = 0; i < size; i++) {
point_cloud.points[i].x = cpu_cloud[i][2];
point_cloud.points[i].y = -cpu_cloud[i][0];
point_cloud.points[i].z = -cpu_cloud[i][1];
point_cloud.points[i].rgb = cpu_cloud[i][3];
}
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(point_cloud, output); // Convert the point cloud to a ROS message
output.header.frame_id = point_cloud_frame_id; // Set the header values of the ROS message
output.header.stamp = point_cloud_time;
output.height = height;
output.width = width;
output.is_bigendian = false;
output.is_dense = false;
pub_cloud.publish(output);
}
/* \brief Publish the informations of a camera with a ros Publisher
* \param cam_info_msg : the information message to publish
* \param pub_cam_info : the publisher object to use
* \param t : the ros::Time to stamp the message
*/
void publishCamInfo(sensor_msgs::CameraInfoPtr cam_info_msg, ros::Publisher pub_cam_info, ros::Time t) {
static int seq = 0;
cam_info_msg->header.stamp = t;
cam_info_msg->header.seq = seq;
pub_cam_info.publish(cam_info_msg);
seq++;
}
/* \brief Get the information of the ZED cameras and store them in an information message
* \param zed : the sl::zed::Camera* pointer to an instance
* \param left_cam_info_msg : the information message to fill with the left camera informations
* \param right_cam_info_msg : the information message to fill with the right camera informations
* \param left_frame_id : the id of the reference frame of the left camera
* \param right_frame_id : the id of the reference frame of the right camera
*/
void fillCamInfo(sl::Camera* zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, sensor_msgs::CameraInfoPtr right_cam_info_msg,
string left_frame_id, string right_frame_id) {
int width = zed->getResolution().width;
int height = zed->getResolution().height;
sl::CameraInformation zedParam = zed->getCameraInformation();
float baseline = zedParam.calibration_parameters.T[0] * 0.001; // baseline converted in meters
float fx = zedParam.calibration_parameters.left_cam.fx;
float fy = zedParam.calibration_parameters.left_cam.fy;
float cx = zedParam.calibration_parameters.left_cam.cx;
float cy = zedParam.calibration_parameters.left_cam.cy;
// There is no distorsions since the images are rectified
double k1 = 0;
double k2 = 0;
double k3 = 0;
double p1 = 0;
double p2 = 0;
left_cam_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
right_cam_info_msg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
left_cam_info_msg->D.resize(5);
right_cam_info_msg->D.resize(5);
left_cam_info_msg->D[0] = right_cam_info_msg->D[0] = k1;
left_cam_info_msg->D[1] = right_cam_info_msg->D[1] = k2;
left_cam_info_msg->D[2] = right_cam_info_msg->D[2] = k3;
left_cam_info_msg->D[3] = right_cam_info_msg->D[3] = p1;
left_cam_info_msg->D[4] = right_cam_info_msg->D[4] = p2;
left_cam_info_msg->K.fill(0.0);
right_cam_info_msg->K.fill(0.0);
left_cam_info_msg->K[0] = right_cam_info_msg->K[0] = fx;
left_cam_info_msg->K[2] = right_cam_info_msg->K[2] = cx;
left_cam_info_msg->K[4] = right_cam_info_msg->K[4] = fy;
left_cam_info_msg->K[5] = right_cam_info_msg->K[5] = cy;
left_cam_info_msg->K[8] = right_cam_info_msg->K[8] = 1.0;
left_cam_info_msg->R.fill(0.0);
right_cam_info_msg->R.fill(0.0);
left_cam_info_msg->P.fill(0.0);
right_cam_info_msg->P.fill(0.0);
left_cam_info_msg->P[0] = right_cam_info_msg->P[0] = fx;
left_cam_info_msg->P[2] = right_cam_info_msg->P[2] = cx;
left_cam_info_msg->P[5] = right_cam_info_msg->P[5] = fy;
left_cam_info_msg->P[6] = right_cam_info_msg->P[6] = cy;
left_cam_info_msg->P[10] = right_cam_info_msg->P[10] = 1.0;
right_cam_info_msg->P[3] = (-1 * fx * baseline);
left_cam_info_msg->width = right_cam_info_msg->width = width;
left_cam_info_msg->height = right_cam_info_msg->height = height;
left_cam_info_msg->header.frame_id = left_frame_id;
right_cam_info_msg->header.frame_id = right_frame_id;
}
void callback(zed_wrapper::ZedConfig &config, uint32_t level) {
NODELET_INFO("Reconfigure confidence : %d", config.confidence);
confidence = config.confidence;
}
void device_poll() {
ros::Rate loop_rate(rate);
ros::Time old_t = ros::Time::now();
bool old_image = false;
bool tracking_activated = false;
// Get the parameters of the ZED images
int width = zed->getResolution().width;
int height = zed->getResolution().height;
NODELET_DEBUG_STREAM("Image size : " << width << "x" << height);
cv::Size cvSize(width, height);
cv::Mat leftImRGB(cvSize, CV_8UC3);
cv::Mat rightImRGB(cvSize, CV_8UC3);
// Create and fill the camera information messages
sensor_msgs::CameraInfoPtr rgb_cam_info_msg(new sensor_msgs::CameraInfo());
sensor_msgs::CameraInfoPtr left_cam_info_msg(new sensor_msgs::CameraInfo());
sensor_msgs::CameraInfoPtr right_cam_info_msg(new sensor_msgs::CameraInfo());
sensor_msgs::CameraInfoPtr depth_cam_info_msg(new sensor_msgs::CameraInfo());
fillCamInfo(zed.get(), left_cam_info_msg, right_cam_info_msg, left_frame_id, right_frame_id);
rgb_cam_info_msg = depth_cam_info_msg = left_cam_info_msg; // the reference camera is the Left one (next to the ZED logo)
sl::RuntimeParameters runParams;
runParams.sensing_mode = static_cast<sl::SENSING_MODE> (sensing_mode);
sl::TrackingParameters trackParams;
trackParams.area_file_path = odometry_DB.c_str();
sl::Mat leftZEDMat, rightZEDMat, depthZEDMat;
// Main loop
while (nh_ns.ok()) {
// Check for subscribers
int rgb_SubNumber = pub_rgb.getNumSubscribers();
int rgb_raw_SubNumber = pub_raw_rgb.getNumSubscribers();
int left_SubNumber = pub_left.getNumSubscribers();
int left_raw_SubNumber = pub_raw_left.getNumSubscribers();
int right_SubNumber = pub_right.getNumSubscribers();
int right_raw_SubNumber = pub_raw_right.getNumSubscribers();
int depth_SubNumber = pub_depth.getNumSubscribers();
int cloud_SubNumber = pub_cloud.getNumSubscribers();
int odom_SubNumber = pub_odom.getNumSubscribers();
bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
runParams.enable_point_cloud = false;
if (cloud_SubNumber > 0)
runParams.enable_point_cloud = true;
// Run the loop only if there is some subscribers
if (runLoop) {
if ( (depth_stabilization || odom_SubNumber > 0) && !tracking_activated) { //Start the tracking
if (odometry_DB != "" && !file_exist(odometry_DB)) {
odometry_DB = "";
NODELET_WARN("odometry_DB path doesn't exist or is unreachable.");
}
zed->enableTracking(trackParams);
tracking_activated = true;
} else if (!depth_stabilization && odom_SubNumber == 0 && tracking_activated) { //Stop the tracking
zed->disableTracking();
tracking_activated = false;
}
computeDepth = (depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
ros::Time t = ros::Time::now(); // Get current time
grabbing = true;
if (computeDepth) {
int actual_confidence = zed->getConfidenceThreshold();
if (actual_confidence != confidence)
zed->setConfidenceThreshold(confidence);
runParams.enable_depth = true; // Ask to compute the depth
} else
runParams.enable_depth = false;
old_image = zed->grab(runParams); // Ask to not compute the depth
grabbing = false;
if (old_image) { // Detect if a error occurred (for example: the zed have been disconnected) and re-initialize the ZED
NODELET_DEBUG("Wait for a new image to proceed");
std::this_thread::sleep_for(std::chrono::milliseconds(2));
if ((t - old_t).toSec() > 5) {
// delete the old object before constructing a new one
zed.reset();
zed.reset(new sl::Camera());
NODELET_INFO("Re-openning the ZED");
sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED;
while (err != sl::SUCCESS) {
err = zed->open(param); // Try to initialize the ZED
NODELET_INFO_STREAM(errorCode2str(err));
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
}
tracking_activated = false;
if (depth_stabilization || odom_SubNumber > 0) { //Start the tracking
if (odometry_DB != "" && !file_exist(odometry_DB)) {
odometry_DB = "";
NODELET_WARN("odometry_DB path doesn't exist or is unreachable.");
}
zed->enableTracking(trackParams);
tracking_activated = true;
}
}
continue;
}
old_t = ros::Time::now();
// Publish the left == rgb image if someone has subscribed to
if (left_SubNumber > 0 || rgb_SubNumber > 0) {
// Retrieve RGBA Left image
zed->retrieveImage(leftZEDMat, sl::VIEW_LEFT);
cv::cvtColor(toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB);
if (left_SubNumber > 0) {
publishCamInfo(left_cam_info_msg, pub_left_cam_info, t);
publishImage(leftImRGB, pub_left, left_frame_id, t);
}
if (rgb_SubNumber > 0) {
publishCamInfo(rgb_cam_info_msg, pub_rgb_cam_info, t);
publishImage(leftImRGB, pub_rgb, rgb_frame_id, t); // rgb is the left image
}
}
// Publish the left_raw == rgb_raw image if someone has subscribed to
if (left_raw_SubNumber > 0 || rgb_raw_SubNumber > 0) {
// Retrieve RGBA Left image
zed->retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED);
cv::cvtColor(toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB);
if (left_raw_SubNumber > 0) {
publishCamInfo(left_cam_info_msg, pub_left_cam_info, t);
publishImage(leftImRGB, pub_raw_left, left_frame_id, t);
}
if (rgb_raw_SubNumber > 0) {
publishCamInfo(rgb_cam_info_msg, pub_rgb_cam_info, t);
publishImage(leftImRGB, pub_raw_rgb, rgb_frame_id, t);
}
}
// Publish the right image if someone has subscribed to
if (right_SubNumber > 0) {
// Retrieve RGBA Right image
zed->retrieveImage(rightZEDMat, sl::VIEW_RIGHT);
cv::cvtColor(toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB);
publishCamInfo(right_cam_info_msg, pub_right_cam_info, t);
publishImage(rightImRGB, pub_right, right_frame_id, t);
}
// Publish the right image if someone has subscribed to
if (right_raw_SubNumber > 0) {
// Retrieve RGBA Right image
zed->retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED);
cv::cvtColor(toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB);
publishCamInfo(right_cam_info_msg, pub_right_cam_info, t);
publishImage(rightImRGB, pub_raw_right, right_frame_id, t);
}
// Publish the depth image if someone has subscribed to
if (depth_SubNumber > 0) {
zed->retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH);
publishCamInfo(depth_cam_info_msg, pub_depth_cam_info, t);
publishDepth(toCVMat(depthZEDMat), pub_depth, depth_frame_id, t); // in meters
}
// Publish the point cloud if someone has subscribed to
if (cloud_SubNumber > 0) {
// Run the point cloud convertion asynchronously to avoid slowing down all the program
// Retrieve raw pointCloud data
zed->retrieveMeasure(cloud, sl::MEASURE_XYZBGRA);
point_cloud_frame_id = cloud_frame_id;
point_cloud_time = t;
publishPointCloud(width, height, pub_cloud);
}
// Transform from base to sensor
tf2::Transform base_to_sensor;
// Look up the transformation from base frame to camera link
try {
// Save the transformation from base to frame
geometry_msgs::TransformStamped b2s = tfBuffer->lookupTransform(base_frame_id, camera_frame_id, t);
// Get the TF2 transformation
tf2::fromMsg(b2s.transform, base_to_sensor);
} catch (tf2::TransformException &ex) {
ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available, "
"will assume it as identity!",
base_frame_id.c_str(),
camera_frame_id.c_str());
ROS_DEBUG("Transform error: %s", ex.what());
base_to_sensor.setIdentity();
}
// Publish the odometry if someone has subscribed to
if (odom_SubNumber > 0) {
zed->getPosition(pose);
// Transform ZED pose in TF2 Transformation
tf2::Transform camera_transform;
geometry_msgs::Transform c2s;
sl::Translation translation = pose.getTranslation();
c2s.translation.x = translation(2);
c2s.translation.y = -translation(0);
c2s.translation.z = -translation(1);
sl::Orientation quat = pose.getOrientation();
c2s.rotation.x = quat(2);
c2s.rotation.y = -quat(0);
c2s.rotation.z = -quat(1);
c2s.rotation.w = quat(3);
tf2::fromMsg(c2s, camera_transform);
// Transformation from camera sensor to base frame
base_transform = base_to_sensor * camera_transform * base_to_sensor.inverse();
// Publish odometry message
publishOdom(base_transform, pub_odom, odometry_frame_id, t);
}
// Publish odometry tf only if enabled
if(publish_tf) {
//Note, the frame is published, but its values will only change if someone has subscribed to odom
publishTrackedFrame(base_transform, transform_odom_broadcaster, base_frame_id, t); //publish the tracked Frame
}
loop_rate.sleep();
} else {
// Publish odometry tf only if enabled
if(publish_tf) {
publishTrackedFrame(base_transform, transform_odom_broadcaster, base_frame_id, ros::Time::now()); //publish the tracked Frame before the sleep
}
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // No subscribers, we just wait
}
} // while loop
zed.reset();
}
boost::shared_ptr<dynamic_reconfigure::Server<zed_wrapper::ZedConfig>> server;
void onInit() {
// Launch file parameters
resolution = sl::RESOLUTION_HD720;
quality = sl::DEPTH_MODE_PERFORMANCE;
sensing_mode = sl::SENSING_MODE_STANDARD;
rate = 30;
gpu_id = -1;
zed_id = 0;
odometry_DB = "";
nh = getMTNodeHandle();
nh_ns = getMTPrivateNodeHandle();
// Set default cordinate frames
// If unknown left and right frames are set in the same camera coordinate frame
nh_ns.param<std::string>("odometry_frame", odometry_frame_id, "odometry_frame");
nh_ns.param<std::string>("base_frame", base_frame_id, "base_frame");
nh_ns.param<std::string>("camera_frame", camera_frame_id, "camera_frame");
nh_ns.param<std::string>("depth_frame", depth_frame_id, "depth_frame");
// Get parameters from launch file
nh_ns.getParam("resolution", resolution);
nh_ns.getParam("quality", quality);
nh_ns.getParam("sensing_mode", sensing_mode);
nh_ns.getParam("frame_rate", rate);
nh_ns.getParam("odometry_DB", odometry_DB);
nh_ns.getParam("openni_depth_mode", openniDepthMode);
nh_ns.getParam("gpu_id", gpu_id);
nh_ns.getParam("zed_id", zed_id);
nh_ns.getParam("depth_stabilization", depth_stabilization);
// Publish odometry tf
nh_ns.param<bool>("publish_tf", publish_tf, true);
// Print order frames
ROS_INFO_STREAM("odometry_frame: " << odometry_frame_id);
ROS_INFO_STREAM("base_frame: " << base_frame_id);
ROS_INFO_STREAM("camera_frame: " << camera_frame_id);
ROS_INFO_STREAM("depth_frame: " << depth_frame_id);
// Status of odometry TF
ROS_INFO_STREAM("Publish " << odometry_frame_id << " [" << (publish_tf ? "TRUE" : "FALSE") << "]");
std::string img_topic = "image_rect_color";
std::string img_raw_topic = "image_raw_color";
// Set the default topic names
string left_topic = "left/" + img_topic;
string left_raw_topic = "left/" + img_raw_topic;
string left_cam_info_topic = "left/camera_info";
left_frame_id = camera_frame_id;
string right_topic = "right/" + img_topic;
string right_raw_topic = "right/" + img_raw_topic;
string right_cam_info_topic = "right/camera_info";
right_frame_id = camera_frame_id;
string rgb_topic = "rgb/" + img_topic;
string rgb_raw_topic = "rgb/" + img_raw_topic;
string rgb_cam_info_topic = "rgb/camera_info";
rgb_frame_id = depth_frame_id;
string depth_topic = "depth/";
if (openniDepthMode) {
NODELET_INFO_STREAM("Openni depth mode activated");
depth_topic += "depth_raw_registered";
} else {
depth_topic += "depth_registered";
}
string depth_cam_info_topic = "depth/camera_info";
string point_cloud_topic = "point_cloud/cloud_registered";
cloud_frame_id = camera_frame_id;
string odometry_topic = "odom";
nh_ns.getParam("rgb_topic", rgb_topic);
nh_ns.getParam("rgb_raw_topic", rgb_raw_topic);
nh_ns.getParam("rgb_cam_info_topic", rgb_cam_info_topic);
nh_ns.getParam("left_topic", left_topic);
nh_ns.getParam("left_raw_topic", left_raw_topic);
nh_ns.getParam("left_cam_info_topic", left_cam_info_topic);
nh_ns.getParam("right_topic", right_topic);
nh_ns.getParam("right_raw_topic", right_raw_topic);
nh_ns.getParam("right_cam_info_topic", right_cam_info_topic);
nh_ns.getParam("depth_topic", depth_topic);
nh_ns.getParam("depth_cam_info_topic", depth_cam_info_topic);
nh_ns.getParam("point_cloud_topic", point_cloud_topic);
nh_ns.getParam("odometry_topic", odometry_topic);
nh_ns.param<std::string>("svo_filepath", svo_filepath, std::string());
// Initialization transformation listener
tfBuffer.reset( new tf2_ros::Buffer );
tf_listener.reset( new tf2_ros::TransformListener(*tfBuffer) );
// Create the ZED object
zed.reset(new sl::Camera());
// Initialize tf2 transformation
base_transform.setIdentity();
// Try to initialize the ZED
if (!svo_filepath.empty())
param.svo_input_filename = svo_filepath.c_str();
else {
param.camera_fps = rate;
param.camera_resolution = static_cast<sl::RESOLUTION> (resolution);
param.camera_linux_id = zed_id;
}
param.coordinate_units = sl::UNIT_METER;
param.coordinate_system = sl::COORDINATE_SYSTEM_IMAGE;
param.depth_mode = static_cast<sl::DEPTH_MODE> (quality);
param.sdk_verbose = true;
param.sdk_gpu_id = gpu_id;
param.depth_stabilization = depth_stabilization;
sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED;
while (err != sl::SUCCESS) {
err = zed->open(param);
NODELET_INFO_STREAM(errorCode2str(err));
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
}
//Reconfigure confidence
server = boost::make_shared<dynamic_reconfigure::Server<zed_wrapper::ZedConfig>>();
dynamic_reconfigure::Server<zed_wrapper::ZedConfig>::CallbackType f;
f = boost::bind(&ZEDWrapperNodelet::callback, this, _1, _2);
server->setCallback(f);
nh_ns.getParam("confidence", confidence);
// Create all the publishers
// Image publishers
image_transport::ImageTransport it_zed(nh);
pub_rgb = it_zed.advertise(rgb_topic, 1); //rgb
NODELET_INFO_STREAM("Advertized on topic " << rgb_topic);
pub_raw_rgb = it_zed.advertise(rgb_raw_topic, 1); //rgb raw
NODELET_INFO_STREAM("Advertized on topic " << rgb_raw_topic);
pub_left = it_zed.advertise(left_topic, 1); //left
NODELET_INFO_STREAM("Advertized on topic " << left_topic);
pub_raw_left = it_zed.advertise(left_raw_topic, 1); //left raw
NODELET_INFO_STREAM("Advertized on topic " << left_raw_topic);
pub_right = it_zed.advertise(right_topic, 1); //right
NODELET_INFO_STREAM("Advertized on topic " << right_topic);
pub_raw_right = it_zed.advertise(right_raw_topic, 1); //right raw
NODELET_INFO_STREAM("Advertized on topic " << right_raw_topic);
pub_depth = it_zed.advertise(depth_topic, 1); //depth
NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
//PointCloud publisher
pub_cloud = nh.advertise<sensor_msgs::PointCloud2> (point_cloud_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic);
// Camera info publishers
pub_rgb_cam_info = nh.advertise<sensor_msgs::CameraInfo>(rgb_cam_info_topic, 1); //rgb
NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_topic);
pub_left_cam_info = nh.advertise<sensor_msgs::CameraInfo>(left_cam_info_topic, 1); //left
NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_topic);
pub_right_cam_info = nh.advertise<sensor_msgs::CameraInfo>(right_cam_info_topic, 1); //right
NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_topic);
pub_depth_cam_info = nh.advertise<sensor_msgs::CameraInfo>(depth_cam_info_topic, 1); //depth
NODELET_INFO_STREAM("Advertized on topic " << depth_cam_info_topic);
//Odometry publisher
pub_odom = nh.advertise<nav_msgs::Odometry>(odometry_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << odometry_topic);
device_poll_thread = boost::shared_ptr<boost::thread>
(new boost::thread(boost::bind(&ZEDWrapperNodelet::device_poll, this)));
}
}; // class ZEDROSWrapperNodelet
} // namespace
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(zed_wrapper::ZEDWrapperNodelet, nodelet::Nodelet);

BIN
rover/zed_wrapper/urdf/ZED.stl Normal file
View File

Binary file not shown.

73
rover/zed_wrapper/urdf/zed.urdf Normal file
View File

@@ -0,0 +1,73 @@
<?xml version="1.0"?>
<!--
Copyright (c) 2017, STEREOLABS.
All rights reserved.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<robot name="zed_camera">
<link name="zed_left_camera">
<visual>
<origin xyz="0 0 0" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="0.007" length=".031"/>
</geometry>
<material name="dark_grey">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</visual>
</link>
<link name="zed_center">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://zed_wrapper/urdf/ZED.stl" />
</geometry>
<material name="light_grey">
<color rgba="0.8 0.8 0.8 0.8"/>
</material>
</visual>
</link>
<link name="zed_right_camera">
<visual>
<origin xyz="0 0 0" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="0.007" length=".031"/>
</geometry>
<material name="dark_grey">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</visual>
</link>
<link name="zed_depth_camera" />
<joint name="zed_left_camera_joint" type="fixed">
<parent link="zed_center"/>
<child link="zed_left_camera"/>
<origin xyz="0 0.06 0" rpy="0 0 0" />
</joint>
<joint name="zed_depth_camera_joint" type="fixed">
<parent link="zed_left_camera"/>
<child link="zed_depth_camera"/>
<origin xyz="0 0 0" rpy="-1.5707963267948966 0 -1.5707963267948966" />
</joint>
<joint name="zed_right_camera_joint" type="fixed">
<parent link="zed_center"/>
<child link="zed_right_camera"/>
<origin xyz="0 -0.06 0" rpy="0 0 0" />
</joint>
</robot>