diff --git a/.gitignore b/.gitignore index 44198ca..d53a461 100644 --- a/.gitignore +++ b/.gitignore @@ -1,10 +1,4 @@ -*.a -*.o -*.os *.pyc -*.so *.tar.gz *~ TAGS -bin/ -build/ diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..3af959a --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,32 @@ +Change history +============== + +0.1.0 (forthcoming) +------------------- + + * Groovy and Hydro release. + * Rename **compressed** topic **image_raw/compressed** (`#5`_). + * Convert to catkin (`#12`_). + +0.0.2 (2013-04-10) +------------------ + + * Fuerte update. + * Add **frame_id** parameter (fixes `#8`_) + * Add camera_info_manager support (`#10`_). Adds a new dependency on + **camera_info_manager_py**, and a new **camera_info_url** + parameter. + * Add some additional PTZ control nodes: teleop.py, teleop_twist.py, + axis_twist.py, axis_all.py. + * Add PTZ transform publisher: publish_axis_tf.py. + +0.0.1 (2012-12-05) +------------------ + + * Fuerte release. + * Initial axis_camera package. + +.. _`#5`: https://github.com/clearpathrobotics/axis_camera/issues/5 +.. _`#8`: https://github.com/clearpathrobotics/axis_camera/issues/8 +.. _`#10`: https://github.com/clearpathrobotics/axis_camera/issues/10 +.. _`#12`: https://github.com/clearpathrobotics/axis_camera/issues/12 diff --git a/CMakeLists.txt b/CMakeLists.txt index 10b7e6e..614461c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,31 +1,34 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +cmake_minimum_required(VERSION 2.8.3) +project(axis_camera) -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) +find_package(catkin REQUIRED + COMPONENTS + camera_info_manager_py + geometry_msgs + message_generation + rospy + sensor_msgs + tf +) +catkin_python_setup() -rosbuild_init() +# ROS message generation +add_message_files(DIRECTORY msg FILES Axis.msg) +generate_messages(DEPENDENCIES geometry_msgs std_msgs) -#set the default path for built executables to the "bin" directory -#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) +catkin_package( + CATKIN_DEPENDS + camera_info_manager_py + geometry_msgs + message_runtime + sensor_msgs + tf +) -#uncomment if you have defined messages -rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) -rosbuild_make_distribution(0.0.2) +install(PROGRAMS + nodes/axis.py + nodes/axis_ptz.py + nodes/publish_axis_tf.py + nodes/teleop.py + nodes/teleop_speed_control.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/Makefile b/Makefile deleted file mode 100644 index b75b928..0000000 --- a/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/README.rst b/README.rst new file mode 100644 index 0000000..a877a84 --- /dev/null +++ b/README.rst @@ -0,0 +1,26 @@ +Overview +======== + +This ROS_ package provides an `Axis network camera`_ driver, written +in Python. + +ROS wiki documentation: `axis_camera`_. + +This driver is under active development. Its ROS interfaces are +relatively stable, but may still change. + +There is no released code API. + +**Warning**:: + + The master branch normally contains code being tested for the next + ROS release. It does not always work with previous ROS distributions. + Sometimes, it may not work at all. + +Each official release is tagged in the repository. The `change +history`_ describes every version. + +.. _`Axis network camera`: http://www.axis.com/products/video/camera/index.htm +.. _`change history`: https://github.com/clearpathrobotics/axis_camera/blob/master/CHANGELOG.rst +.. _`axis_camera`: http://ros.org/wiki/axis_camera +.. _ROS: http://ros.org diff --git a/conf.py b/conf.py new file mode 100644 index 0000000..1713534 --- /dev/null +++ b/conf.py @@ -0,0 +1,250 @@ +# -*- coding: utf-8 -*- +# +# axis_camera documentation build configuration file, created by +# sphinx-quickstart on Wed Apr 25 16:01:20 2012. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. + +#sys.path.insert(0, os.path.abspath('./src')) + +# -- General configuration ----------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.intersphinx', 'sphinx.ext.todo', 'sphinx.ext.viewcode'] + +# List automatically-documented module members in source code order. +autodoc_member_order = 'bysource' + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'axis_camera' +copyright = u'2012, Ryan Gariepy' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.1' +# The full version, including alpha/beta/rc tags. +release = '0.1.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = [] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'axis_camera_doc' + + +# -- Options for LaTeX output -------------------------------------------------- + +latex_elements = { +# The paper size ('letterpaper' or 'a4paper'). +#'papersize': 'letterpaper', + +# The font size ('10pt', '11pt' or '12pt'). +#'pointsize': '10pt', + +# Additional stuff for the LaTeX preamble. +#'preamble': '', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'axis_camera.tex', u'ROS Axis camera driver.', + u'Ryan Gariepy', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + + +# -- Options for manual page output -------------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + ('index', 'axis_camera', u'ROS Python Axis camera driver.', + [u'Ryan Gariepy'], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------------ + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ('index', 'axis_camera', u'axis_camera Documentation', + u'Ryan Gariepy', 'axis_camera', 'ROS Axis camera driver.', + 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'http://docs.python.org/': None} diff --git a/index.rst b/index.rst new file mode 100644 index 0000000..de235e0 --- /dev/null +++ b/index.rst @@ -0,0 +1,16 @@ +camera_info_manager: ROS CameraInfo support for Python camera drivers +===================================================================== + +Contents: + +.. toctree:: + :maxdepth: 2 + + README + CHANGELOG + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` diff --git a/manifest.xml b/manifest.xml deleted file mode 100644 index a059ee8..0000000 --- a/manifest.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - Contains basic Python drivers for accessing an Axis camera's MJPG stream. Also provides control for PTZ cameras. - - Ryan Gariepy - BSD - - http://ros.org/wiki/axis_camera - - - - - - - - diff --git a/nodes/axis.py b/nodes/axis.py index bdea595..7a8d165 100755 --- a/nodes/axis.py +++ b/nodes/axis.py @@ -1,163 +1,204 @@ #!/usr/bin/env python # # Axis camera image driver. Based on: -# https://code.ros.org/svn/wg-ros-pkg/branches/trunk_cturtle/sandbox/axis_camera/axis.py +# https://code.ros.org/svn/wg-ros-pkg/branches/trunk_cturtle/sandbox/axis_camera +# /axis.py # -import os, sys, string, time +import threading import urllib2 -import roslib; roslib.load_manifest('axis_camera') import rospy - from sensor_msgs.msg import CompressedImage, CameraInfo - -import threading import camera_info_manager class StreamThread(threading.Thread): - def __init__(self, axis): - threading.Thread.__init__(self) - - self.axis = axis - self.daemon = True - - def run(self): - while True: - try: - self.stream() - except: - import traceback - traceback.print_exc() - time.sleep(1) - - def stream(self): - url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname - url = url + "?fps=0&resolultion=%dx%d" % (self.axis.width, self.axis.height) - - rospy.logdebug('opening ' + str(self.axis)) - - # only try to authenticate if user/pass configured - if self.axis.password != '' and self.axis.username != '': - # create a password manager - password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm() - - # Add the username and password, use default realm. - top_level_url = "http://" + self.axis.hostname - password_mgr.add_password(None, top_level_url, - self.axis.username, - self.axis.password) - handler = urllib2.HTTPBasicAuthHandler(password_mgr) - - # create "opener" (OpenerDirector instance) - opener = urllib2.build_opener(handler) - - # ...and install it globally so it can be used with urlopen. - urllib2.install_opener(opener) - - fp = urllib2.urlopen(url) - - while True: - boundary = fp.readline() - - header = {} - while 1: - line = fp.readline() - if line == "\r\n": break - line = line.strip() - - parts = line.split(": ", 1) - header[parts[0]] = parts[1] - - content_length = int(header['Content-Length']) - - img = fp.read(content_length) - line = fp.readline() - - msg = CompressedImage() - msg.header.stamp = rospy.Time.now() - msg.header.frame_id = self.axis.frame_id - msg.format = "jpeg" - msg.data = img - - self.axis.pub.publish(msg) - - cimsg = self.axis.cinfo.getCameraInfo() - cimsg.header.stamp = msg.header.stamp - cimsg.header.frame_id = self.axis.frame_id - cimsg.width = self.axis.width - cimsg.height = self.axis.height - self.axis.caminfo_pub.publish(cimsg) + def __init__(self, axis): + threading.Thread.__init__(self) + self.axis = axis + self.daemon = True + self.timeoutSeconds = 2.5 + + def run(self): + while(True): + self.stream() + + def stream(self): + while(True): + self.formURL() + self.authenticate() + if self.openURL(): + self.publishFramesContinuously() + rospy.sleep(2) # if stream stays intact we shouldn't get to this + + def formURL(self): + self.url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname + self.url += "?fps=0&resolultion=%dx%d" % (self.axis.width, + self.axis.height) + rospy.logdebug('opening ' + str(self.axis)) + + def authenticate(self): + '''only try to authenticate if user/pass configured. I have not + used this method (yet).''' + if self.axis.password != '' and self.axis.username != '': + # create a password manager + password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm() + + # Add the username and password, use default realm. + top_level_url = "http://" + self.axis.hostname + password_mgr.add_password(None, top_level_url, self.axis.username, + self.axis.password) + handler = urllib2.HTTPBasicAuthHandler(password_mgr) + + # create "opener" (OpenerDirector instance) + opener = urllib2.build_opener(handler) + + # ...and install it globally so it can be used with urlopen. + urllib2.install_opener(opener) + + def openURL(self): + '''Open connection to Axis camera using http''' + try: + self.fp = urllib2.urlopen(self.url, timeout=self.timeoutSeconds) + return(True) + except urllib2.URLError, e: + rospy.logwarn('Error opening URL %s' % (self.url) + + 'Possible timeout. Looping until camera appears') + return(False) + + def publishFramesContinuously(self): + '''Continuous loop to publish images''' + while(True): + try: + self.findBoundary() + self.getImage() + self.publishMsg() + except: + rospy.loginfo('Timed out while trying to get message.') + break + + def findBoundary(self): + '''The string "--myboundary" is used to denote the start of an image in + Axis cameras''' + while(True): + boundary = self.fp.readline() + if boundary=='--myboundary\r\n': + break + + def getImage(self): + '''Get the image header and image itself''' + self.getHeader() + self.getImageData() + + def getHeader(self): + self.header = {} + while(True): + line = self.fp.readline() + if line == "\r\n": + break + line = line.strip() + parts = line.split(": ", 1) + try: + self.header[parts[0]] = parts[1] + except: + rospy.logwarn('Problem encountered with image header. Setting ' + 'content_length to zero') + self.header['Content-Length'] = 0 # set content_length to zero if + # there is a problem reading header + self.content_length = int(self.header['Content-Length']) + + def getImageData(self): + '''Get the binary image data itself (ie. without header)''' + if self.content_length>0: + self.img = self.fp.read(self.content_length) + self.fp.readline() # Read terminating \r\n and do nothing with it + + def publishMsg(self): + '''Publish jpeg image as a ROS message''' + self.msg = CompressedImage() + self.msg.header.stamp = rospy.Time.now() + self.msg.header.frame_id = self.axis.frame_id + self.msg.format = "jpeg" + self.msg.data = self.img + self.axis.pub.publish(self.msg) + + def publishCameraInfoMsg(self): + '''Publish camera info manager message''' + cimsg = self.axis.cinfo.getCameraInfo() + cimsg.header.stamp = msg.header.stamp + cimsg.header.frame_id = self.axis.frame_id + cimsg.width = self.axis.width + cimsg.height = self.axis.height + self.axis.caminfo_pub.publish(cimsg) class Axis: - def __init__(self, hostname, username, password, - width, height, frame_id, camera_info_url): - self.hostname = hostname - self.username = username - self.password = password - self.width = width - self.height = height - self.frame_id = frame_id - self.camera_info_url = camera_info_url - - # generate a valid camera name based on the hostname - self.cname = camera_info_manager.genCameraName(self.hostname) - self.cinfo = camera_info_manager.CameraInfoManager(cname = self.cname, - url = self.camera_info_url) - self.cinfo.loadCameraInfo() # required before getCameraInfo() - self.st = None - self.pub = rospy.Publisher("image_raw/compressed", CompressedImage, self) - self.caminfo_pub = rospy.Publisher("camera_info", CameraInfo, self) - - def __str__(self): - """Return string representation.""" - return(self.hostname + ',' + self.username + ',' + self.password + - '(' + str(self.width) + 'x' + str(self.height) + ')') - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - # Lazy-start the image-publisher. - if self.st is None: - self.st = StreamThread(self) - self.st.start() + def __init__(self, hostname, username, password, width, height, frame_id, + camera_info_url): + self.hostname = hostname + self.username = username + self.password = password + self.width = width + self.height = height + self.frame_id = frame_id + self.camera_info_url = camera_info_url + + # generate a valid camera name based on the hostname + self.cname = camera_info_manager.genCameraName(self.hostname) + self.cinfo = camera_info_manager.CameraInfoManager(cname = self.cname, + url = self.camera_info_url) + self.cinfo.loadCameraInfo() # required before getCameraInfo() + self.st = None + self.pub = rospy.Publisher("image_raw/compressed", CompressedImage, self) + self.caminfo_pub = rospy.Publisher("camera_info", CameraInfo, self) + + def __str__(self): + """Return string representation.""" + return(self.hostname + ',' + self.username + ',' + self.password + + '(' + str(self.width) + 'x' + str(self.height) + ')') + + def peer_subscribe(self, topic_name, topic_publish, peer_publish): + '''Lazy-start the image-publisher.''' + if self.st is None: + self.st = StreamThread(self) + self.st.start() def main(): - rospy.init_node("axis_driver") - - arg_defaults = { - 'hostname': '192.168.0.90', # default IP address - 'username': 'root', # default login name - 'password': '', - 'width': 640, - 'height': 480, - 'frame_id': 'axis_camera', - 'camera_info_url': ''} - - # Look up parameters starting in the driver's private parameter - # space, but also searching outer namespaces. Defining them in a - # higher namespace allows the axis_ptz.py script to share parameters - # with the driver. - args = {} - for name, val in arg_defaults.iteritems(): - full_name = rospy.search_param(name) - if full_name is None: - args[name] = val - else: - args[name] = rospy.get_param(full_name, val) - - # resolve frame_id with tf_prefix (unless already absolute) - if args['frame_id'][0] != '/': # not absolute? - tf_prefix = rospy.search_param('tf_prefix') - prefix_val = '' - if tf_prefix is not None: # prefix defined? - prefix_val = rospy.get_param(tf_prefix) - if prefix_val[0] != '/': # prefix not absolute? - prefix_val = '/' + prefix_val - args['frame_id'] = prefix_val + '/' + args['frame_id'] - - Axis(**args) - rospy.spin() - + rospy.init_node("axis_driver") + + arg_defaults = { + 'hostname': '192.168.0.90', # default IP address + 'username': 'root', # default login name + 'password': '', + 'width': 640, + 'height': 480, + 'frame_id': 'axis_camera', + 'camera_info_url': ''} + args = updateArgs(arg_defaults) + Axis(**args) + rospy.spin() + +def updateArgs(arg_defaults): + '''Look up parameters starting in the driver's private parameter space, but + also searching outer namespaces. Defining them in a higher namespace allows + the axis_ptz.py script to share parameters with the driver.''' + args = {} + for name, val in arg_defaults.iteritems(): + full_name = rospy.search_param(name) + if full_name is None: + args[name] = val + else: + args[name] = rospy.get_param(full_name, val) + # resolve frame_id with tf_prefix (unless already absolute) + if args['frame_id'][0] != '/': # not absolute? + tf_prefix = rospy.search_param('tf_prefix') + prefix_val = '' + if tf_prefix is not None: # prefix defined? + prefix_val = rospy.get_param(tf_prefix) + if prefix_val[0] != '/': # prefix not absolute? + prefix_val = '/' + prefix_val + args['frame_id'] = prefix_val + '/' + args['frame_id'] + return(args) if __name__ == "__main__": - main() + main() diff --git a/nodes/axis_all.py b/nodes/axis_all.py deleted file mode 100755 index 335c486..0000000 --- a/nodes/axis_all.py +++ /dev/null @@ -1,141 +0,0 @@ -#!/usr/bin/env python -# -# Basic PTZ node, based on documentation here: -# http://www.axis.com/files/manuals/vapix_ptz_45621_en_1112.pdf -# - -import os, sys, string, time, getopt, threading -import httplib, urllib - -import roslib; roslib.load_manifest('axis_camera') -import rospy - -from axis_camera.msg import Axis -from geometry_msgs.msg import Twist -import math - - -class StateThread(threading.Thread): - def __init__(self, axis): - threading.Thread.__init__(self) - - self.axis = axis - self.daemon = True - - def run(self): - r = rospy.Rate(10) - msg = Axis() - - while True: - if not self.axis.twist_timeout and ((rospy.Time.now() - self.axis.last_request).to_sec() > 1.0): - self.axis.twist_timeout = True - self.axis.cmd_twist(Twist(),reset_timeout=False) - conn = httplib.HTTPConnection(self.axis.hostname) - params = { 'query':'position' } - conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params)) - response = conn.getresponse() - if response.status == 200: - body = response.read() - params = dict([s.split('=',2) for s in body.splitlines()]) - msg.pan = float(params['pan']) - # Flip pan orient if the camera is mounted backwards and facing down - if self.axis.flip: - msg.pan = 180 - msg.pan - if msg.pan > 180: - msg.pan -= 360 - if msg.pan < -180: - msg.pan += 360 - msg.tilt = float(params['tilt']) - msg.zoom = float(params['zoom']) - msg.iris = float(params['iris']) - msg.focus = 0.0 - if 'focus' in params: - msg.focus = float(params['focus']) - msg.autofocus = (params['autofocus'] == 'on') - self.axis.pub.publish(msg) - r.sleep() - - -class AxisPTZ: - def __init__(self, hostname, username, password, flip): - self.hostname = hostname - self.username = username - self.password = password - self.flip = flip - - self.st = None - self.twist_timeout = True - self.pub = rospy.Publisher("state", Axis, self) - self.sub = rospy.Subscriber("twist", Twist, self.cmd_twist) - self.sub = rospy.Subscriber("cmd", Axis, self.cmd_abs) - self.cmd_twist(Twist(),reset_timeout=False) - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - # Lazy-start the state publisher. - if self.st is None: - self.st = StateThread(self) - self.st.start() - - def cmd_abs(self, msg): - if not self.twist_timeout: - self.axis.twist_timeout = True - self.axis.cmd_twist(Twist(),reset_timeout=False) - conn = httplib.HTTPConnection(self.hostname) - # Flip pan orient if the camera is mounted backwards and facing down - if self.flip: - msg.pan = 180 - msg.pan - if msg.pan > 180: - msg.pan -= 360 - if msg.pan < -180: - msg.pan += 360 - params = { 'pan': msg.pan, 'tilt': msg.tilt, 'zoom': msg.zoom, 'brightness': msg.brightness } - if msg.autofocus: - params['autofocus'] = 'on' - else: - params['autofocus'] = 'off' - params['focus'] = msg.focus - if msg.iris > 0.000001: - rospy.logwarn("Iris value is read-only.") - conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params)) - - def cmd_twist(self, msg, reset_timeout=True): - if reset_timeout: - self.twist_timeout = False - self.last_request = rospy.Time.now() - conn = httplib.HTTPConnection(self.hostname) - pan = int(msg.angular.z * 180./math.pi) - tilt = int(msg.angular.y * 180./math.pi) - if pan>100: - pan=100 - if pan<-100: - pan=-100 - if tilt>100: - tilt=100 - if tilt<-100: - tilt=-100 - # Flip pan orient if the camera is mounted backwards and facing down - if self.flip: - pan = -pan - tilt = -tilt - conn.request("GET", "/axis-cgi/com/ptz.cgi?continuouspantiltmove=%d,%d" % (pan,tilt)) - - -def main(): - rospy.init_node("axis_twist") - - arg_defaults = { - 'hostname': '192.168.0.90', - 'username': '', - 'password': '', - 'flip': True, - } - args = {} - for name in arg_defaults: - args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) - - AxisPTZ(**args) - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/nodes/axis_ptz.py b/nodes/axis_ptz.py index 756f44c..d32de16 100755 --- a/nodes/axis_ptz.py +++ b/nodes/axis_ptz.py @@ -3,107 +3,253 @@ # Basic PTZ node, based on documentation here: # http://www.axis.com/files/manuals/vapix_ptz_45621_en_1112.pdf # - -import os, sys, string, time, getopt, threading +import threading import httplib, urllib - -import roslib; roslib.load_manifest('axis_camera') import rospy - from axis_camera.msg import Axis - +from std_msgs.msg import Bool +import math class StateThread(threading.Thread): - def __init__(self, axis): - threading.Thread.__init__(self) - - self.axis = axis - self.daemon = True - - def run(self): - r = rospy.Rate(10) - msg = Axis() - - while True: - conn = httplib.HTTPConnection(self.axis.hostname) - params = { 'query':'position' } - conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params)) - response = conn.getresponse() - if response.status == 200: - body = response.read() - params = dict([s.split('=',2) for s in body.splitlines()]) - msg.pan = float(params['pan']) - # Flip pan orient if the camera is mounted backwards and facing down - if self.axis.flip: - msg.pan = 180 - msg.pan - if msg.pan > 180: - msg.pan -= 360 - if msg.pan < -180: - msg.pan += 360 - msg.tilt = float(params['tilt']) - msg.zoom = float(params['zoom']) - msg.iris = float(params['iris']) - msg.focus = 0.0 - if 'focus' in params: - msg.focus = float(params['focus']) - msg.autofocus = (params['autofocus'] == 'on') - self.axis.pub.publish(msg) - r.sleep() - + '''This class handles the publication of the positional state of the camera + to a ROS message''' + + def __init__(self, axis): + threading.Thread.__init__(self) + self.axis = axis + # Permit program to exit even if threads are still running by flagging + # thread as a daemon: + self.daemon = True + + def run(self): + r = rospy.Rate(1) + self.msg = Axis() + + while True: + self.queryCameraPosition() + self.publishCameraState() + r.sleep() + + def queryCameraPosition(self): + '''Using Axis VAPIX protocol, described in the comments at the top of + this file, is used to query the state of the camera''' + conn = httplib.HTTPConnection(self.axis.hostname) + queryParams = { 'query':'position' } + try: + conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % + urllib.urlencode(queryParams)) + response = conn.getresponse() + # Response code 200 means there are data to be read: + if response.status == 200: + body = response.read() + self.cameraPosition = dict([s.split('=',2) for s in + body.splitlines()]) + else: + self.cameraPosition = None + except: + rospy.logwarn('Encountered a problem getting a repsonse from %s. ' + 'Possibly a problem with the network connection.' % + self.axis.hostname) + self.cameraPosition = None + + def publishCameraState(self): + '''Publish camera state to a ROS message''' + if self.cameraPosition is not None: + self.msg.pan = float(self.cameraPosition['pan']) + if self.axis.flip: + self.adjustForFlippedOrientation() + self.msg.tilt = float(self.cameraPosition['tilt']) + self.msg.zoom = float(self.cameraPosition['zoom']) + self.msg.iris = 0.0 + if 'iris' in self.cameraPosition: + self.msg.iris = float(self.cameraPosition['iris']) + self.msg.focus = 0.0 + if 'focus' in self.cameraPosition: + self.msg.focus = float(self.cameraPosition['focus']) + self.msg.autofocus = (self.cameraPosition['autofocus'] == 'on') + self.axis.pub.publish(self.msg) + + def adjustForFlippedOrientation(self): + '''Correct pan and tilt parameters if camera is mounted backwards and + facing down''' + self.msg.pan = 180 - self.msg.pan + if self.msg.pan > 180: + self.msg.pan -= 360 + elif self.msg.pan < -180: + self.msg.pan += 360 + self.msg.tilt = -self.msg.tilt class AxisPTZ: - def __init__(self, hostname, username, password, flip): - self.hostname = hostname - self.username = username - self.password = password - self.flip = flip - - self.st = None - self.pub = rospy.Publisher("state", Axis, self) - self.sub = rospy.Subscriber("cmd", Axis, self.cmd) - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - # Lazy-start the state publisher. - if self.st is None: - self.st = StateThread(self) - self.st.start() - - def cmd(self, msg): - conn = httplib.HTTPConnection(self.hostname) - # Flip pan orient if the camera is mounted backwards and facing down - if self.flip: - msg.pan = 180 - msg.pan - if msg.pan > 180: - msg.pan -= 360 - if msg.pan < -180: - msg.pan += 360 - params = { 'pan': msg.pan, 'tilt': msg.tilt, 'zoom': msg.zoom, 'brightness': msg.brightness } - if msg.autofocus: - params['autofocus'] = 'on' - else: - params['autofocus'] = 'off' - params['focus'] = msg.focus - if msg.iris > 0.000001: - rospy.logwarn("Iris value is read-only.") - conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params)) - + '''This class creates a node to manage the PTZ functions of an Axis PTZ + camera''' + def __init__(self, hostname, username, password, flip, speed_control): + self.hostname = hostname + self.username = username + self.password = password + self.flip = flip + # speed_control is true for speed control and false for + # position control: + self.speedControl = speed_control + self.mirror = False + + self.st = None + self.pub = rospy.Publisher("state", Axis, self) + self.sub = rospy.Subscriber("cmd", Axis, self.cmd, queue_size=1) + self.sub_mirror = rospy.Subscriber("mirror", Bool, self.mirrorCallback, + queue_size=1) + + def peer_subscribe(self, topic_name, topic_publish, peer_publish): + '''Lazy-start the state publisher.''' + if self.st is None: + self.st = StateThread(self) + self.st.start() + + def cmd(self, msg): + '''Command the camera with speed control or position control commands''' + self.msg = msg + if self.flip: + self.adjustForFlippedOrientation() + if self.mirror: + self.msg.pan = -self.msg.pan + self.sanitisePTZCommands() + self.applySetpoints() + + def adjustForFlippedOrientation(self): + '''If camera is mounted backwards and upside down (ie. self.flip==True + then apply appropriate transforms to pan and tilt''' + self.msg.tilt = -self.msg.tilt + if self.speedControl: + self.msg.pan = -self.msg.pan + else: + self.msg.pan = 180.0 - self.msg.pan + + def sanitisePTZCommands(self): + '''Applies limits to message and corrects for flipped camera if + necessary''' + self.sanitisePan() + self.sanitiseTilt() + self.sanitiseZoom() + self.sanitiseFocus() + self.sanitiseBrightness() + self.sanitiseIris() + + def sanitisePan(self): + '''Pan speed (in percent) must be: -100100.0: + self.msg.pan = math.copysign(100.0, self.msg.pan) + else: # position control so need to ensure -180100.0: + self.msg.tilt = math.copysign(100.0, self.msg.tilt) + else: # position control so ensure tilt: -180100: + self.msg.zoom = math.copysign(100.0, self.msg.zoom) + else: # position control: + if self.msg.zoom>9999.0: + self.msg.zoom = 9999.0 + elif self.msg.zoom<1.0: + self.msg.zoom = 1.0 + + def sanitiseFocus(self): + '''Focus must be: 1100.0: + self.msg.focus = math.copysign(100.0, self.msg.focus) + else: # position control: + if self.msg.focus>9999.0: + self.msg.focus = 9999.0 + elif self.msg.focus < 1.0: + self.msg.focus = 1.0 + + def sanitiseBrightness(self): + '''Brightness must be: 1 100.0: + self.msg.brightness = math.copysign(100.0, self.msg.brightness) + else: # position control: + if self.msg.brightness>9999.0: + self.msg.brightness = 9999.0 + elif self.msg.brightness<1.0: + self.msg.brightness = 1.0 + + def sanitiseIris(self): + '''Iris value is read only because autoiris has been set to "on"''' + if self.msg.iris>0.000001: + rospy.logwarn("Iris value is read-only.") + + def applySetpoints(self): + '''Apply set-points to camera via HTTP''' + conn = httplib.HTTPConnection(self.hostname) + self.createCmdString() + try: + conn.request('GET', self.cmdString) + except: + rospy.logwarn('Failed to connect to camera to send command message') + conn.close() + + def createCmdString(self): + '''creates http cgi string to command PTZ camera''' + self.cmdString = '/axis-cgi/com/ptz.cgi?' + if self.speedControl: + self.cmdString += 'continuouspantiltmove=%d,%d&' % \ + (int(self.msg.pan), int(self.msg.tilt)) \ + + 'continuouszoommove=%d&' % (int(self.msg.zoom)) \ + + 'continuousbrightnessmove=%d&' % \ + (int(self.msg.brightness)) + # Note that brightness adjustment has no effect for Axis 214PTZ. + if self.msg.autofocus: + self.cmdString += 'autofocus=on&' + else: + self.cmdString += 'autofocus=off&continuousfocusmove=%d&' % \ + (int(self.msg.focus)) + self.cmdString += 'autoiris=on' + else: # position control: + self.cmdString += 'pan=%d&tilt=%d&' % (self.msg.pan, self.msg.tilt)\ + + 'zoom=%d&' % (int(self.msg.zoom)) \ + + 'brightness=%d&' % (int(self.msg.brightness)) + if self.msg.autofocus: + self.cmdString += 'autofocus=on&' + else: + self.cmdString += 'autofocus=off&focus=%d&' % \ + (int(self.msg.focus)) + self.cmdString += 'autoiris=on' + + def mirrorCallback(self, msg): + '''Command the camera with speed control or position control commands''' + self.mirror = msg.data def main(): - rospy.init_node("axis") - - arg_defaults = { - 'hostname': '192.168.0.90', - 'username': '', - 'password': '', - 'flip': True, - } - args = {} - for name in arg_defaults: - args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) - - AxisPTZ(**args) - rospy.spin() - + rospy.init_node("axis_twist") + + arg_defaults = { + 'hostname': '192.168.0.90', + 'username': '', + 'password': '', + 'flip': True, + 'speed_control': False + } + args = {} + for name in arg_defaults: + args[name] = rospy.get_param(rospy.search_param(name), + arg_defaults[name]) + + AxisPTZ(**args) + rospy.spin() if __name__ == "__main__": - main() + main() diff --git a/nodes/axis_twist.py b/nodes/axis_twist.py deleted file mode 100755 index ae133ad..0000000 --- a/nodes/axis_twist.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python -# -# Basic PTZ node, based on documentation here: -# http://www.axis.com/files/manuals/vapix_ptz_45621_en_1112.pdf -# - -import os, sys, string, time, getopt, threading -import httplib, urllib - -import roslib; roslib.load_manifest('axis_camera') -import rospy - -from axis_camera.msg import Axis -from geometry_msgs.msg import Twist -import math - - -class StateThread(threading.Thread): - def __init__(self, axis): - threading.Thread.__init__(self) - - self.axis = axis - self.daemon = True - - def run(self): - r = rospy.Rate(10) - msg = Axis() - - while True: - if not self.axis.timeout and ((rospy.Time.now() - self.axis.last_request).to_sec() > 1.0): - self.axis.timeout = True - self.axis.cmd(Twist(),reset_timeout=False) - conn = httplib.HTTPConnection(self.axis.hostname) - params = { 'query':'position' } - conn.request("GET", "/axis-cgi/com/ptz.cgi?%s" % urllib.urlencode(params)) - response = conn.getresponse() - if response.status == 200: - body = response.read() - params = dict([s.split('=',2) for s in body.splitlines()]) - msg.pan = float(params['pan']) - # Flip pan orient if the camera is mounted backwards and facing down - if self.axis.flip: - msg.pan = 180 - msg.pan - if msg.pan > 180: - msg.pan -= 360 - if msg.pan < -180: - msg.pan += 360 - msg.tilt = float(params['tilt']) - msg.zoom = float(params['zoom']) - msg.iris = float(params['iris']) - msg.focus = 0.0 - if 'focus' in params: - msg.focus = float(params['focus']) - msg.autofocus = (params['autofocus'] == 'on') - self.axis.pub.publish(msg) - r.sleep() - - -class AxisPTZ: - def __init__(self, hostname, username, password, flip): - self.hostname = hostname - self.username = username - self.password = password - self.flip = flip - - self.st = None - self.timeout = True - self.pub = rospy.Publisher("state", Axis, self) - self.sub = rospy.Subscriber("twist", Twist, self.cmd) - self.cmd(Twist(),reset_timeout=False) - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - # Lazy-start the state publisher. - if self.st is None: - self.st = StateThread(self) - self.st.start() - - def cmd(self, msg, reset_timeout=True): - if reset_timeout: - self.timeout = False - self.last_request = rospy.Time.now() - conn = httplib.HTTPConnection(self.hostname) - pan = int(msg.angular.z * 180./math.pi) - tilt = int(msg.angular.y * 180./math.pi) - if pan>100: - pan=100 - if pan<-100: - pan=-100 - if tilt>100: - tilt=100 - if tilt<-100: - tilt=-100 - # Flip pan orient if the camera is mounted backwards and facing down - if self.flip: - pan = -pan - tilt = -tilt - conn.request("GET", "/axis-cgi/com/ptz.cgi?continuouspantiltmove=%d,%d" % (pan,tilt)) - - -def main(): - rospy.init_node("axis_twist") - - arg_defaults = { - 'hostname': '192.168.0.90', - 'username': '', - 'password': '', - 'flip': True, - } - args = {} - for name in arg_defaults: - args[name] = rospy.get_param(rospy.search_param(name), arg_defaults[name]) - - AxisPTZ(**args) - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/nodes/publish_axis_tf.py b/nodes/publish_axis_tf.py index df4eefb..97b1a1e 100755 --- a/nodes/publish_axis_tf.py +++ b/nodes/publish_axis_tf.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -import roslib; roslib.load_manifest('axis_camera') + import rospy import math from axis_camera.msg import Axis diff --git a/nodes/teleop.py b/nodes/teleop.py index 7a740cc..e0dbb41 100755 --- a/nodes/teleop.py +++ b/nodes/teleop.py @@ -1,8 +1,6 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('axis_camera') import rospy - from sensor_msgs.msg import Joy from axis_camera.msg import Axis diff --git a/nodes/teleop_speed_control.py b/nodes/teleop_speed_control.py new file mode 100755 index 0000000..fc7d6ee --- /dev/null +++ b/nodes/teleop_speed_control.py @@ -0,0 +1,75 @@ +#!/usr/bin/python + +import rospy +from sensor_msgs.msg import Joy +from axis_camera.msg import Axis +from std_msgs.msg import Bool + +class Teleop: + def __init__(self): + rospy.init_node('axis_ptz_speed_controller') + self.initialiseVariables() + self.pub = rospy.Publisher('cmd', Axis) + rospy.Subscriber("joy", Joy, self.joy_callback, queue_size=1) + self.pub_mirror = rospy.Publisher('mirror', Bool) + + def initialiseVariables(self): + self.joy = None + self.msg = Axis() # instantiate Axis message + self.msg.autofocus = True # autofocus is on by default + # sensitivities[0..5] corresponding to fwd, left, up, tilt right, + # tilt forwards, anticlockwise twist + self.mirror = False + self.mirror_already_actioned = False # to stop mirror flip-flopping + self.sensitivities = [120, -60, 40, 0, 0, 30] + self.deadband = [0.2, 0.2, 0.2, 0.2, 0.4, 0.4] + + def spin(self): + self.pub.publish(self.msg) + r = rospy.Rate(5) + while not rospy.is_shutdown(): + if self.joy != None: + self.createCmdMessage() + self.createMirrorMessage() + r.sleep() + + def createCmdMessage(self): + '''Creates and publishes message to command the camera. Spacenav axes + are: [fwd, left, up, tilt_right, tilt_forward, twist_anticlockwise''' + self.applyThresholds() + self.msg.pan = self.axes_thresholded[1] * self.sensitivities[1] + self.msg.tilt = self.axes_thresholded[2] * self.sensitivities[2] + self.msg.zoom = self.axes_thresholded[0] * self.sensitivities[0] + if self.joy.buttons[0]==1: + self.msg.autofocus = True + else: + self.msg.focus = self.axes_thresholded[5] * self.sensitivities[5] + if (abs(self.msg.focus)>0.00001): + # Only turn autofocus off if msg.focus!=0 + self.msg.autofocus = False + self.pub.publish(self.msg) + + def applyThresholds(self): + '''apply deadband to joystick output''' + n = len(self.joy.axes) + self.axes_thresholded = n * [0.0] + for i in range(n): + if (abs(self.joy.axes[i])>self.deadband[i]): + self.axes_thresholded[i] = self.joy.axes[i] + + def joy_callback(self, data): + self.joy = data + + def createMirrorMessage(self): + '''Creates and publishes message to indicate image should be mirrored''' + if self.joy.buttons[1]==1: + if not self.mirror_already_actioned: + self.mirror = not self.mirror + self.mirror_already_actioned = True + else: + self.mirror_already_actioned = False + self.pub_mirror.publish(Bool(self.mirror)) + +if __name__ == "__main__": + teleop = Teleop() + teleop.spin() diff --git a/nodes/teleop_twist.py b/nodes/teleop_twist.py deleted file mode 100755 index ec2e452..0000000 --- a/nodes/teleop_twist.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/python - -import roslib; roslib.load_manifest('axis_camera') -import rospy - -import math -from sensor_msgs.msg import Joy -from axis_camera.msg import Axis -from geometry_msgs.msg import Twist - - -class Teleop: - def __init__(self): - rospy.init_node('axis_twist_teleop') - self.enable_button = rospy.get_param('~enable_button', 1) - self.zero_button = rospy.get_param('~zero_button', 2) - self.scale_pan = rospy.get_param('~scale_pan_deg', 10) - self.scale_tilt = rospy.get_param('~scale_tilt_deg', 10) - self.state = Axis(pan=180,tilt=0,zoom=1) - self.joy = None - - self.cmd_pub = rospy.Publisher('cmd', Axis) - self.twist_pub = rospy.Publisher('twist', Twist) - rospy.Subscriber("/joy", Joy, self.joy_callback) - # rospy.Subscriber("state", Axis, self.state_callback) - - def spin(self): - twist = Twist() - self.cmd_pub.publish(self.state) - r = rospy.Rate(5) - while not rospy.is_shutdown(): - if self.joy != None and self.joy.buttons[self.enable_button] == 1: - twist.angular.z = self.joy.axes[0]*self.scale_pan*math.pi/180. - twist.angular.y = -self.joy.axes[1]*self.scale_tilt*math.pi/180. - self.twist_pub.publish(twist) - if self.joy != None and self.joy.buttons[self.zero_button] == 1: - self.cmd_pub.publish(self.state) - r.sleep() - - def joy_callback(self, data): - self.joy = data - - -if __name__ == "__main__": Teleop().spin() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..ff06e93 --- /dev/null +++ b/package.xml @@ -0,0 +1,37 @@ + + + axis_camera + 0.2.0 + + Python ROS drivers for accessing an Axis camera's MJPG + stream. Also provides control for PTZ cameras. + + Mike Purvis + Ryan Gariepy + BSD + + http://ros.org/wiki/axis_camera + https://github.com/clearpathrobotics/axis_camera + https://github.com/clearpathrobotics/axis_camera/issues + + catkin + + camera_info_manager_py + geometry_msgs + message_generation + rospy + sensor_msgs + tf + + camera_info_manager_py + geometry_msgs + message_runtime + rospy + sensor_msgs + tf + + + + + + diff --git a/rosdoc.yaml b/rosdoc.yaml new file mode 100644 index 0000000..1bda6fb --- /dev/null +++ b/rosdoc.yaml @@ -0,0 +1,4 @@ + - builder: sphinx + name: Python API + output_dir: python + diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..6317e60 --- /dev/null +++ b/setup.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + install_requires=['rospy', 'urllib2'], + ) + +setup(**d) diff --git a/stack.xml b/stack.xml deleted file mode 100644 index 012236e..0000000 --- a/stack.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - Contains basic Python drivers for accessing an Axis camera's MJPG stream. Also provides control for PTZ cameras. - - Maintained by Ryan Gariepy - BSD - - http://ros.org/wiki/axis_camera - - - - - diff --git a/tests/test_absolute.launch b/tests/test_absolute.launch new file mode 100644 index 0000000..c0ce787 --- /dev/null +++ b/tests/test_absolute.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/test_flipped_speed_control.launch b/tests/test_flipped_speed_control.launch new file mode 100644 index 0000000..074c84a --- /dev/null +++ b/tests/test_flipped_speed_control.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/test_speed_control.launch b/tests/test_speed_control.launch new file mode 100644 index 0000000..4d181a9 --- /dev/null +++ b/tests/test_speed_control.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + +