From 4632eacf75c2703246e0d44e81f3be5a2a424fba Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 18:48:56 -0400 Subject: [PATCH 01/20] Port custom_msgs to ROS2 (ament_cmake + rosidl) - Rename messages to ROS2-legal CamelCase: GSOF_INS->GsofIns, GSOF_EVT->GsofEvt, EVT->Evt, PASHR->Pashr - Replace removed 'time' builtin with builtin_interfaces/Time - Rename tPvErr srv field to pv_err (ROS2 requires snake_case fields) - Drop unused interfaces: ImageWithMask, POSAVX, CamControl, DiskUsage, EraseDataDisk, SetUInt - msgdispatch: publishers now require an rclpy node; fix add_publisher storing every publisher under the literal key 'name' - Replace catkin build files with ament_cmake + rosidl_default_generators; install msgdispatch via ament_python_install_package --- src/custom_msgs/CMakeLists.txt | 248 +++--------------- .../dispatch/msgdispatch/archive.py | 7 +- src/custom_msgs/dispatch/msgdispatch/base.py | 17 +- src/custom_msgs/msg/{EVT.msg => Evt.msg} | 0 .../msg/{GSOF_EVT.msg => GsofEvt.msg} | 4 +- .../msg/{GSOF_INS.msg => GsofIns.msg} | 4 +- src/custom_msgs/msg/ImageSpaceDetection.msg | 2 +- .../msg/ImageSpaceDetectionList.msg | 2 +- src/custom_msgs/msg/ImageWithMask.msg | 2 - src/custom_msgs/msg/POSAVX.msg | 75 ------ src/custom_msgs/msg/{PASHR.msg => Pashr.msg} | 0 src/custom_msgs/msg/SyncedPathImages.msg | 6 +- src/custom_msgs/package.xml | 60 ++--- src/custom_msgs/setup.py | 9 - src/custom_msgs/srv/CamControl.srv | 29 -- src/custom_msgs/srv/CamGetAttr.srv | 2 +- src/custom_msgs/srv/CamSetAttr.srv | 2 +- src/custom_msgs/srv/DiskUsage.srv | 7 - src/custom_msgs/srv/EraseDataDisk.srv | 10 - src/custom_msgs/srv/SetUInt.srv | 12 - src/custom_msgs/srv/StrList.srv | 2 +- 21 files changed, 75 insertions(+), 425 deletions(-) rename src/custom_msgs/msg/{EVT.msg => Evt.msg} (100%) rename src/custom_msgs/msg/{GSOF_EVT.msg => GsofEvt.msg} (87%) rename src/custom_msgs/msg/{GSOF_INS.msg => GsofIns.msg} (95%) delete mode 100644 src/custom_msgs/msg/ImageWithMask.msg delete mode 100644 src/custom_msgs/msg/POSAVX.msg rename src/custom_msgs/msg/{PASHR.msg => Pashr.msg} (100%) delete mode 100755 src/custom_msgs/setup.py delete mode 100644 src/custom_msgs/srv/CamControl.srv delete mode 100644 src/custom_msgs/srv/DiskUsage.srv delete mode 100644 src/custom_msgs/srv/EraseDataDisk.srv delete mode 100644 src/custom_msgs/srv/SetUInt.srv diff --git a/src/custom_msgs/CMakeLists.txt b/src/custom_msgs/CMakeLists.txt index 940e3ceb..5ece6e11 100644 --- a/src/custom_msgs/CMakeLists.txt +++ b/src/custom_msgs/CMakeLists.txt @@ -1,215 +1,43 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(custom_msgs) -## 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 - message_generation - rospy - sensor_msgs - std_msgs - geometry_msgs +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/ArchiveSchema.msg + msg/Evt.msg + msg/GsofEvt.msg + msg/GsofIns.msg + msg/ImageSpaceDetection.msg + msg/ImageSpaceDetectionList.msg + msg/Pashr.msg + msg/PathImage.msg + msg/Stat.msg + msg/SyncedPathImages.msg + msg/SynchronizedImages.msg + srv/AddToEventLog.srv + srv/CamGetAttr.srv + srv/CamSetAttr.srv + srv/ReadPin.srv + srv/RequestCompressedImageView.srv + srv/RequestImageMetadata.srv + srv/RequestImageView.srv + srv/SetArchiving.srv + srv/SetTriggerRate.srv + srv/StrList.srv + srv/SysCall.srv + srv/TransformDetectionList.srv + DEPENDENCIES builtin_interfaces std_msgs sensor_msgs geometry_msgs ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +# Python helper library for publishing dispatch wrappers around these messages +ament_python_install_package(msgdispatch PACKAGE_DIR dispatch/msgdispatch) - -## 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 - ImageSpaceDetection.msg - ImageSpaceDetectionList.msg - ImageWithMask.msg - POSAVX.msg - PASHR.msg - EVT.msg - GSOF_INS.msg - GSOF_EVT.msg - SynchronizedImages.msg - PathImage.msg - SyncedPathImages.msg - ArchiveSchema.msg - Stat.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - AddToEventLog.srv - CamGetAttr.srv - CamSetAttr.srv - ReadPin.srv - RequestCompressedImageView.srv - RequestImageMetadata.srv - RequestImageView.srv - SetArchiving.srv - SetTriggerRate.srv - SetUInt.srv - StrList.srv - SysCall.srv - TransformDetectionList.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 - sensor_msgs - geometry_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 you 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( - CATKIN_DEPENDS message_runtime -# INCLUDE_DIRS include -# LIBRARIES custom_msgs -# CATKIN_DEPENDS sensor_msgs std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(custom_msgs -# src/${PROJECT_NAME}/custom_msgs.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(custom_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(custom_msgs src/custom_msgs_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(custom_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(custom_msgs_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 custom_msgs custom_msgs_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_custom_msgs.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) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/custom_msgs/dispatch/msgdispatch/archive.py b/src/custom_msgs/dispatch/msgdispatch/archive.py index edaff6d6..b81b3dc5 100644 --- a/src/custom_msgs/dispatch/msgdispatch/archive.py +++ b/src/custom_msgs/dispatch/msgdispatch/archive.py @@ -1,17 +1,16 @@ from . import base from custom_msgs.msg import ArchiveSchema + class ArchiveSchemaDispatch(base.DispatchBase): message_class = ArchiveSchema pubs = {} __slots__ = 'project', 'flight' - def __new__(cls, **kwargs): + def __new__(cls, **kwargs): self = object.__new__(cls) self.msg = self.new_message() self.msg.project = kwargs.get('project', 'default2019') - self.msg.flight = kwargs.get('flight', 'fl00') + self.msg.flight = kwargs.get('flight', 'fl00') return self - -aa = ArchiveSchemaDispatch() diff --git a/src/custom_msgs/dispatch/msgdispatch/base.py b/src/custom_msgs/dispatch/msgdispatch/base.py index b8e37e8c..12a952cd 100644 --- a/src/custom_msgs/dispatch/msgdispatch/base.py +++ b/src/custom_msgs/dispatch/msgdispatch/base.py @@ -1,11 +1,7 @@ -import rospy - # This is an abstract base class class DispatchBase(object): - publisher = None msg = None - # todo: better pattern to manage class-bound variables # message_class, pubs, and optionally counter should be re-defined in each child message_class = None pubs = {} @@ -18,13 +14,14 @@ def next_id(cls): return x @classmethod - def add_publisher(cls, name, queue_size=3): - if name not in cls.pubs: - cls.pubs.update( - {'name': rospy.Publisher(name, cls.message_class, tcp_nodelay=True, - queue_size=queue_size)}) - else: + def add_publisher(cls, node, name, queue_size=3): + """Register a publisher on `node` for this dispatch class. + + In ROS2 publishers are owned by a node, so one must be provided. + """ + if name in cls.pubs: raise ValueError('Publisher already exists: {}'.format(name)) + cls.pubs[name] = node.create_publisher(cls.message_class, name, queue_size) @property def new_message(self): diff --git a/src/custom_msgs/msg/EVT.msg b/src/custom_msgs/msg/Evt.msg similarity index 100% rename from src/custom_msgs/msg/EVT.msg rename to src/custom_msgs/msg/Evt.msg diff --git a/src/custom_msgs/msg/GSOF_EVT.msg b/src/custom_msgs/msg/GsofEvt.msg similarity index 87% rename from src/custom_msgs/msg/GSOF_EVT.msg rename to src/custom_msgs/msg/GsofEvt.msg index 16d26f40..5977a8bf 100644 --- a/src/custom_msgs/msg/GSOF_EVT.msg +++ b/src/custom_msgs/msg/GsofEvt.msg @@ -8,10 +8,10 @@ std_msgs/Header header float64 time # system time of the system receiving the event -time sys_time +builtin_interfaces/Time sys_time # time associated with the gps event message -time gps_time +builtin_interfaces/Time gps_time # Event port associated with captured pulse uint8 event_port diff --git a/src/custom_msgs/msg/GSOF_INS.msg b/src/custom_msgs/msg/GsofIns.msg similarity index 95% rename from src/custom_msgs/msg/GSOF_INS.msg rename to src/custom_msgs/msg/GsofIns.msg index b47694d4..51c28a2b 100644 --- a/src/custom_msgs/msg/GSOF_INS.msg +++ b/src/custom_msgs/msg/GsofIns.msg @@ -8,10 +8,10 @@ std_msgs/Header header float64 time # system time of the system receiving the event -time sys_time +builtin_interfaces/Time sys_time # time associated with the gps event message -time gps_time +builtin_interfaces/Time gps_time # INS quality indicator. # {0: 'gps only', 1: 'coarse leveling', 2: 'degraded', 3: 'aligned', 4: 'full nav'} diff --git a/src/custom_msgs/msg/ImageSpaceDetection.msg b/src/custom_msgs/msg/ImageSpaceDetection.msg index 11f510b5..d89a2ca9 100644 --- a/src/custom_msgs/msg/ImageSpaceDetection.msg +++ b/src/custom_msgs/msg/ImageSpaceDetection.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header # Identifies the camera image stream that the detector operated on. # Typically, this is the ROS topic of the source imagery. diff --git a/src/custom_msgs/msg/ImageSpaceDetectionList.msg b/src/custom_msgs/msg/ImageSpaceDetectionList.msg index 9cee513d..bb468385 100644 --- a/src/custom_msgs/msg/ImageSpaceDetectionList.msg +++ b/src/custom_msgs/msg/ImageSpaceDetectionList.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header uint32 image_width uint32 image_height ImageSpaceDetection[] detections diff --git a/src/custom_msgs/msg/ImageWithMask.msg b/src/custom_msgs/msg/ImageWithMask.msg deleted file mode 100644 index 42c21493..00000000 --- a/src/custom_msgs/msg/ImageWithMask.msg +++ /dev/null @@ -1,2 +0,0 @@ -sensor_msgs/Image image -sensor_msgs/Image mask diff --git a/src/custom_msgs/msg/POSAVX.msg b/src/custom_msgs/msg/POSAVX.msg deleted file mode 100644 index db6bc325..00000000 --- a/src/custom_msgs/msg/POSAVX.msg +++ /dev/null @@ -1,75 +0,0 @@ -# State output from POS AVX Intertial navigation system. Message components -# taken from page 42 of the POS AVX 210 User Guide. - -# Header for this tuple of images -std_msgs/Header header - -# Time since 00:00:00 Thursday, 1 January 1970 -float64 time - -# INS quality indicator -# 0 - GPS Only -# 1 - Coarse leveling -# 2 - Degraded -int8 imu_alignment_status - -# GNSS Quality Indicator -# 0 - Fix not available -# 1 - GNSS SPS Mode -# 2 - Differential GPS,SPS -# 3 - GNSS PPS Mode -# 4 - Fixed RTK Mode -# 5 - Float RTK. -# 6 - DR Mode -int8 gnss_status - -# Latitude (-90,90] (degrees) -float64 latitude - -# Longitude (-180,180] (degrees) -float64 longitude - -# Altitude (meters) -float64 altitude - -# North velocity (meters/sec) -float32 north_velocity - -# East velocity (meters/sec) -float32 east_velocity - -# Down velocity (meters/sec) -float32 down_velocity - -# Total speed (meters/sec) -float32 total_speed - -# Roll (-180,180] (degrees) -float64 roll - -# Pitch (-180,180] (degrees) -float64 pitch - -# Heading [0,360) (degrees) -float64 heading - -# Track Angle [0,360) (degrees/sec) -float32 track_angle - -# Angular Rate X (degrees/sec) -float32 angular_rate_x - -# Angular Rate Y (degrees/sec) -float32 angular_rate_y - -# Angular Rate Z (degrees/sec) -float32 angular_rate_z - -# Acceleration X (meters/sec^2) -float32 acceleration_x - -# Acceleration Y (meters/sec^2) -float32 acceleration_y - -# Acceleration Z (meters/sec^2) -float32 acceleration_z \ No newline at end of file diff --git a/src/custom_msgs/msg/PASHR.msg b/src/custom_msgs/msg/Pashr.msg similarity index 100% rename from src/custom_msgs/msg/PASHR.msg rename to src/custom_msgs/msg/Pashr.msg diff --git a/src/custom_msgs/msg/SyncedPathImages.msg b/src/custom_msgs/msg/SyncedPathImages.msg index 67c3740c..f3715059 100644 --- a/src/custom_msgs/msg/SyncedPathImages.msg +++ b/src/custom_msgs/msg/SyncedPathImages.msg @@ -12,9 +12,9 @@ string file_path_ir string file_path_uv # Actual data -custom_msgs/PathImage image_rgb -custom_msgs/PathImage image_ir -custom_msgs/PathImage image_uv +PathImage image_rgb +PathImage image_ir +PathImage image_uv # metadata expansion field string meta_json diff --git a/src/custom_msgs/package.xml b/src/custom_msgs/package.xml index ea784217..2b171f45 100644 --- a/src/custom_msgs/package.xml +++ b/src/custom_msgs/package.xml @@ -1,59 +1,29 @@ - + + custom_msgs - 0.0.1 - The custom_msgs package + 1.0.0 + KAMERA message and service definitions - - - Adam Romlein Michael McDermott - - - - - Apache 2.0 + ament_cmake + ament_cmake_python + rosidl_default_generators - - - - - + builtin_interfaces + std_msgs + sensor_msgs + geometry_msgs - - - - + rosidl_default_runtime + rclpy + rosidl_interface_packages - - - - - - - - - - - - catkin - message_generation - sensor_msgs - std_msgs - geometry_msgs - sensor_msgs - std_msgs - geometry_msgs - message_runtime - - - - - + ament_cmake diff --git a/src/custom_msgs/setup.py b/src/custom_msgs/setup.py deleted file mode 100755 index 4c744996..00000000 --- a/src/custom_msgs/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['msgdispatch'], - package_dir={'': 'dispatch'}) - -setup(**d) diff --git a/src/custom_msgs/srv/CamControl.srv b/src/custom_msgs/srv/CamControl.srv deleted file mode 100644 index 3f99c132..00000000 --- a/src/custom_msgs/srv/CamControl.srv +++ /dev/null @@ -1,29 +0,0 @@ -## Issue commands to control lifetime of the camera - -# normal priority image request -uint8 CMD_HEALTH=0 -# reset the camera connection without restarting nodelet -uint8 CMD_SOFT_RESET=1 -# reset the nodelet -uint8 CMD_NODE_RESET=2 -# reset the container (this may not be able to be implemented without some sort of relay -uint8 CMD_DOCKER_RESET=3 -uint8 command - -# idk might be useful -string payload - ---- -# Response - -## looks OK -uint8 RES_OK=0 -## general purpose error -uint8 RES_ERR=1 -uint8 response - -# String-ified ok response -string out - -# String-ified error response -string err diff --git a/src/custom_msgs/srv/CamGetAttr.srv b/src/custom_msgs/srv/CamGetAttr.srv index 392c97bb..11dfe9c0 100644 --- a/src/custom_msgs/srv/CamGetAttr.srv +++ b/src/custom_msgs/srv/CamGetAttr.srv @@ -5,7 +5,7 @@ string name # Response # See PvApi.h. 0 if call succeed, enum val if err, e.g. can't find attribute -int32 tPvErr +int32 pv_err # String-ified format of value string value diff --git a/src/custom_msgs/srv/CamSetAttr.srv b/src/custom_msgs/srv/CamSetAttr.srv index 09b19b1b..b3231e9c 100644 --- a/src/custom_msgs/srv/CamSetAttr.srv +++ b/src/custom_msgs/srv/CamSetAttr.srv @@ -14,7 +14,7 @@ string dtype # Response # See PvApi.h. 0 if call succeed, enum val if err, e.g. can't find attribute -int32 tPvErr +int32 pv_err # String-ified format of value string value diff --git a/src/custom_msgs/srv/DiskUsage.srv b/src/custom_msgs/srv/DiskUsage.srv deleted file mode 100644 index 4cc504e5..00000000 --- a/src/custom_msgs/srv/DiskUsage.srv +++ /dev/null @@ -1,7 +0,0 @@ -# Filter by disk name (optional) -string disk_name - ---- -# Response from system call -string stdout -string stderr diff --git a/src/custom_msgs/srv/EraseDataDisk.srv b/src/custom_msgs/srv/EraseDataDisk.srv deleted file mode 100644 index 3149114c..00000000 --- a/src/custom_msgs/srv/EraseDataDisk.srv +++ /dev/null @@ -1,10 +0,0 @@ -# Indicate system to wipe disk on -string system_name - -# Do it. -bool push_the_button ---- -# Response - -# Indicates whether the action was successful. -bool success diff --git a/src/custom_msgs/srv/SetUInt.srv b/src/custom_msgs/srv/SetUInt.srv deleted file mode 100644 index 6f815ae9..00000000 --- a/src/custom_msgs/srv/SetUInt.srv +++ /dev/null @@ -1,12 +0,0 @@ -# Set some sort of scalar Unsigned Int -uint32 val - - -# if this flag is true, do not set, only return the value. for debugging only -bool only_get - ---- -# Response - -# Indicates whether the action was successful. -uint32 gotval diff --git a/src/custom_msgs/srv/StrList.srv b/src/custom_msgs/srv/StrList.srv index 0fa8f8aa..bcaff668 100644 --- a/src/custom_msgs/srv/StrList.srv +++ b/src/custom_msgs/srv/StrList.srv @@ -5,7 +5,7 @@ string name # Response # See PvApi.h. 0 if call succeed, enum val if err, e.g. can't find attribute -int32 tPvErr +int32 pv_err # String-ified format of value string[] values From 7e4128abd7020fff212df78597bde200a5d81122 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 18:56:33 -0400 Subject: [PATCH 02/20] Port roskv and kamcore to ROS2 roskv: - ament_cmake build; C++ lib was already ROS-free, now exported via modern CMake targets instead of catkin_LIBRARIES - Drop ROS1-only modules: reactor.py (relied on rosservice runtime introspection which has no ROS2 equivalent), rosparam_kv.py (global param server is gone in ROS2), and the rospy test nodes - roskv CLI and python lib are now fully ROS-independent kamcore: - Convert to ament_python; monitor scripts become console_script entry points (fps_monitor, cam_param_monitor, shapefile_monitor, seed_redis_config) - fps/cam_param/shapefile monitor nodes ported rospy -> rclpy; fps_monitor FPS math deduplicated; update loop is now a ROS timer - phase_one srv import is optional so non-PhaseOne systems (nayak, taiga prosilica RGB) don't need that package installed - Drop no-op kamcore_node.py keepalive node + kamcore.launch (existed to babysit roscore, which no longer exists) and unused diagnostics_to_influxdb.py - Launch files converted to ROS2 XML (.launch.xml) --- src/core/kamcore/CMakeLists.txt | 18 -- .../kamcore/launch/cam_param_monitor.launch | 5 - .../launch/cam_param_monitor.launch.xml | 4 + src/core/kamcore/launch/fps_monitor.launch | 5 - .../kamcore/launch/fps_monitor.launch.xml | 4 + src/core/kamcore/launch/kamcore.launch | 12 -- .../kamcore/launch/shapefile_monitor.launch | 9 - .../launch/shapefile_monitor.launch.xml | 6 + src/core/kamcore/package.xml | 27 +-- .../{scripts/__init__.py => resource/kamcore} | 0 .../scripts/diagnostics_to_influxdb.py | 75 ------- src/core/kamcore/scripts/kamcore_node.py | 15 -- src/core/kamcore/setup.cfg | 4 + src/core/kamcore/setup.py | 38 +++- .../kamcore}/cam_param_monitor_node.py | 196 ++++++++++-------- .../kamcore}/fps_monitor_node.py | 103 +++++---- .../kamcore}/seed_redis_config.py | 0 .../kamcore}/shapefile_monitor_node.py | 96 +++++---- src/core/roskv/CMakeLists.txt | 148 +++---------- src/core/roskv/noros-setup.py | 17 -- src/core/roskv/package.xml | 63 +----- src/core/roskv/setup.py | 20 -- src/core/roskv/src/roskv/client.py | 3 - src/core/roskv/src/roskv/impl/rosparam_kv.py | 20 -- src/core/roskv/src/roskv/nodes/__init__.py | 0 src/core/roskv/src/roskv/nodes/kvtest.py | 45 ---- .../roskv/src/roskv/nodes/test_destructor.py | 16 -- src/core/roskv/src/roskv/reactor.py | 146 ------------- 28 files changed, 301 insertions(+), 794 deletions(-) delete mode 100644 src/core/kamcore/CMakeLists.txt delete mode 100644 src/core/kamcore/launch/cam_param_monitor.launch create mode 100644 src/core/kamcore/launch/cam_param_monitor.launch.xml delete mode 100644 src/core/kamcore/launch/fps_monitor.launch create mode 100644 src/core/kamcore/launch/fps_monitor.launch.xml delete mode 100644 src/core/kamcore/launch/kamcore.launch delete mode 100644 src/core/kamcore/launch/shapefile_monitor.launch create mode 100644 src/core/kamcore/launch/shapefile_monitor.launch.xml rename src/core/kamcore/{scripts/__init__.py => resource/kamcore} (100%) delete mode 100755 src/core/kamcore/scripts/diagnostics_to_influxdb.py delete mode 100755 src/core/kamcore/scripts/kamcore_node.py create mode 100644 src/core/kamcore/setup.cfg rename src/core/kamcore/{scripts => src/kamcore}/cam_param_monitor_node.py (54%) rename src/core/kamcore/{scripts => src/kamcore}/fps_monitor_node.py (62%) rename src/core/kamcore/{scripts => src/kamcore}/seed_redis_config.py (100%) rename src/core/kamcore/{scripts => src/kamcore}/shapefile_monitor_node.py (70%) delete mode 100644 src/core/roskv/noros-setup.py delete mode 100755 src/core/roskv/setup.py delete mode 100644 src/core/roskv/src/roskv/impl/rosparam_kv.py delete mode 100755 src/core/roskv/src/roskv/nodes/__init__.py delete mode 100755 src/core/roskv/src/roskv/nodes/kvtest.py delete mode 100755 src/core/roskv/src/roskv/nodes/test_destructor.py delete mode 100644 src/core/roskv/src/roskv/reactor.py diff --git a/src/core/kamcore/CMakeLists.txt b/src/core/kamcore/CMakeLists.txt deleted file mode 100644 index b9f25eb0..00000000 --- a/src/core/kamcore/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kamcore) - -find_package(catkin REQUIRED) - -catkin_python_setup() -catkin_package() - -install(PROGRAMS - scripts/kamcore_node.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -#if (CATKIN_ENABLE_TESTING) -# find_package(roslint) -# roslint_python() -# roslint_add_test() -#endif() diff --git a/src/core/kamcore/launch/cam_param_monitor.launch b/src/core/kamcore/launch/cam_param_monitor.launch deleted file mode 100644 index 82b4ffc2..00000000 --- a/src/core/kamcore/launch/cam_param_monitor.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/core/kamcore/launch/cam_param_monitor.launch.xml b/src/core/kamcore/launch/cam_param_monitor.launch.xml new file mode 100644 index 00000000..8f61fafb --- /dev/null +++ b/src/core/kamcore/launch/cam_param_monitor.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/core/kamcore/launch/fps_monitor.launch b/src/core/kamcore/launch/fps_monitor.launch deleted file mode 100644 index fdfb1731..00000000 --- a/src/core/kamcore/launch/fps_monitor.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/core/kamcore/launch/fps_monitor.launch.xml b/src/core/kamcore/launch/fps_monitor.launch.xml new file mode 100644 index 00000000..e8b14bbf --- /dev/null +++ b/src/core/kamcore/launch/fps_monitor.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/core/kamcore/launch/kamcore.launch b/src/core/kamcore/launch/kamcore.launch deleted file mode 100644 index 9c06c154..00000000 --- a/src/core/kamcore/launch/kamcore.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/src/core/kamcore/launch/shapefile_monitor.launch b/src/core/kamcore/launch/shapefile_monitor.launch deleted file mode 100644 index e50ead96..00000000 --- a/src/core/kamcore/launch/shapefile_monitor.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/src/core/kamcore/launch/shapefile_monitor.launch.xml b/src/core/kamcore/launch/shapefile_monitor.launch.xml new file mode 100644 index 00000000..e6f2e807 --- /dev/null +++ b/src/core/kamcore/launch/shapefile_monitor.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/core/kamcore/package.xml b/src/core/kamcore/package.xml index c2d3daba..9caa9391 100644 --- a/src/core/kamcore/package.xml +++ b/src/core/kamcore/package.xml @@ -1,31 +1,26 @@ - + + kamcore - 0.1.0 + 1.0.0 - Common library for KAMERA nodes + Common library and monitor nodes for KAMERA Adam Romlein Michael McDermott - - - - - Apache 2.0 - - - catkin - - rospy + rclpy custom_msgs + sensor_msgs + roskv + python3-numpy + python3-redis + python3-yaml - - - + ament_python diff --git a/src/core/kamcore/scripts/__init__.py b/src/core/kamcore/resource/kamcore similarity index 100% rename from src/core/kamcore/scripts/__init__.py rename to src/core/kamcore/resource/kamcore diff --git a/src/core/kamcore/scripts/diagnostics_to_influxdb.py b/src/core/kamcore/scripts/diagnostics_to_influxdb.py deleted file mode 100755 index be1133a7..00000000 --- a/src/core/kamcore/scripts/diagnostics_to_influxdb.py +++ /dev/null @@ -1,75 +0,0 @@ -#! /usr/bin/python - -import os -import requests - -import rospy -from diagnostic_msgs.msg import DiagnosticArray - -from roskv.impl.redis_envoy import RedisEnvoy - - -class D2I(object): - """ A class to perform the operations of taking in diagnostics data from - ROS nodes and input into a give influxdb timeseries database. - """ - def __init__(self, host, org, bucket, token): - self.envoy = RedisEnvoy(os.environ["REDIS_HOST"], - client_name="diagnostic2influxdb") - self.influxdb_url = "http://" + host + ":8086/api/v2/write?org=" + org + "&bucket=" + bucket - self.influxdb_header = {"Authorization": "Token " + token} - sub = rospy.Subscriber("/diagnostics", DiagnosticArray, - callback=self.diagnostics_to_influxdb, - queue_size=100) - - def diagnostics_to_influxdb(self, array): - """ Takes and input DiagnosticArray msg and places into the database - of the class. - - :param array: The input ros msg. - :type array: DiagnosticArray.msg. - """ - rospy.loginfo_throttle(10, "Processing incoming diagnostics array.") - for msg in array.status: - tag_value = msg.hardware_id - measurement = msg.name.replace(" ", "_") - payload = "" - for i, pair in enumerate(msg.values): - tag_key = "origin" - key = pair.key.replace(" ", "_") - if key == "Info" or key == "Intrinsics": - continue - value = pair.value.replace(" ", "_") - if key == "Actual_frequency_(Hz)": - parts = measurement.split("/") - host = parts[0] - chan = parts[1] - topic = '/'.join(['', 'sys', "actual_geni_params", host, chan, "fps"]) - print("Setting %s to %s fps" % (topic, value)) - self.envoy.set(topic, value) - payload += measurement + "," + tag_key + "=" + tag_value + " " - payload += key + "=" + value + "\n" - payload=payload[:-1] - try: - rospy.loginfo_throttle(60, payload) - response = requests.post(self.influxdb_url, headers=self.influxdb_header, data=payload) - if response == 400: - rospy.logerr("Malformed payload:") - rospy.logerr(payload) - except requests.exceptions.RequestException as e: - rospy.logerr('Diagnostics to influxdb error: ') - rospy.logerr(e) - return - -def main(): - rospy.init_node("diagnostics_to_influxdb") - host = rospy.get_param("~host") - org = rospy.get_param("~org") - bucket = rospy.get_param("~bucket") - token = rospy.get_param("~token") - D2I(host, org, bucket, token) - rospy.loginfo("Waiting for incoming messages on /diagnostics ...") - rospy.spin() - -if __name__ == "__main__": - main() diff --git a/src/core/kamcore/scripts/kamcore_node.py b/src/core/kamcore/scripts/kamcore_node.py deleted file mode 100755 index 2385919f..00000000 --- a/src/core/kamcore/scripts/kamcore_node.py +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy - - -def main(): - print("[i] started KAMCORE") - rospy.init_node("kamcore") - rospy.loginfo("started KAMCORE") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/src/core/kamcore/setup.cfg b/src/core/kamcore/setup.cfg new file mode 100644 index 00000000..48c8eee8 --- /dev/null +++ b/src/core/kamcore/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/kamcore +[install] +install_scripts=$base/lib/kamcore diff --git a/src/core/kamcore/setup.py b/src/core/kamcore/setup.py index 420ec726..79df61e8 100644 --- a/src/core/kamcore/setup.py +++ b/src/core/kamcore/setup.py @@ -1,11 +1,31 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['kamcore'], - package_dir={'': 'src'}, - install_requires=['flask', 'redis', 'six'], - ) +from setuptools import setup -setup(**d) +package_name = "kamcore" + +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Common library and monitor nodes for KAMERA", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "fps_monitor = kamcore.fps_monitor_node:main", + "cam_param_monitor = kamcore.cam_param_monitor_node:main", + "shapefile_monitor = kamcore.shapefile_monitor_node:main", + "seed_redis_config = kamcore.seed_redis_config:main", + ], + }, +) diff --git a/src/core/kamcore/scripts/cam_param_monitor_node.py b/src/core/kamcore/src/kamcore/cam_param_monitor_node.py similarity index 54% rename from src/core/kamcore/scripts/cam_param_monitor_node.py rename to src/core/kamcore/src/kamcore/cam_param_monitor_node.py index ec4652c8..a20c11fb 100755 --- a/src/core/kamcore/scripts/cam_param_monitor_node.py +++ b/src/core/kamcore/src/kamcore/cam_param_monitor_node.py @@ -1,21 +1,23 @@ #! /usr/bin/python -import io import os -import requests -import shapefile -import shapely -import shapely.geometry import threading import time -import rospy +import rclpy +from rclpy.node import Node from roskv.impl.redis_envoy import RedisEnvoy -from phase_one.srv import GetPhaseOneParameter, SetPhaseOneParameter from custom_msgs.srv import CamGetAttr, CamSetAttr from roskv.util import filter_hosts_by_system +try: + from phase_one.srv import GetPhaseOneParameter, SetPhaseOneParameter +except ImportError: + # Phase One driver is only installed on systems flying a Phase One RGB + # camera; nayak/taiga use the prosilica driver instead. + GetPhaseOneParameter = SetPhaseOneParameter = None + p1setsrv = "set_phaseone_parameter" p1getsrv = "get_phaseone_parameter" setsrv = "set_camera_attr" @@ -37,20 +39,21 @@ def normalize_p1_shutter_mode(val): return s -class CamParamMonitor(object): +class CamParamMonitor(Node): """ A class to monitor state that is set in Redis, and the value reported by the cameras, and attempts to sync the 2. """ def __init__(self): + super().__init__("cam_param_monitor") redis_host = os.environ["REDIS_HOST"] print("Redis host: %s" % redis_host) - self.envoy = RedisEnvoy(os.environ["REDIS_HOST"], - client_name="cam_param_monitor") + self.envoy = RedisEnvoy(redis_host, client_name="cam_param_monitor") self.hosts = filter_hosts_by_system( self.envoy.get("/sys/arch/hosts").keys() ) self.modes = self.envoy.get("/sys/channels").keys() + self._clients = {} print("hosts: ") print(self.hosts) print("modes: ") @@ -61,47 +64,61 @@ def start_threads(self): t.daemon = True t.start() + def _get_client(self, topic, srv_type): + client = self._clients.get(topic) + if client is None: + client = self.create_client(srv_type, topic) + self._clients[topic] = client + return client + + def _call(self, topic, srv_type, wait_time, request): + """Synchronous service call; returns None on unavailability/timeout.""" + client = self._get_client(topic, srv_type) + if not client.wait_for_service(timeout_sec=wait_time): + return None + future = client.call_async(request) + deadline = time.time() + 10.0 + while not future.done(): + if time.time() > deadline: + client.remove_pending_request(future) + return None + time.sleep(0.01) + return future.result() + def get_param_val(self, host, mode, param, requested_val): - #rospy.logwarn(f"|GET| Starting call on {host} for {param}.") driver = "%s_driver" % mode wait_time = 0.1 - try: - if mode == "ir": - topic = '/'.join(['', host, mode, getsrv]) - rospy.wait_for_service(topic, timeout=wait_time) - srv = rospy.ServiceProxy(topic, CamGetAttr, - persistent=False) - elif mode == "uv": - topic = '/'.join(['', host, mode, driver, - getsrv]) - rospy.wait_for_service(topic, timeout=wait_time) - srv = rospy.ServiceProxy(topic, CamGetAttr, - persistent=False) - elif mode == "rgb": - topic = '/'.join(['', host, mode, driver, - p1getsrv]) - rospy.wait_for_service(topic, timeout=wait_time) - srv = rospy.ServiceProxy(topic, GetPhaseOneParameter, - persistent=False) - resp = srv(name=param) - except (rospy.exceptions.ROSException, rospy.service.ServiceException) as e: - #rospy.logerr("|GET| Service exception!") - #rospy.logerr(e) - #print(topic) - #print(param) - resp = None + if mode == "ir": + topic = '/'.join(['', host, mode, getsrv]) + resp = self._call(topic, CamGetAttr, wait_time, + CamGetAttr.Request(name=param)) + elif mode == "uv": + topic = '/'.join(['', host, mode, driver, getsrv]) + resp = self._call(topic, CamGetAttr, wait_time, + CamGetAttr.Request(name=param)) + elif mode == "rgb": + if GetPhaseOneParameter is None: + topic = '/'.join(['', host, mode, driver, getsrv]) + resp = self._call(topic, CamGetAttr, wait_time, + CamGetAttr.Request(name=param)) + else: + topic = '/'.join(['', host, mode, driver, p1getsrv]) + resp = self._call(topic, GetPhaseOneParameter, wait_time, + GetPhaseOneParameter.Request(name=param)) + else: + return + if resp is None: return getsrv_val = None dtype = None try: getsrv_val = resp.value - #rospy.loginfo("|GET| getsrv_val: ", getsrv_val) if getsrv_val == "error": - rospy.loginfo(resp) - rospy.loginfo(topic) - rospy.logerr("|GET| Failed to get parameter %s!" % param) + self.get_logger().info(str(resp)) + self.get_logger().info(topic) + self.get_logger().error("|GET| Failed to get parameter %s!" % param) return - if mode == "rgb": + if mode == "rgb" and GetPhaseOneParameter is not None: # Phase one params have the type in the return string getsrv_val = ''.join(getsrv_val.split(' ')[1:]) # A random s is sometimes in shutter speed @@ -113,15 +130,15 @@ def get_param_val(self, host, mode, param, requested_val): elif isinstance(requested_val, float): try: getsrv_val = float(getsrv_val) - except: - num,den = map(int, getsrv_val.split( '/' )) + except Exception: + num, den = map(int, getsrv_val.split('/')) getsrv_val = float(num / den) elif isinstance(requested_val, int): try: getsrv_val = float(getsrv_val) getsrv_val = int(getsrv_val) - except: - num,den = map(int, getsrv_val.split( '/' )) + except Exception: + num, den = map(int, getsrv_val.split('/')) getsrv_val = float(num / den) else: dtype = resp.dtype @@ -130,15 +147,16 @@ def get_param_val(self, host, mode, param, requested_val): elif isinstance(requested_val, int): getsrv_val = int(getsrv_val.rstrip('\x00')) except Exception as e: - rospy.logwarn(f"|GET| value coercion failed on {param}, resp value: %s" % resp.value) - rospy.logerr(e) + self.get_logger().warning( + f"|GET| value coercion failed on {param}, resp value: %s" % resp.value) + self.get_logger().error(str(e)) return if getsrv_val is None: return if mode == "rgb": param = '_'.join(param.split(' ')) self.envoy.set("/sys/actual_geni_params/%s/%s/%s" - % (host, mode, param), getsrv_val) + % (host, mode, param), getsrv_val) if param == "GainValue" or param == "ExposureValue"\ or param == "ISO" or param == "Shutter_Speed"\ or param == "Sensor_Temperature": @@ -151,57 +169,50 @@ def get_param_val(self, host, mode, param, requested_val): if getsrv_val != requested_val: print("Param: %s, getsrv_val: %s, requested_val: %s" % (param, getsrv_val, requested_val)) - rospy.logwarn("Setting parameter %s on %s because it differs." - % (param, host + "/" + mode)) + self.get_logger().warning("Setting parameter %s on %s because it differs." + % (param, host + "/" + mode)) # Return real value to set return str(param), str(requested_val), dtype - #rospy.loginfo("|GET| Finished.") return None def set_params(self, host, mode, params_to_set, requested_params): # Set all params that differ from those in redis db driver = f"{mode}_driver" + use_p1 = mode == "rgb" and SetPhaseOneParameter is not None if mode == "ir": topic = '/'.join(['', host, mode, setsrv]) - srv = rospy.ServiceProxy(topic, CamSetAttr, - persistent=False) - elif mode == "uv": - topic = '/'.join(['', host, mode, driver, setsrv]) - srv = rospy.ServiceProxy(topic, CamSetAttr, - persistent=False) - elif mode == "rgb": + elif use_p1: topic = '/'.join(['', host, mode, driver, p1setsrv]) - srv = rospy.ServiceProxy(topic, SetPhaseOneParameter, - persistent=False) - if mode == "rgb": - req = ','.join([ f"{name}={v}" for name, (v, d) in - params_to_set.items() ]) - resp = None + elif mode in ("uv", "rgb"): + topic = '/'.join(['', host, mode, driver, setsrv]) + else: + return + if use_p1: + req_str = ','.join([f"{name}={v}" for name, (v, d) in + params_to_set.items()]) if len(params_to_set): - try: - rospy.loginfo("|SET| Setting the following params on P1:") - print(req) - resp = srv(parameters=req) - except Exception as e: - rospy.logwarn("|SET| Failed to set params for system %s camera %s." % - (host, mode)) - rospy.logerr(e) + self.get_logger().info("|SET| Setting the following params on P1:") + print(req_str) + resp = self._call(topic, SetPhaseOneParameter, 1.0, + SetPhaseOneParameter.Request(parameters=req_str)) + if resp is None: + self.get_logger().warning( + "|SET| Failed to set params for system %s camera %s." % + (host, mode)) else: for name, (v, d) in params_to_set.items(): - try: - rospy.loginfo("|SET| Parameters: {} {} {}".format(name, v, d)) - resp = srv(name=name, value=v, dtype=d) - except Exception as e: - rospy.logwarn("|SET| Failed to set params for system %s camera %s." % - (host, mode)) - rospy.logerr(e) - #rospy.loginfo("|SET| Finished.") + self.get_logger().info("|SET| Parameters: {} {} {}".format(name, v, d)) + resp = self._call(topic, CamSetAttr, 1.0, + CamSetAttr.Request(name=name, value=v, dtype=d or "")) + if resp is None: + self.get_logger().warning( + "|SET| Failed to set params for system %s camera %s." % + (host, mode)) return def check_cam_params(self): # Check all params once every 3 s - ros_rate = rospy.Rate(0.333) - while not rospy.is_shutdown(): + while rclpy.ok(): tic = time.time() for host in self.hosts: for mode in self.modes: @@ -222,15 +233,22 @@ def check_cam_params(self): params_to_set[ret[0]] = (ret[1], ret[2]) self.set_params(host, mode, params_to_set, requested_params) - rospy.loginfo("Time to set parameters was %0.4fs." % (time.time() - tic)) - ros_rate.sleep() + self.get_logger().info("Time to set parameters was %0.4fs." % (time.time() - tic)) + time.sleep(3.0) -def main(): - rospy.init_node("cam_param_monitor") - CPM = CamParamMonitor() - CPM.start_threads() - rospy.spin() +def main(args=None): + rclpy.init(args=args) + cpm = CamParamMonitor() + cpm.start_threads() + try: + rclpy.spin(cpm) + except KeyboardInterrupt: + pass + finally: + cpm.destroy_node() + rclpy.shutdown() + if __name__ == "__main__": main() diff --git a/src/core/kamcore/scripts/fps_monitor_node.py b/src/core/kamcore/src/kamcore/fps_monitor_node.py similarity index 62% rename from src/core/kamcore/scripts/fps_monitor_node.py rename to src/core/kamcore/src/kamcore/fps_monitor_node.py index 1ab296fb..1bf374b7 100755 --- a/src/core/kamcore/scripts/fps_monitor_node.py +++ b/src/core/kamcore/src/kamcore/fps_monitor_node.py @@ -2,20 +2,26 @@ import os import socket -import numpy as np -from six.moves.queue import deque +from collections import deque -import rospy +import numpy as np +import rclpy +from rclpy.node import Node from roskv.impl.redis_envoy import RedisEnvoy from sensor_msgs.msg import Image -from custom_msgs.msg import GSOF_EVT +from custom_msgs.msg import GsofEvt hostname = socket.gethostname() -class FPSMonitor: +def stamp_to_sec(stamp): + return stamp.sec + stamp.nanosec * 1e-9 + + +class FPSMonitor(Node): def __init__(self) -> None: + super().__init__(f"{hostname}_fps_monitor") self.envoy = RedisEnvoy(os.environ["REDIS_HOST"], client_name="fps_monitor") self.hostname = hostname self.ir_drops = 0 @@ -28,23 +34,27 @@ def __init__(self) -> None: self.processed_times = deque(maxlen=50) self.previously_archiving = False self.init_ros() + self.update_timer = self.create_timer(1.0, self.update) def init_ros(self): - self.event_sub = rospy.Subscriber( - "/event", GSOF_EVT, callback=self.ingest_event, queue_size=2 + self.event_sub = self.create_subscription( + GsofEvt, "/event", self.ingest_event, 2 ) + self.image_subs = [] channels = self.envoy.get("/sys/channels").keys() for channel in channels: - _ = rospy.Subscriber( - f"/{hostname}/{channel}/image_raw", - Image, - callback=self.ingest_image, - queue_size=2, + self.image_subs.append( + self.create_subscription( + Image, + f"/{hostname}/{channel}/image_raw", + self.ingest_image, + 2, + ) ) def ingest_event(self, msg): - time = msg.gps_time.to_sec() - rospy.loginfo("Received event message, time %0.5f." % time) + time = stamp_to_sec(msg.gps_time) + self.get_logger().info("Received event message, time %0.5f." % time) is_archiving = self.envoy.get("/sys/arch/is_archiving") == "1" # Skip the first event, so we don't accidentally report drops if is_archiving and self.previously_archiving: @@ -58,7 +68,7 @@ def ingest_event(self, msg): def ingest_image(self, msg): frame_id = msg.header.frame_id - time = msg.header.stamp.to_sec() + time = stamp_to_sec(msg.header.stamp) modality = "" if "uv" in frame_id: @@ -71,9 +81,16 @@ def ingest_image(self, msg): modality = "ir" self.ir_queue.append(time) else: - rospy.logwarn("No valid modality found in image message!") + self.get_logger().warning("No valid modality found in image message!") + + self.get_logger().info("Received %s message, time: %0.5f." % (modality, time)) - rospy.loginfo("Received %s message, time: %0.5f." % (modality, time)) + @staticmethod + def _fps(times): + if len(times) < 2: + return 0 + den = np.mean([times[i] - times[i - 1] for i in range(1, len(times))]) + return round(1 / den, 3) if den != 0 else 0 def update(self): # copy over data structures and sort @@ -82,38 +99,9 @@ def update(self): uv_list = list(self.uv_queue) times = list(self.evt_queue) - # calculate FPS - if len(rgb_list) > 1: - den = np.mean( - [rgb_list[i] - rgb_list[i - 1] for i in range(1, len(rgb_list))] - ) - if den != 0: - rgb_fps = 1 / den - else: - rgb_fps = 0 - else: - rgb_fps = 0 - if len(ir_list) > 1: - den = np.mean([ir_list[i] - ir_list[i - 1] for i in range(1, len(ir_list))]) - if den != 0: - ir_fps = 1 / den - else: - ir_fps = 0 - else: - ir_fps = 0 - if len(uv_list) > 1: - den = np.mean([uv_list[i] - uv_list[i - 1] for i in range(1, len(uv_list))]) - if den != 0: - uv_fps = 1 / den - else: - uv_fps = 0 - else: - uv_fps = 0 - - # don't need so many sigfigs - rgb_fps = round(rgb_fps, 3) - ir_fps = round(ir_fps, 3) - uv_fps = round(uv_fps, 3) + rgb_fps = self._fps(rgb_list) + ir_fps = self._fps(ir_list) + uv_fps = self._fps(uv_list) # register missed frames # Assume that if we haven't seen this time in the last 5 frames, @@ -142,14 +130,17 @@ def update(self): self.envoy.set(f"/sys/arch/{hostname}/uv/dropped", self.uv_drops) -def main(): - rospy.init_node(f"{hostname}_fps_monitor") +def main(args=None): + rclpy.init(args=args) mon = FPSMonitor() - rospy.loginfo("Waiting for incoming image and event messages ...") - - while not rospy.is_shutdown(): - mon.update() - rospy.sleep(1) + mon.get_logger().info("Waiting for incoming image and event messages ...") + try: + rclpy.spin(mon) + except KeyboardInterrupt: + pass + finally: + mon.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/core/kamcore/scripts/seed_redis_config.py b/src/core/kamcore/src/kamcore/seed_redis_config.py similarity index 100% rename from src/core/kamcore/scripts/seed_redis_config.py rename to src/core/kamcore/src/kamcore/seed_redis_config.py diff --git a/src/core/kamcore/scripts/shapefile_monitor_node.py b/src/core/kamcore/src/kamcore/shapefile_monitor_node.py similarity index 70% rename from src/core/kamcore/scripts/shapefile_monitor_node.py rename to src/core/kamcore/src/kamcore/shapefile_monitor_node.py index 7f4f6c33..76075981 100755 --- a/src/core/kamcore/scripts/shapefile_monitor_node.py +++ b/src/core/kamcore/src/kamcore/shapefile_monitor_node.py @@ -2,18 +2,19 @@ import io import os +import time + +import numpy as np import pygeodesy import redis -import requests import shapefile import shapely import shapely.geometry -import time -import numpy as np -import rospy +import rclpy +from rclpy.node import Node -from custom_msgs.msg import GSOF_INS +from custom_msgs.msg import GsofIns from roskv.impl.redis_envoy import RedisEnvoy from roskv.util import filter_hosts_by_system @@ -22,15 +23,17 @@ geod_filename = os.path.join(KAM_DIR, 'assets/geods/egm84-15.pgm') geod = pygeodesy.geoids.GeoidPGM(geod_filename) -class ShapefileMonitor(object): + +class ShapefileMonitor(Node): """ A class to monitor the shapefile provided by a redis db, monitoring if any changes occur and setting archiving if the INS reports within the shapefile, turning off archiving if it reports outside the shapefile. """ def __init__(self): + super().__init__("shapefile_monitor") self.redis = redis.Redis(os.environ["REDIS_HOST"], - client_name="shapefile_monitor") + client_name="shapefile_monitor") envoy = RedisEnvoy(os.environ["REDIS_HOST"], client_name="shapefile_monitor") self.hosts = filter_hosts_by_system( @@ -63,26 +66,26 @@ def load_shapefile(self): self.archive_region = archive_region if fn is not None: self.redis.set("/stat/shapefile_name", fn) - rospy.loginfo("Successfully loaded shapefile from Redis db!") + self.get_logger().info("Successfully loaded shapefile from Redis db!") else: - rospy.logwarn("Failed to load shapefile from Redis db, waiting for " - "entry.") - rospy.loginfo("Time for shapefile check was %0.4fs." % - (time.time() - tic)) + self.get_logger().warning( + "Failed to load shapefile from Redis db, waiting for entry.") + self.get_logger().info("Time for shapefile check was %0.4fs." % + (time.time() - tic)) def listener(self): - sub = rospy.Subscriber("/ins", GSOF_INS, - callback=self.ins_callback, - queue_size=100) + self.sub = self.create_subscription(GsofIns, "/ins", + self.ins_callback, 100) def ins_callback(self, msg): - """ Takes and input GSOF_INS msg and turns archiving on/off + """ Takes an input GsofIns msg and turns archiving on/off depending on if it's in the shapefile or not. :param msg: The input ros msg. - :type msg: custom_msgs.msg/GSOF_EVENT. + :type msg: custom_msgs.msg/GsofIns. """ - rospy.loginfo_throttle(10, "Processing incoming INS msgs.") + self.get_logger().info("Processing incoming INS msgs.", + throttle_duration_sec=10) # Only check points every 1 s if self.cnt < 100: @@ -117,14 +120,14 @@ def ins_callback(self, msg): min_fps = float(self.redis.get("/sys/arch/min_frame_rate")) max_fps = float(self.redis.get("/sys/arch/max_frame_rate")) if rate < min_fps or rate > max_fps: - rospy.logwarn("Overlap percent of %s at alt %s wants " - "to set framerate to %s, but min and max " - "are %s and %s." % (overlap, alt, rate, - min_fps, max_fps)) + self.get_logger().warning( + "Overlap percent of %s at alt %s wants " + "to set framerate to %s, but min and max " + "are %s and %s." % (overlap, alt, rate, min_fps, max_fps)) fps = np.clip(rate, min_fps, max_fps) self.redis.set("/sys/arch/trigger_freq", fps) - rospy.loginfo("Set triggering rate to map overlap %s to %s." % - (overlap, fps)) + self.get_logger().info("Set triggering rate to map overlap %s to %s." % + (overlap, fps)) use_archive_region = int(self.redis.get("/sys/arch/use_archive_region")) load_sf = int(self.redis.get("/sys/arch/load_shapefile")) @@ -145,35 +148,38 @@ def ins_callback(self, msg): print("is_archiving = 0") self.redis.set("/sys/arch/is_archiving", 0) # make sure nucmode is set to automatic by default - for host in self.hosts: - topic = "/".join(["", 'sys', 'requested_geni_params', - host, "ir", "CorrectionAutoEnabled"]) - self.redis.set(topic, "1") + self.set_nuc_mode("1") else: # make sure nucmode is set to automatic by default - for host in self.hosts: - topic = "/".join(["", 'sys', 'requested_geni_params', - host, "ir", "CorrectionAutoEnabled"]) - self.redis.set(topic, "1") + self.set_nuc_mode("1") is_archiving = int(self.redis.get("/sys/arch/is_archiving")) == 1 allow_ir_nuc = int(self.redis.get("/sys/arch/allow_ir_nuc")) == 1 if not allow_ir_nuc and is_archiving: print("Turning off NUCing when archiving.") # Make sure NUCing is turned off - for host in self.hosts: - topic = "/".join(["", 'sys', 'requested_geni_params', - host, "ir", "CorrectionAutoEnabled"]) - self.redis.set(topic, "0") - - + self.set_nuc_mode("0") + + def set_nuc_mode(self, val): + for host in self.hosts: + topic = "/".join(["", 'sys', 'requested_geni_params', + host, "ir", "CorrectionAutoEnabled"]) + self.redis.set(topic, val) + + +def main(args=None): + rclpy.init(args=args) + sm = ShapefileMonitor() + sm.load_shapefile() + sm.listener() + sm.get_logger().info("Waiting for incoming messages on /ins ...") + try: + rclpy.spin(sm) + except KeyboardInterrupt: + pass + finally: + sm.destroy_node() + rclpy.shutdown() -def main(): - rospy.init_node("shapefile_monitor") - SM = ShapefileMonitor() - SM.load_shapefile() - SM.listener() - rospy.loginfo("Waiting for incoming messages on /ins ...") - rospy.spin() if __name__ == "__main__": main() diff --git a/src/core/roskv/CMakeLists.txt b/src/core/roskv/CMakeLists.txt index 18d3bc87..83bbc5eb 100644 --- a/src/core/roskv/CMakeLists.txt +++ b/src/core/roskv/CMakeLists.txt @@ -1,138 +1,46 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(roskv) -include(CMakePrintHelpers) set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) - -## 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 - roscpp -) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(nlohmann_json REQUIRED) find_path(HIREDIS_HEADER hiredis) find_path(REDIS_PLUS_PLUS_HEADER sw) find_library(HIREDIS_LIB hiredis) find_library(REDIS_PLUS_PLUS_LIB redis++) -## NOTE: this should be *sw* NOT *redis++* - - -## 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() - - -################################### -## 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 ${PROJECT_NAME} - CATKIN_DEPENDS roscpp rospy - DEPENDS +add_library(roskv + libroskv/envoy.cpp + libroskv/archiver.cpp ) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +target_include_directories(roskv PUBLIC + $ + $ + ${REDIS_PLUS_PLUS_HEADER} ) - -include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS}) -add_library(roskv - include/roskv/envoy.h - include/roskv/archiver.h - libroskv/envoy.cpp - libroskv/archiver.cpp - ) -add_executable(test_roskv libroskv/test_roskv.cpp) target_link_libraries(roskv - ${catkin_LIBRARIES} - ${nlohmann_json_LIBRARIES} - ${HIREDIS_LIB} - ${REDIS_PLUS_PLUS_LIB}) - -target_link_libraries(test_roskv - ${catkin_LIBRARIES} - ${nlohmann_json_LIBRARIES} - ${HIREDIS_LIB} - ${REDIS_PLUS_PLUS_LIB}) -target_include_directories(test_roskv PUBLIC ${REDIS_PLUS_PLUS_HEADER}) - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + nlohmann_json::nlohmann_json + ${HIREDIS_LIB} + ${REDIS_PLUS_PLUS_LIB} +) -## 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} -# ) +add_executable(test_roskv libroskv/test_roskv.cpp) +target_link_libraries(test_roskv roskv) -## Mark executables and/or libraries for installation install(TARGETS roskv - 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 -# ) -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +install(DIRECTORY include/ DESTINATION include) -## 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_nexus.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +# Pure-python KV library + CLI +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/roskv) +install(PROGRAMS scripts/roskv DESTINATION lib/${PROJECT_NAME}) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) -cmake_print_variables(PROJECT_NAME) -cmake_print_variables(CATKIN_PACKAGE_INCLUDE_DESTINATION catkin_LIBRARIES) -cmake_print_variables(nlohmann_json_LIBRARIES REDIS_PLUS_PLUS_HEADER REDIS_PLUS_PLUS_LIB) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(nlohmann_json) +ament_package() diff --git a/src/core/roskv/noros-setup.py b/src/core/roskv/noros-setup.py deleted file mode 100644 index 3db6485b..00000000 --- a/src/core/roskv/noros-setup.py +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -from setuptools import find_packages, setup - -packages = find_packages() -deps = ["typing", "enum34", "pytz", "python-dateutil", "boltons", "redis"] - -setup( - name="roskv", - version="0.1.0", - script_name="noros-setup.py", - python_requires=">2.7", - zip_safe=False, - packages=packages, - install_requires=deps, - include_package_data=True, - entry_points={"console_scripts": ["roskv=roskv.client:main"]}, -) diff --git a/src/core/roskv/package.xml b/src/core/roskv/package.xml index 7441f857..df2eb796 100644 --- a/src/core/roskv/package.xml +++ b/src/core/roskv/package.xml @@ -1,66 +1,23 @@ - + + roskv - 0.1.0 - An abstract key-value store for multiple KV backends + 1.0.0 + An abstract key-value store for multiple KV backends (Redis-backed) - - - Adam Romlein Michael McDermott - - - - - Apache 2.0 + ament_cmake + ament_cmake_python - - - - - - - - - - + nlohmann-json-dev + python3-redis + python3-yaml - - - - - - - - - - - - - - - - - - - - - catkin - rospy - roscpp - rospy - roscpp - rospy - roscpp - - - - - + ament_cmake diff --git a/src/core/roskv/setup.py b/src/core/roskv/setup.py deleted file mode 100755 index 25e3d9eb..00000000 --- a/src/core/roskv/setup.py +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env python -from setuptools import find_packages, setup -from catkin_pkg.python_setup import generate_distutils_setup - -deps = [ - "typing", -] - -# this function uses information from package.xml to populate dict -packages = find_packages(where="src") -d = generate_distutils_setup( - packages=packages, - package_dir={"": "src"}, - install_requires=deps, - # entry_points={"console_scripts": ["roskv=roskv.client:main"]}, - scripts=["scripts/roskv"], - extras_require={"redis": ["redis"]}, -) - -setup(**d) diff --git a/src/core/roskv/src/roskv/client.py b/src/core/roskv/src/roskv/client.py index 97f979ab..6fc32622 100644 --- a/src/core/roskv/src/roskv/client.py +++ b/src/core/roskv/src/roskv/client.py @@ -2,9 +2,6 @@ # -*- coding: utf-8 -*- from __future__ import division, print_function -import sys - -import rospy from roskv.impl.redis_envoy import RedisEnvoy diff --git a/src/core/roskv/src/roskv/impl/rosparam_kv.py b/src/core/roskv/src/roskv/impl/rosparam_kv.py deleted file mode 100644 index b9c4e91b..00000000 --- a/src/core/roskv/src/roskv/impl/rosparam_kv.py +++ /dev/null @@ -1,20 +0,0 @@ -import rospy -from roskv.base import KV, NullDefault - -_default = NullDefault() - - -class RosParamKV(KV): - def __init__(self): - pass - - def get(self, key, default=_default, **kwargs): - if _default is _default: - return rospy.get_param(key) - return rospy.get_param(key, default) - - def put(self, key, val, **kwargs): - return rospy.set_param(key, val) - - def delete(self, key, **kwargs): - return rospy.delete_param(key) diff --git a/src/core/roskv/src/roskv/nodes/__init__.py b/src/core/roskv/src/roskv/nodes/__init__.py deleted file mode 100755 index e69de29b..00000000 diff --git a/src/core/roskv/src/roskv/nodes/kvtest.py b/src/core/roskv/src/roskv/nodes/kvtest.py deleted file mode 100755 index e00943ab..00000000 --- a/src/core/roskv/src/roskv/nodes/kvtest.py +++ /dev/null @@ -1,45 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import warnings -import rospy -from roskv.impl.rosparam_kv import RosParamKV - - -def ros_test(): - node = "kvtest" - rospy.init_node(node) - print("ROS Test. Parent: ", rospy.get_namespace()) - kv = RosParamKV() - node_name = rospy.get_name() - key, val = "foo", "bar_from_{}".format(node) - x = kv.put(key, val) - res = kv.get(key) - print("{}: {}".format(key, res)) - - key = "foostruct" - val = {"name": "bar_from_{}".format(node), "nest": {"spam": "eggs", "num": 42}} - x = kv.put(key, val) - res = kv.get(key) - print("{}: {}".format(key, res)) - res = kv.get("foostruct/nest/num") - print("{}: {} ({})".format(key, res, type(res))) - - key = "test_list" - kv.put(key, [1, 2, 3]) - res = kv.get(key) - print("{}: {} ({})".format(key, res, type(res))) - - -def main(): - try: - ros_test() - except Exception as exc: - warnings.warn("ros_test: {}: {}".format(exc.__class__.__name__, exc)) - - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/core/roskv/src/roskv/nodes/test_destructor.py b/src/core/roskv/src/roskv/nodes/test_destructor.py deleted file mode 100755 index b53bbdf2..00000000 --- a/src/core/roskv/src/roskv/nodes/test_destructor.py +++ /dev/null @@ -1,16 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import sys -from roskv.impl.redis_envoy import RedisEnvoy as Envoy - - -envoy = Envoy(host=sys.argv[1]) - - -def main(): - print(envoy.name) - - -if __name__ == "__main__": - main() diff --git a/src/core/roskv/src/roskv/reactor.py b/src/core/roskv/src/roskv/reactor.py deleted file mode 100644 index 5721c187..00000000 --- a/src/core/roskv/src/roskv/reactor.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -from __future__ import division, print_function -import os -import sys -import json -import threading -from queue import Queue, Empty - -import rospy -import rosservice - -from roskv.impl.redis_envoy import RedisEnvoy, StateService -from kamcore.datatypes import TryIntoAttrMxn, ManditoryInitializer, ToDictMxn - -# from roskv.util import redis_decode -from custom_msgs.srv import CamGetAttr, CamGetAttrRequest, CamSetAttr, CamSetAttrRequest -from custom_msgs.srv import SetTriggerRate, SetTriggerRateRequest - -ROS_INSTANT = rospy.Duration(0, 1) - -## Dataclass objects to parse messages -class DC_CamGetAttrReq(TryIntoAttrMxn, ManditoryInitializer, ToDictMxn): - __slots__ = ["name"] - __defaults__ = {"name": ""} - - -class DC_CamSetAttrReq(TryIntoAttrMxn, ManditoryInitializer, ToDictMxn): - __slots__ = ["name", "value", "dtype"] - __defaults__ = {"name": "", "value": "", "dtype": ""} - - -class DC_SetTriggerRateReq(TryIntoAttrMxn, ManditoryInitializer, ToDictMxn): - __slots__ = ["rate"] - __defaults__ = {"rate": 1.0} - - -topic_to_service = { - "get_camera_attr": {"srv": CamGetAttr, "msg": CamGetAttrRequest, "dc": DC_CamGetAttrReq}, - "set_camera_attr": {"srv": CamSetAttr, "msg": CamSetAttrRequest, "dc": DC_CamSetAttrReq}, - "set_trigger_rate": {"srv": SetTriggerRate, "msg": SetTriggerRateRequest, "dc": DC_SetTriggerRateReq}, -} - - -class Executor(threading.Thread): - def __init__(self, cmd_queue): - threading.Thread.__init__(self) - self.cmd_queue = cmd_queue # type: Queue - - def run(self): - print("starting thread") - while not rospy.core.is_shutdown(): - try: - # print('waiting') - cmdpair = self.cmd_queue.get(block=True, timeout=1.0) - self.issue_cmd(cmdpair) - except Empty: - pass - - def issue_cmd(self, cmdpair): - key, payload = cmdpair - rospy.loginfo("Issuing: {}: {}".format(key, payload)) - topic = key.replace('/cmd', '') - topic_kind = os.path.basename(topic) - dispatch = topic_to_service.get(topic_kind, None) - if dispatch is None: - rospy.logwarn("Is not an allowed service kind: {}".format(key)) - return - try: - DC = dispatch['dc'] - # payload = json.loads(payload) - dc = DC(**payload) - req = dispatch['msg']() - ## cast it into a message to make sure it works - dc.into(req) - print(req) - except RuntimeError as e: - rospy.logerr("{}: {}".format(e.__class__.__name__, e)) - return - - try: - cls = rosservice.get_service_class_by_name(topic) - except rosservice.ROSServiceException as e: - rospy.logerr("{}: {}".format(e.__class__.__name__, e)) - return - proxy = rospy.ServiceProxy(topic, cls, persistent=False) - print(req) - print(dc.to_dict()) - - res = proxy(**dc.to_dict()) - print(res) - - - - -class KeyWatcher(object): - def __init__(self, cmd_queue, host="nuvo0"): - node_host = rospy.get_namespace().strip("/") - self.envoy = RedisEnvoy(host, client_name=node_host + "-reactor") - self.cmd_recv = dict() - self.cmd_queue = cmd_queue - self.queue_timer = rospy.Timer(ROS_INSTANT, self.cb_sync, oneshot=True) - - def cb_sync(self, event=None): - try: - resp = self.envoy.get_dict("/cmd", flatten=True) - except KeyError as e: - resp = {} - cmd_recv = dict(resp) - if cmd_recv: - rospy.loginfo("Got command(s): {}".format(cmd_recv)) - self.cmd_recv.update(cmd_recv) - try: - self.envoy.delete_dict("/cmd") - except KeyError: - pass - for cmdpair in cmd_recv.items(): - self.cmd_queue.put(cmdpair) - self.queue_timer = rospy.Timer(ROS_INSTANT, self.cb_sync, oneshot=True) - else: - print('.', end='') - - sys.stdout.flush() - - -def main(): - # Launch the node. - node = "redis_reactor" - rospy.init_node(node, anonymous=False) - node_name = rospy.get_name() - cmd_queue = Queue() - executor = Executor(cmd_queue) - watcher = KeyWatcher(cmd_queue) - sync_timer = rospy.Timer(rospy.Duration(0.5), watcher.cb_sync) - - executor.start() - rospy.spin() - - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - print("Interrupt") - pass From f72f9e30dc80e58ce8ac372b565531402c5be9c1 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:05:12 -0400 Subject: [PATCH 03/20] Port nexus and view_server (imageview) to ROS2 nexus: - ArchiverBase/ArchiveManager now take an rclpy node; subscriptions, publishers, and services hang off it. Service callbacks use the ROS2 request/response signature - ROS1 global params replaced: /cfg/hosts fov lookup dropped (was unused), /system_name -> SYSTEM_NAME env, /cfg/file_formats -> /sys/arch/ext_* in Redis (matching what img_nexus already used) - msg_as_dict rewritten from genpy introspection to rosidl get_fields_and_field_types; header.seq removed (gone in ROS2) - /rawmsg error publisher created once instead of per-publish - Package becomes a pure ament_python library: dropped duplicate nexus_node/img_nexus/evt_listener/simulate_heading nodes (nayak and taiga run the copy embedded in view_server; nothing launches these) view_server: - image_view_server ported to rclpy with a MultiThreadedExecutor and reentrant callback group so blocking image requests don't starve image callbacks - Nexus sync epochs keyed on (sec, nanosec) tuples since ROS2 message stamps are unhashable; event logging keyed on event_num since header.seq no longer exists - Dropped dead sync_queue_callback2 path and unused web_server node - roskv: hash_genpy_msg -> hash_ros_msg using rclpy serialize_message --- src/core/roskv/src/roskv/util.py | 11 +- src/process/nexus/CMakeLists.txt | 200 ----- src/process/nexus/launch/evt_listener.launch | 19 - src/process/nexus/launch/nexus.launch | 40 - .../nexus/launch/simulate_heading.launch | 28 - src/process/nexus/nexus_node.py | 36 - src/process/nexus/nodes/evt_listener.py | 51 -- src/process/nexus/nodes/img_nexus.py | 699 ------------------ src/process/nexus/package.xml | 75 +- .../{nodes/__init__.py => resource/nexus} | 0 src/process/nexus/scripts/simulate_heading.py | 54 -- src/process/nexus/setup.py | 26 +- src/process/nexus/src/__init__.py | 0 src/process/nexus/src/nexus/archiver.py | 13 +- src/process/nexus/src/nexus/archiver_core.py | 148 ++-- src/process/view_server/CMakeLists.txt | 201 ----- src/process/view_server/__init__.py | 2 - .../launch/image_view_server.launch | 48 -- .../launch/image_view_server.launch.xml | 16 + .../view_server/launch/web_server.launch | 6 - src/process/view_server/package.xml | 51 +- .../resource/view_server} | 0 src/process/view_server/setup.cfg | 4 + src/process/view_server/setup.py | 33 +- src/process/view_server/src/__init__.py | 2 - .../src/view_server/image_view_server.py | 421 ++++------- .../src/view_server/image_view_server.py.bak | 254 ------- .../view_server/src/view_server/img_nexus.py | 383 +++------- .../src/view_server/web_server_node.py | 66 -- src/run_scripts/entry/nexus.sh | 14 - 30 files changed, 414 insertions(+), 2487 deletions(-) delete mode 100644 src/process/nexus/CMakeLists.txt delete mode 100644 src/process/nexus/launch/evt_listener.launch delete mode 100644 src/process/nexus/launch/nexus.launch delete mode 100644 src/process/nexus/launch/simulate_heading.launch delete mode 100755 src/process/nexus/nexus_node.py delete mode 100755 src/process/nexus/nodes/evt_listener.py delete mode 100755 src/process/nexus/nodes/img_nexus.py rename src/process/nexus/{nodes/__init__.py => resource/nexus} (100%) mode change 100755 => 100644 delete mode 100755 src/process/nexus/scripts/simulate_heading.py delete mode 100644 src/process/nexus/src/__init__.py delete mode 100644 src/process/view_server/CMakeLists.txt delete mode 100644 src/process/view_server/__init__.py delete mode 100644 src/process/view_server/launch/image_view_server.launch create mode 100644 src/process/view_server/launch/image_view_server.launch.xml delete mode 100644 src/process/view_server/launch/web_server.launch rename src/process/{nexus/scripts/__init__.py => view_server/resource/view_server} (100%) create mode 100644 src/process/view_server/setup.cfg delete mode 100644 src/process/view_server/src/__init__.py delete mode 100755 src/process/view_server/src/view_server/image_view_server.py.bak delete mode 100755 src/process/view_server/src/view_server/web_server_node.py delete mode 100755 src/run_scripts/entry/nexus.sh diff --git a/src/core/roskv/src/roskv/util.py b/src/core/roskv/src/roskv/util.py index 9266b412..6e9d2eb5 100755 --- a/src/core/roskv/src/roskv/util.py +++ b/src/core/roskv/src/roskv/util.py @@ -1,7 +1,6 @@ #! /usr/bin/env python # -*- coding: utf-8 -*- from typing import Any, List, Optional, Tuple, Union -from io import BytesIO import json from hashlib import md5 from collections.abc import Mapping @@ -38,11 +37,11 @@ def _unflatten(flat, sep="/"): return result -def hash_genpy_msg(msg): - # type: (genpy.message.Message) -> bytes - buf = BytesIO() - msg.serialize(buf) - return md5(buf.getvalue()).hexdigest() +def hash_ros_msg(msg): + # type: (Any) -> str + from rclpy.serialization import serialize_message + + return md5(serialize_message(msg)).hexdigest() def simple_hash_jsonable(obj): diff --git a/src/process/nexus/CMakeLists.txt b/src/process/nexus/CMakeLists.txt deleted file mode 100644 index c44bfcc4..00000000 --- a/src/process/nexus/CMakeLists.txt +++ /dev/null @@ -1,200 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(nexus) - -## 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 - std_msgs - custom_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 exec_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 exec_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 exec_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 nexus -# CATKIN_DEPENDS roscpp 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}/nexus.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/nexus_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_nexus.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) diff --git a/src/process/nexus/launch/evt_listener.launch b/src/process/nexus/launch/evt_listener.launch deleted file mode 100644 index 38d089a2..00000000 --- a/src/process/nexus/launch/evt_listener.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/src/process/nexus/launch/nexus.launch b/src/process/nexus/launch/nexus.launch deleted file mode 100644 index 89d3c49e..00000000 --- a/src/process/nexus/launch/nexus.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/process/nexus/launch/simulate_heading.launch b/src/process/nexus/launch/simulate_heading.launch deleted file mode 100644 index 9f6a4fac..00000000 --- a/src/process/nexus/launch/simulate_heading.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/process/nexus/nexus_node.py b/src/process/nexus/nexus_node.py deleted file mode 100755 index 5d467281..00000000 --- a/src/process/nexus/nexus_node.py +++ /dev/null @@ -1,36 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import rospy - -from nodes.img_nexus import Nexus - - -def set_up_nexus(): - node = 'img_nexus' - rospy.init_node(node) - print('Parent', rospy.get_namespace()) - node_name = rospy.get_name() - for param in rospy.get_param_names(): - print(param) - verbosity = rospy.get_param('~verbosity') - rgb_topic = rospy.get_param('~rgb_topic') - ir_topic = rospy.get_param('~ir_topic') - uv_topic = rospy.get_param('~uv_topic') - out_topic = rospy.get_param('~out_topic') - max_wait = rospy.get_param('/max_frame_period', 444) / 1000.0 - - return Nexus(rgb_topic, ir_topic, uv_topic, out_topic, max_wait, - verbosity=verbosity) - - -def main(): - nexus = set_up_nexus() - rospy.spin() - - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/process/nexus/nodes/evt_listener.py b/src/process/nexus/nodes/evt_listener.py deleted file mode 100755 index 3e409167..00000000 --- a/src/process/nexus/nodes/evt_listener.py +++ /dev/null @@ -1,51 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -from __future__ import division, print_function, absolute_import -import os -import json -from datetime import datetime - -import rospy - -from std_msgs.msg import Header -from custom_msgs.msg import SynchronizedImages, GSOF_EVT, GSOF_INS, SyncedPathImages, Stat -from nexus.archiver import make_path, ArchiveManager -from nexus.archiver_core import msg_as_dict - - -class EvtListener(object): - """ - Listens to event messages, and if is_archiving, write a evt.json file alongside sync messages. - This is mostly for debugging and profiling. - - """ - - def __init__(self, verbosity=0): - rospy.Subscriber('/event', GSOF_EVT, self.event_queue_callback) - self.archiver = ArchiveManager(agent_name='evt_listener', verbosity=verbosity) - self.last_gps_time = datetime.now() - - def event_queue_callback(self, msg): - # type: (GSOF_EVT) -> None - evttime = datetime.utcfromtimestamp(msg.gps_time.to_sec()) - dt = evttime - self.last_gps_time - self.last_gps_time = evttime - rospy.loginfo("EVT[{: 4d}]: {} dt: {}".format(msg.header.seq, evttime, dt)) - - if not self.archiver.is_archiving: - return - # - this is a debugging node so we can't actually interfere, lest that mess with something - make_dir = False - dd = msg_as_dict(msg) - filename = self.archiver.dump_json(dd, evttime, mode='evt', make_dir=make_dir) - rospy.loginfo(filename) - - -def main(): - rospy.init_node("evt_listener") - EvtListener() - rospy.spin() - -if __name__ == "__main__": - main() diff --git a/src/process/nexus/nodes/img_nexus.py b/src/process/nexus/nodes/img_nexus.py deleted file mode 100755 index ea0d2db3..00000000 --- a/src/process/nexus/nodes/img_nexus.py +++ /dev/null @@ -1,699 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -from __future__ import division, print_function, absolute_import -import os -import sys -import json -from typing import List, Tuple, Optional -from datetime import datetime -import threading -from functools import partial -import time - -# import redis -from six.moves import urllib_parse -import numpy as np -import cv2 - -# ROS imports -import rospy -from cv_bridge import CvBridge -from std_msgs.msg import Bool as MsgBool, Float64 as MsgFloat64, String as MsgString -from std_msgs.msg import Header -from sensor_msgs.msg import Image as MsgImage -from custom_msgs.msg import SynchronizedImages, GSOF_EVT, GSOF_INS, SyncedPathImages, Stat - -# Project imports -from nexus.ros_numpy_lite import ImageEncodingMissingError -from nexus.archiver import make_path -from nexus.archiver import ArchiveManager, SimpleStatsLogger -from nexus.image_leveler import ir_trim_top, eliminate_inband -from nexus.pathimg_bridge import PathImgBridge, ExtendedBridge, coerce_message, InMemBridge -from libnmea_navsat_driver.gsof import GsofSpoofEventDispatch - -G_DO_DEBAYER = True - -MEM_TRANSPORT_DIR = os.environ.get('MEM_TRANSPORT_DIR', False) -if MEM_TRANSPORT_DIR: - membridge = PathImgBridge() - membridge.dirname = MEM_TRANSPORT_DIR - SyncedImageMsg = SyncedPathImages -else: - membridge = ExtendedBridge() - SyncedImageMsg = SynchronizedImages - -bridge = CvBridge() - -bayer_patterns = {} -bayer_patterns['bayer_rggb8'] = cv2.COLOR_BayerBG2RGB -bayer_patterns['bayer_grbg8'] = cv2.COLOR_BayerGB2RGB -bayer_patterns['bayer_bggr8'] = cv2.COLOR_BayerRG2RGB -bayer_patterns['bayer_gbrg8'] = cv2.COLOR_BayerGR2RGB -bayer_patterns['bayer_rggb16'] = cv2.COLOR_BayerBG2RGB -bayer_patterns['bayer_grbg16'] = cv2.COLOR_BayerGB2RGB -bayer_patterns['bayer_bggr16'] = cv2.COLOR_BayerRG2RGB -bayer_patterns['bayer_gbrg16'] = cv2.COLOR_BayerGR2RGB - -ros_immediate = rospy.Duration(nsecs=1) - - -def rostime_to_datetime(stamp): - # type: (rospy.Time) -> datetime - t = stamp.to_sec() - return datetime.utcfromtimestamp(t) - - -def check_image_msg(msg, mode=''): - # type: (MsgImage, Optional[str]) -> Optional[np.ndarray] - """ - Checks validity (presence, size, encoding) of image before sending off - :param msg: Image message object - :param mode: [optional] Type of message - used for logging - :return: Decoded image (this step is pretty fast) or None on failure - """ - if not msg: - rospy.logerr('Message {} is None'.format(mode)) - return None - if not msg.encoding: - rospy.logerr('Message {} is missing encoding'.format(mode)) - return None - - data = bridge.imgmsg_to_cv2(msg) # type: np.ndarray - rospy.logdebug(data.shape, data.size) - if not data.shape: - rospy.logerr('Message {} has no shape'.format(mode)) - return None - if not data.size: - rospy.logerr('Message {} has no size'.format(mode)) - return None - - if data.ndim not in (2, 3): - rospy.logerr('Message {} has incorrect ndim: {}'.format(mode, data.ndim)) - return None - - if not(np.prod(data.shape)): - rospy.logerr('Message {} has null shape: {}'.format(mode, data.shape)) - return None - - return data - - -def dump_image_array(filename, data, verbosity=0): - # type: (str, MsgImage, int) -> None - - start = time.time() - - # Extra jpg params won't hurt writing other formats - cv2.imwrite(filename, data, (cv2.IMWRITE_JPEG_QUALITY, 100)) - end = time.time() - if verbosity >= 2: - rospy.loginfo("Image Writer saved: {} in {:.3f}s".format(filename, end - start)) - - -def dump_image_msg(filename, msg, mode='', verbosity=0): - # type: (str, MsgImage, str, int) -> None - if not msg.encoding: - raise ImageEncodingMissingError - - start = time.time() - data = bridge.imgmsg_to_cv2(msg) # type: np.ndarray - if mode == 'ir': - data = eliminate_inband(data) - if verbosity >= 5: - print(data.shape, data.size) - - # in the rare event of the filename being a dupe, just tag it as such - if os.path.exists(filename): - rospy.logerr("OOPS! Duplicate: {}".format(filename)) - fn, ext = os.path.splitext(filename) - filename = fn + '_dupe' + ext - - - - # Extra jpg params won't hurt writing other formats - cv2.imwrite(filename, data, (cv2.IMWRITE_JPEG_QUALITY, 100)) - end = time.time() - if verbosity >= 4: - print('{} {} {:.3f} sec'.format(msg.encoding, data.shape, end - start)) - if verbosity >= 2: - rospy.loginfo("Image Writer saved: {}".format(filename)) - - -def debayer_image_msg(msg, do_debayer=G_DO_DEBAYER): - # type: (MsgImage, bool) -> MsgImage - """ - Optionally debayer an image message - :param msg: - :param do_debayer: - :return: processed image message - """ - if not do_debayer: - return msg - tic = time.time() - if msg.encoding in bayer_patterns.keys(): - rospy.logdebug('DeBayering from encoding {}'.format(msg.encoding)) - image = bridge.imgmsg_to_cv2(msg) - - # image = self.gamma_to_linear_lut[image] - image = cv2.cvtColor(image, bayer_patterns[msg.encoding]) - # image = self.linear_to_gamma_lut[image] - - # White balance - """ - RGB_rescale = [0.59987517, 1, 0.96323181] - for i in range(3): - lut = np.round(np.arange(256)*RGB_rescale[i]).astype(np.uint8) - image[:,:,i] = cv2.LUT(image[:,:,i], lut) - """ - - debayered_msg = bridge.cv2_to_imgmsg(image, encoding="rgb8") - - debayered_msg.header.stamp = msg.header.stamp - debayered_msg.header.frame_id = msg.header.frame_id - rospy.logdebug('Debayer Time elapsed: {:.3f} s'.format(time.time() - tic)) - else: - rospy.logwarn('Unrecognized Bayer encoding `{}`'.format(msg.encoding)) - return msg - return debayered_msg - - -class LowpassIIR(object): - """ - Digital Infinite impulse response lowpass filter AKA exponential moving - average. Smooths values. - """ - def __init__(self, gamma=0.1, init_state=1.0): - """ - :param gamma: Coefficient for lowpass, (0,1] - gam=1 -> 100% pass - """ - self.gamma = gamma - self.state = init_state - - def update(self, x): - """ - Push a value into the filter - :param x: Value of input signal - :return: Lowpassed signal output - """ - self.state = (x * self.gamma) + (1.0-self.gamma) * self.state - return self.state - - -class Nexus(object): - """ - Buffering camera stream. Will gather frames from an incoming topic, push - them to a deque (automatically sheds to buffer_size) continuous. When rip() - is called, the most recent frame is returned and the deque cleared. - - """ - symbol_dict = {'rgb': '█', 'ir': '▒', 'uv': 'Ü', 'evt': 'E' } - - def __init__(self, rgb_topic, ir_topic, uv_topic, out_topic, max_wait=0.66, - verbosity=0): - """ - :param rgb_topic: Topic to receive RGB ROS Image messages on. - :type rgb_topic: str - - :param ir_topic: Topic to receive IR ROS Image messages on. - :type ir_topic: str - - :param uv_topic: Topic to receive UV ROS Image messages on. - :type uv_topic: str - - :param out_topic: Topic to publish ROS SynchronizedImages messages on. - :type out_topic: str - - :param max_wait: Time to wait after receiving one image for the - other-modality images to arrive (seconds). - :type max_wait: float - - """ - raise NotImplementedError("This code path is disabled") - # redis_host = os.environ.get('REDIS_HOST', 'nuvo0') - # self.rc = redis.Redis(host=redis_host) - # print('redis established, term: {}'.format(self.rc.get('term'))) - node_host = rospy.get_namespace().strip('/') - cam_fov = rospy.get_param(os.path.join('/cfg', 'hosts', node_host, 'fov')) - max_frame_rate = rospy.get_param('/cfg/parameters/max_frame_rate') - - if MEM_TRANSPORT_DIR: - out_topic += '_shm' - - - self.image_formats = {} - for chan in ['rgb', 'uv', 'ir', 'evt', 'ins']: - self.image_formats[chan] = rospy.get_param(os.path.join('/cfg/file_formats', chan)) - - max_wait = 1.0 / max_frame_rate - rospy.loginfo("node host: {} fov: {} max_wait: {:.3f}".format(node_host, cam_fov, max_wait)) - - self.node_host = node_host - self.cam_fov = cam_fov - self.node_name = rospy.get_name() - - self.image_lock = threading.RLock() - self.pub_timer = None - self._current_epoch = rospy.Time.now() - self.epoch_dict = dict() - self._msg_dict = dict() - self._recent_epochs = [] - self.max_wait = max_wait - self.rolling_success = LowpassIIR() - self.topics = {'rgb_topic': rospy.resolve_name(rgb_topic), 'ir_topic': rospy.resolve_name(ir_topic), - 'uv_topic': rospy.resolve_name(uv_topic), 'out_topic': rospy.resolve_name(out_topic)} - - self.enabled = { - 'rgb': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'rgb'), True), - 'ir': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'ir'), True), - 'uv': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'uv'), False) - } - self.enabled_list = [k for k, v in self.enabled.items() if v] - self.full_packet_list = self.enabled_list + ['evt'] - self.skip_ir = not self.enabled['ir'] - self.skip_uv = not self.enabled['uv'] - self._is_archiving = False - self._ir_out_encoding = 'mono8' - self.verbosity = verbosity - self._pub_ir_leveled = True # Outputs a stream of z-normalized IR - self.archiver = ArchiveManager(verbosity=verbosity) - self.archiver.advertise_services() - self.stats_logger = SimpleStatsLogger(archiver=self.archiver) - self.pub_missed = {} # publish when a frame is missed - self.image_writers = {} - - if self.enabled['rgb']: - rospy.loginfo('Subscribing to Images topic \'%s\'' - % rgb_topic) - rospy.Subscriber(rgb_topic, MsgImage, self.any_queue_callback, - callback_args='rgb', queue_size=1) - self.pub_missed['rgb'] = rospy.Publisher('rgb/missed', Header, queue_size=5) - - if self.enabled['ir']: - rospy.loginfo('Subscribing to Images topic \'%s\'' - % ir_topic) - rospy.Subscriber(ir_topic, MsgImage, self.any_queue_callback, - callback_args='ir', queue_size=1) - self.pub_missed['ir'] = rospy.Publisher('ir/missed', Header, queue_size=5) - - if self.enabled['uv']: - rospy.loginfo('Subscribing to Images topic \'%s\'' - % uv_topic) - rospy.Subscriber(uv_topic, MsgImage, self.any_queue_callback, - callback_args='uv', queue_size=1) - self.pub_missed['uv'] = rospy.Publisher('uv/missed', Header, queue_size=5) - - rospy.Subscriber('/event', GSOF_EVT, self.any_queue_callback, - callback_args='evt') - - self.publisher = rospy.Publisher( - out_topic, SyncedImageMsg, queue_size=1) - - self.pub_status = rospy.Publisher( - 'status', MsgString, queue_size=3) - - self.stat_pub = rospy.Publisher('/stat', Stat, queue_size=3) - self.pstat_pub = rospy.Publisher(self.node_name + '/stat', Stat, queue_size=3) - self.stat_counter = 0 - - @property - def msg_dict(self): - """Get the most recent message dict""" - return self.epoch_dict.get(self._current_epoch, {}) - - def get_spoof_event(self): - rospy.logwarn('No event msg detected, generating spoof event') - return GsofSpoofEventDispatch() - - def is_msg_dict_full(self): - """Check if all requisite messages have been received (regardless of - image message content) - """ - check_evt = 'evt' in self.msg_dict - check_rgb = ('rgb' not in self.enabled_list) or ('rgb' in self.msg_dict) - check_ir = ('ir' not in self.enabled_list) or ('ir' in self.msg_dict) - check_uv = ('uv' not in self.enabled_list) or ('uv' in self.msg_dict) - return all([check_evt, check_rgb, check_ir, check_uv]) - - def reset_timer(self): - if self.pub_timer is not None: - self.pub_timer.shutdown() - self.pub_timer = None - - def timer_writer_callback(self, timer_event=None, msg=None, mode=''): - if not msg: - raise RuntimeError("No message in timer callback, this should not happen") - if not mode: - raise RuntimeError("No mode in timer callback, this should not happen") - - self.image_writer_callback(msg=msg, args=mode) - - def image_writer_callback(self, msg, args): - # type: (MsgImage, str) -> None - mode = args - ext = self.image_formats[mode] - - data = check_image_msg(msg, mode) - # We want to emit missed frame messages iff message is bad and we are archiving - if data is None and self.archiver.is_archiving: - self.pub_missed[mode].publish(msg.header or Header()) - - if not self.archiver.is_archiving: - return - now = datetime.utcfromtimestamp(msg.header.stamp.to_sec()) - template = self.archiver.fmt_sync_path(now) - filename = template.format(mode=mode, ext=ext) - dirname = make_path(filename, from_file=True) - try: - dump_image_msg(filename, msg, mode, verbosity=self.verbosity) - - except ImageEncodingMissingError: - pass # we logged this with check_image - except Exception: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("dump_image_msg failed: {}: {}".format(exc_type, value)) - - def end_of_turn(self, stale_time=0.75): - """ - Finalize and publish completed packets - :param stale_time: - :param timeout_time: - :return: - """ - stale_time = rospy.Duration.from_sec(stale_time) - now = rospy.Time.now() - completed = [] - stale = [] - with self.image_lock: - for ep in self.epoch_dict: - msg_dict = self.epoch_dict.get(ep) - age = now - ep - if all(key in msg_dict for key in self.full_packet_list): - rospy.loginfo("[_] Comp {: >4}: {} {}".format(msg_dict['evt'].event_num, ep, msg_dict.keys())) - completed.append(ep) - - elif age > stale_time: - rospy.logerr("[_] Messages timed out, epoch {}: {}".format(ep, msg_dict.keys())) - stale.append(ep) - else: - pass - # rospy.loginfo("[_] Partial : {}".format(ep)) - - for candidate in completed + stale: - msg_dict = self.epoch_dict.pop(candidate) - # rospy.loginfo("Publishing {}".format(candidate)) - self._publish(msg_dict=msg_dict) - - self._recent_epochs = self._recent_epochs[-20:] - - def any_queue_callback(self, msg, modality='evt'): - modality = modality.lower() - urlp = urllib_parse.urlparse(msg.header.frame_id) - qs = urllib_parse.parse_qs(urlp.query) - rospy.loginfo('<^>{:>3} {:>6}: {:.6f} {}'.format(modality, qs.get('eventNum', ['?'])[0], msg.header.stamp.to_sec(), qs)) - - if modality == 'evt': - self.event_queue_callback(msg, modality=modality) - else: - self.sync_queue_callback(msg, modality=modality) - - def event_queue_callback(self, event_msg, modality='evt'): - modality = modality.lower() - stat = Stat() - stat.trace_header = event_msg.header - stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 - stat.trace_topic = self.node_name + '/queue/' + modality - - # rospy.loginfo('<^>{:>3} {:>6}: {:.6f}'.format(modality, event_msg.header.seq, event_msg.header.stamp.to_sec())) - # rospy.loginfo(modality + ': ' + str(image_msg.header)) - with self.image_lock: - current_epoch = event_msg.header.stamp - msg_dict = self.epoch_dict.get(current_epoch, {}) - if len(msg_dict): - rospy.logwarn("Messages beat event: {}".format(msg_dict.keys())) - msg_dict.update({'evt': event_msg}) - self.epoch_dict[current_epoch] = msg_dict - rospy.loginfo("Starting {: >4}: epoch {}, epochs: {}".format(event_msg.header.seq, current_epoch, self.epoch_dict.keys())) - if current_epoch in self._recent_epochs: - rospy.logerr("Duplicate event! {}".format(event_msg.header)) - else: - self._recent_epochs.append(current_epoch) - self._current_epoch = current_epoch - - self.stat_pub.publish(stat) - self.end_of_turn() - - def insert_msg(self, image_msg, modality='rgb'): - modality = modality.lower() - stat = Stat() - stat.trace_header = image_msg.header - stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 - stat.trace_topic = self.node_name + '/queue/' + modality - - # rospy.loginfo('<^>{:>3} {:>6}: {:.6f}'.format(modality, image_msg.header.seq, image_msg.header.stamp.to_sec())) - # rospy.loginfo(modality + ': ' + str(image_msg.header)) - with self.image_lock: - epoch = image_msg.header.stamp - msg_dict = self.epoch_dict.get(epoch, {}) - if 'evt' not in msg_dict: - rospy.logwarn("{} Message beat event: {}, epochs: {}".format(modality, epoch, self.epoch_dict.keys())) - if modality in msg_dict: - rospy.logerr("Duplicate message {} in epoch: {}".format(modality, epoch)) - if modality == 'rgb': - image_msg = debayer_image_msg(image_msg) - - msg_dict.update({modality: image_msg}) - self.epoch_dict[epoch] = msg_dict - # send message off to be written in separate thread (hopefully) - if modality in ['rgb', 'uv', 'ir']: - rospy.Timer(ros_immediate, - partial(self.timer_writer_callback, msg=image_msg, - mode=modality), - oneshot=True) - - self.stat_pub.publish(stat) - - def sync_queue_callback(self, image_msg, modality): - self.insert_msg(image_msg=image_msg, modality=modality) - self.end_of_turn() - - def sync_queue_callback2(self, image_msg, modality): - # type: (MsgImage, str) -> None - """Method that receives messages published on self.image_topic - - :param image_msg: ROS image message. - :type image_msg: Image - - :param modality: Which image stream from which to return an image view. - :type modality: str {'EVT', 'RGB','IR','UV'} - - """ - modality = modality.lower() - stat = Stat() - stat.trace_header = image_msg.header - stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 - stat.trace_topic = self.node_name + '/queue/' + modality - - rospy.loginfo('{:>3} {:>6}: {:.6f}'.format(modality, image_msg.header.seq, image_msg.header.stamp.to_sec())) -# rospy.loginfo(modality + ': ' + str(image_msg.header)) - with self.image_lock: - header = image_msg.header - t = header.stamp.secs + header.stamp.nsecs / 1e9 - t = datetime.utcfromtimestamp(t) - if modality == 'evt': - raise NotImplementedError("Dead end! shouldn't happen") - self.stat_pub.publish(stat) - - if header.stamp != self._current_epoch: - if header.stamp in self._recent_epochs: - rospy.logerr("Stale epoch on {}: {}".format(modality, header.stamp)) - else: - rospy.logerr("Stale epoch on {}: {}".format(modality, header.stamp)) - -# rospy.loginfo('{:>3} {:>6} {}'.format(modality, image_msg.header.seq, t.isoformat()[11:24])) - -# rospy.logdebug('{:>3} {:>6} {:.3f}'.format(modality, image_msg.header.seq, image_msg.header.stamp.to_sec())) - - if modality == 'rgb': - image_msg = debayer_image_msg(image_msg) - - # send message off to be written in separate thread (hopefully) - if modality in ['rgb', 'uv', 'ir']: - rospy.Timer(ros_immediate, - partial(self.timer_writer_callback, msg=image_msg, - mode=modality), - oneshot=True) - - - if modality == 'evt' and modality in self.msg_dict: - # oops, we got double event before buffer filled - # publish and roll over message - raise NotImplementedError("Dead end! shouldn't happen") - rospy.logwarn("OOPS double event! Missed packet?: {}".format( - self.msg_dict.keys())) - self.publish() - self.msg_dict.update({modality: image_msg}) - return - elif modality == 'evt' and 'ir' in self.msg_dict: - evt_time = image_msg.header.stamp.to_sec() - msg_time = self.msg_dict['ir'].header.stamp.to_sec() - rospy.logwarn("IR beat event by {}".format(evt_time - msg_time)) - if abs(evt_time - msg_time) < 0.499: # empirically determined IR can lead by as much as 650 ms but system capped at 2 Hz - self.msg_dict.update({modality: image_msg}) - rospy.logwarn("This is fine") - else: - rospy.logerr("Publishing incomplete message: {}".format( - self.msg_dict.keys())) - self.publish() - self.msg_dict.update({modality: image_msg}) - elif modality == 'evt' and ('rgb' in self.msg_dict or 'uv' in self.msg_dict): - # ok we got event but there is stuff? reset the cycle - # assume event always makes it first - rospy.logwarn("got event but stuff in buffer: {}".format( - self.msg_dict.keys())) - self.publish() - self.msg_dict.update({modality: image_msg}) - else: - self.msg_dict.update({modality: image_msg}) - - - if self.verbosity > 10: - # visual symbols for fast debugging - smsg = '{} Rx {: >4} {}'.format( - self.symbol_dict.get(modality), modality, - rostime_to_datetime(image_msg.header.stamp).isoformat()) - rospy.loginfo('sync msg: {}'.format(smsg)) - if self.is_msg_dict_full(): - self.publish() - elif self.pub_timer is None: - # Start a new timer to publish after 'max_wait'. - self.pub_timer = rospy.Timer(rospy.Duration(self.max_wait), - self.publish, oneshot=True) - self.end_of_turn() - - def check_success(self, msg_dict): - # type: (dict) -> Tuple[list, list] - """Returns list of names of all messages present and non-zero in message buffer - dict, along with list of those that failed""" - success_list = [] - fail_list = [] - for chan in self.enabled_list + ['evt', 'ins']: - if chan not in msg_dict: - # rospy.logerr('Expecting {} Message, not in msg_dict '.format(chan)) - fail_list.append(chan) - continue - - if chan in ['evt', 'ins']: - result = True - else: - result = check_image_msg(msg_dict[chan], chan) - - if result is not None: - success_list.append(chan) - - return success_list, fail_list - - def publish(self, timer_event=None, record_stats=True): - self._publish(timer_event, msg_dict=self.msg_dict, record_stats=record_stats) - - def _publish(self, timer_event=None, msg_dict=None, record_stats=True): - self.reset_timer() - print('topics: {}'.format(self.topics)) - stat = Stat() - with self.image_lock: - stat.node = self.node_name - stat.trace_topic = self.node_name + '/' + 'sync' - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 - if timer_event is not None: - rospy.logerr("Publishing due to timer callback") - msg_dict['ins'] = self.archiver.latch_ins - rospy.logdebug("Pub'd: {}".format(msg_dict.keys())) - - if not any(msg_dict): - # why does this happen? - rospy.logerr("Tried to publish, but no data in buffer") - return - - outmsg = SyncedImageMsg() - - success_list, fail_list = self.check_success(msg_dict) - - success = float(not len(fail_list)) - success_rate = self.rolling_success.update(success) - record = {'ts': datetime.now().isoformat(), - 'have_evt': 'evt' in success_list, - 'have_rgb': 'rgb' in success_list, - 'have_ir' : 'ir' in success_list, - 'have_uv' : 'uv' in success_list} - - # keep only good data messages - this also should simplify dump_sync - msg_dict = {k: msg_dict[k] for k in success_list} - - s = 'img_nexus.py:publish() \n' - for k, v in msg_dict.items(): - s += '||{:>3}: {:.3f}\n'.format(k, v.header.stamp.to_sec()) - # rospy.loginfo(s) - - # Deal with missing event, we still need a header - if 'evt' not in msg_dict: - msg_dict['evt'] = self.get_spoof_event() - event = msg_dict.get('evt') - stat.meta_json = json.dumps(record) - - for mode in self.enabled_list: - if mode in msg_dict: - msg = msg_dict.get(mode) - msg = coerce_message(msg, membridge) - msg.header = event.header - filename = self.archiver.filename_from_msg(msg, mode) - setattr(outmsg, 'image_' + mode, msg) - setattr(outmsg, 'file_path_' + mode, filename) - - pathdict = {} - if 'ir' in msg_dict: - msg_ir = msg_dict['ir'] - msg_dict['ir'] = ir_trim_top(msg_ir) - - if self.archiver.is_archiving: - pathdict = self.archiver.dump_sync_image_messages(msg_dict) - self.stats_logger.append(record) - if self.verbosity > 3: - rospy.loginfo('pathdict: {}'.format(pathdict)) - else: - rospy.loginfo('archived') - - # Reset all the image buffers. - msg_dict = dict() - - - infostr = "SYN ({} {} {}{}) {: >3.0%}"\ - .format('EVT' * record['have_evt'] or ' ', - 'RGB' * record['have_rgb'] or ' ', - 'IR' * record['have_ir'] or ' ', - ' UV ' * record['have_uv'] or '', - success_rate - ) - infomsg = MsgString() - infomsg.data = infostr - outmsg.header = event.header - stat.trace_header = event.header - seq = event.event_num - stat.link = self.node_name + '/sync/event/{}'.format(seq) - stat.note = 'success' if success else 'sync_fail' - self.stat_pub.publish(stat) - self.pstat_pub.publish(stat) - self.pub_status.publish(infomsg) - self.publisher.publish(outmsg) - rospy.loginfo(infostr) diff --git a/src/process/nexus/package.xml b/src/process/nexus/package.xml index 56280006..a993d9f7 100644 --- a/src/process/nexus/package.xml +++ b/src/process/nexus/package.xml @@ -1,75 +1,26 @@ - + + nexus - 0.0.0 - The nexus package + 1.0.0 + KAMERA archiving and image-transport library - - - Adam Romlein Michael McDermott - - - - - Apache 2.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - cv_bridge - roscpp - rospy - std_msgs - custom_msgs - ins_driver - roscpp - rospy - std_msgs - cv_bridge - roscpp - rospy + rclpy std_msgs + sensor_msgs custom_msgs - ins_driver - + cv_bridge + roskv + python3-numpy + python3-opencv + python3-yaml - - - + + ament_python diff --git a/src/process/nexus/nodes/__init__.py b/src/process/nexus/resource/nexus old mode 100755 new mode 100644 similarity index 100% rename from src/process/nexus/nodes/__init__.py rename to src/process/nexus/resource/nexus diff --git a/src/process/nexus/scripts/simulate_heading.py b/src/process/nexus/scripts/simulate_heading.py deleted file mode 100755 index e66bfa31..00000000 --- a/src/process/nexus/scripts/simulate_heading.py +++ /dev/null @@ -1,54 +0,0 @@ -#! /usr/bin/python -from __future__ import division, print_function -import numpy as np - -# ROS imports -import rospy -from std_msgs.msg import String - - -def main(): - # Launch the node. - node = 'simulate_heading' - rospy.init_node(node, anonymous=False) - node = rospy.get_name() - - heading0 = rospy.get_param(''.join([node,'/nominal_heading'])) - heading_range = rospy.get_param(''.join([node,'/heading_range'])) - motion_rate = rospy.get_param(''.join([node,'/motion_rate'])) - pub_rate = rospy.get_param(''.join([node,'/pub_rate'])) - topic = rospy.get_param(''.join([node,'/topic'])) - - print('Nominal heading (deg): ', heading0) - print('Heading range: ', heading_range) - print('Motion rate (deg/s): ', motion_rate) - print('Publish rate: ', pub_rate) - print('BaselineHeading topic: ', topic) - # ------------------------------------------------------------------------ - - heading_pub = rospy.Publisher(topic, String, queue_size=1) - - rate = rospy.Rate(pub_rate) - t0 = rospy.get_time() - heading = heading0 - while not rospy.is_shutdown(): - if heading_range > 0 and motion_rate > 0: - t = rospy.get_time() - t0 - heading = heading0 + heading_range*np.sin(t*motion_rate/heading_range*2*np.pi) - - print('heading:', heading) - msg = String("foo") - # msg.heading = heading*1000 - # msg.n_sats = 10 - # t = rospy.get_time() - # msg.header.stamp.secs = int(np.floor(t)) - # msg.header.stamp.nsecs = int((t - msg.header.stamp.secs)*1e9) - heading_pub.publish(msg) - rate.sleep() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/process/nexus/setup.py b/src/process/nexus/setup.py index 8dd2140c..fd35024e 100755 --- a/src/process/nexus/setup.py +++ b/src/process/nexus/setup.py @@ -1,10 +1,20 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['nexus'], - install_requires=["backports.tempfile"], - package_dir={'': 'src'}) +package_name = "nexus" -setup(**d) +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="KAMERA archiving and image-transport library", + license="Apache 2.0", +) diff --git a/src/process/nexus/src/__init__.py b/src/process/nexus/src/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/process/nexus/src/nexus/archiver.py b/src/process/nexus/src/nexus/archiver.py index 18e6823b..062c184e 100755 --- a/src/process/nexus/src/nexus/archiver.py +++ b/src/process/nexus/src/nexus/archiver.py @@ -40,17 +40,18 @@ class ArchiveManager(ArchiverBase): } def __init__( - self, agent_name="ArchiveManager", bytes_halt_archiving=1e9, verbosity=0 + self, node, agent_name="ArchiveManager", bytes_halt_archiving=1e9, verbosity=0 ): """ Class for managing the archiving of data coming from the system. By convention, paths are '/' terminated. + :param node: rclpy node owning the ROS interfaces :param agent_name: :param bytes_halt_archiving: When free bytes drops below this number, halt :param verbosity: """ - super(ArchiveManager, self).__init__(agent_name=agent_name, verbosity=verbosity) + super(ArchiveManager, self).__init__(node, agent_name=agent_name, verbosity=verbosity) def dump_image_msg(self, msg, mode, fn_template, ext="tif"): # type: (genpy.msg, str, str, str) -> Optional[str] @@ -82,8 +83,8 @@ def filename_from_msg(self, msg, mode, time=None): # type: (genpy.msg, str) -> str ext = self.image_formats[mode] if time is None: - secs = msg.header.stamp.secs - nsecs = msg.header.stamp.nsecs + secs = msg.header.stamp.sec + nsecs = msg.header.stamp.nanosec usecs = int(nsecs / 1e3) fracs = float(usecs / 1e6) t = secs + fracs @@ -109,8 +110,8 @@ def dump_sync_image_messages(self, msg_dict, ext="jpg"): # Always round down the last sigfig maintain parity between # Python / C++ saving. - secs = event.header.stamp.secs - nsecs = event.header.stamp.nsecs + secs = event.header.stamp.sec + nsecs = event.header.stamp.nanosec usecs = int(nsecs / 1e3) fracs = float(usecs / 1e6) t = secs + fracs diff --git a/src/process/nexus/src/nexus/archiver_core.py b/src/process/nexus/src/nexus/archiver_core.py index 121f5d60..532763e3 100755 --- a/src/process/nexus/src/nexus/archiver_core.py +++ b/src/process/nexus/src/nexus/archiver_core.py @@ -5,9 +5,9 @@ import os import re import errno +import socket import datetime -# import threading import time import json import yaml @@ -16,30 +16,25 @@ try: - import rospy - import genpy + from builtin_interfaces.msg import Time as MsgTime - # from profilehooks import timecall from std_msgs.msg import Header from std_msgs.msg import UInt64 as MsgUInt64 from std_msgs.msg import String as MsgString - from msgdispatch.archive import ArchiveSchemaDispatch from custom_msgs.srv import SetArchiving, AddToEventLog - from custom_msgs.msg import GSOF_INS, GSOF_EVT + from custom_msgs.msg import GsofIns, GsofEvt except ImportError as exc: import sys print( - "cannot import rospy or messages. if this is not a test environment, this is bad!", + "cannot import rclpy interfaces. if this is not a test environment, this is bad!", file=sys.stderr, ) if not os.environ.get("IGNORE_ROS_IMPORT", False): raise exc -# from custom_msgs.srv import EraseDataDisk - PAT_BRACED = re.compile(r"\{(\w+)\}") PAT_DOUBLE_SLASH = re.compile(r"//") @@ -66,15 +61,24 @@ def conformKwargsToFormatter(tmpl, kwargs): def msg_as_dict(msg): - if isinstance(msg, genpy.rostime.TVal): - return msg.to_sec() - elif isinstance(msg, genpy.message.Message): - return {str(k): msg_as_dict(getattr(msg, k)) for k in msg.__slots__} + """Recursively convert a ROS2 message to plain python types.""" + if isinstance(msg, MsgTime): + return msg.sec + msg.nanosec * 1e-9 + elif hasattr(msg, "get_fields_and_field_types"): + return { + str(k): msg_as_dict(getattr(msg, k)) + for k in msg.get_fields_and_field_types() + } elif isinstance(msg, dict): return {str(k): v for k, v in msg.items()} return msg +def stamp_to_sec(stamp): + # type: (MsgTime) -> float + return stamp.sec + stamp.nanosec * 1e-9 + + def pathsafe_timestamp(now=None, show_micros=False, show_millis=False): # type: (Optional[datetime.datetime], bool, bool) -> str """ @@ -140,7 +144,7 @@ class _Fmt(object): @staticmethod def msg_as_dict_headless(msg): """unused I think""" - dd = {k: getattr(msg, k) for k in msg.__slots__} + dd = {k: getattr(msg, k) for k in msg.get_fields_and_field_types()} dd.pop("header", None) return dd @@ -151,7 +155,7 @@ def get_image_msg_meta(msg): for field in fields: x = getattr(msg, field, None) d.update({field: str(x)}) - fields = ["seq", "stamp", "frame_id"] + fields = ["stamp", "frame_id"] for field in fields: x = getattr(msg.header, field, None) d["header"].update({field: str(x)}) @@ -163,7 +167,6 @@ def get_field_abr(field): """Ambigous abbreviations, this use should be discouraged""" return field[0].upper() - # @pysnooper.snoop() @staticmethod def fmt_filename( proj, flight, ts, field=None, mode="{mode}", ext="{ext}", note=None @@ -197,22 +200,23 @@ def fmt_log_filename(): class ArchiverBase(object): - def __init__(self, agent_name=None, bytes_halt_archiving=1e9, verbosity=0): + def __init__(self, node, agent_name=None, bytes_halt_archiving=1e9, verbosity=0): """ Class for managing the archiving of data coming from the system. By convention, paths are '/' terminated. + + :param node: rclpy node owning the subscriptions/publishers/services """ - node_host = rospy.get_namespace().strip("/") - cam_fov = rospy.get_param( - os.path.join("/cfg", "hosts", node_host, "fov"), "node" - ) + self.node = node + self.log = node.get_logger() + node_host = os.environ.get("NODE_HOSTNAME") or socket.gethostname() self.node_host = node_host self._redis_host = os.environ.get("REDIS_HOST", "nuvo0") ## deprecated self.verbosity = verbosity - self._name_system = rospy.get_param("/system_name", "default_system") + self._name_system = os.environ.get("SYSTEM_NAME", "default_system") self._name_sync = "sync" self._name_ins = "ins" self._name_meta = "meta" @@ -224,11 +228,16 @@ def __init__(self, agent_name=None, bytes_halt_archiving=1e9, verbosity=0): self.erase_service = None self.bytes_halt_archiving = bytes_halt_archiving - rospy.Subscriber("/ins", GSOF_INS, self.ins_callback) - rospy.Subscriber("/event", GSOF_EVT, self.evt_callback) + node.create_subscription(GsofIns, "/ins", self.ins_callback, 10) + node.create_subscription(GsofEvt, "/event", self.evt_callback, 10) - self.pub_diskfree = rospy.Publisher("disk_free_bytes", MsgUInt64, queue_size=1) + self.pub_diskfree = node.create_publisher(MsgUInt64, "disk_free_bytes", 1) + self.rawmsg_pub = node.create_publisher(MsgString, "/rawmsg", 99) self.counter = 0 + + self.envoy = RedisEnvoy(self._redis_host, client_name=node_host + "-nexus") + self.state_service = StateService(self.envoy, node_host, "nexus") + self.image_formats = {} default_types = { "rgb": "jpg", @@ -238,16 +247,13 @@ def __init__(self, agent_name=None, bytes_halt_archiving=1e9, verbosity=0): "ins": "json", } for chan in ["rgb", "uv", "ir", "evt", "ins"]: - self.image_formats[chan] = rospy.get_param( - os.path.join("/cfg/file_formats", chan), default_types[chan] - ) - - self.last_ins = GSOF_INS() - self.latch_ins = GSOF_INS() - print("\n\n VERSION CHECK 1 \n") + try: + self.image_formats[chan] = self.envoy.get("/sys/arch/ext_%s" % chan) + except KeyError: + self.image_formats[chan] = default_types[chan] - self.envoy = RedisEnvoy(self._redis_host, client_name=node_host + "-nexus") - self.state_service = StateService(self.envoy, node_host, "nexus") + self.last_ins = GsofIns() + self.latch_ins = GsofIns() # Get Redis Params arch = self.envoy.get("/sys/arch") @@ -369,25 +375,21 @@ def advertise_services( # type: (str, str, str, str) -> None if namespace is None: - namespace = rospy.get_namespace() - - else: - if namespace not in ["/", "~"] and namespace[-1] != "/": - namespace += "/" + namespace = self.node.get_namespace() + if namespace not in ["/", "~"] and namespace[-1] != "/": + namespace += "/" + if not namespace.startswith("/"): + namespace = "/" + namespace name_archive = namespace + name_archive name_log = namespace + name_log - name_erase = namespace + name_erase - self.archive_service = rospy.Service( - name_archive, SetArchiving, self.call_set_archiving + self.archive_service = self.node.create_service( + SetArchiving, name_archive, self.call_set_archiving ) - self.log_service = rospy.Service( - name_log, AddToEventLog, self.call_add_to_event_log + self.log_service = self.node.create_service( + AddToEventLog, name_log, self.call_add_to_event_log ) - # self.erase_service = rospy.Service(name_log, EraseDataDisk, self.call_erase_disk) - rospy.loginfo("Subscribing {} to {}".format(namespace, name_archive)) - rospy.loginfo("Subscribing {} to {}".format(namespace, name_log)) - - # self.erase_service = rospy.Service(name_log, EraseDataDisk, self.call_erase_disk) + self.log.info("Subscribing {} to {}".format(namespace, name_archive)) + self.log.info("Subscribing {} to {}".format(namespace, name_log)) def fmt_flight_path(self): # type: () -> str @@ -483,7 +485,6 @@ def fmt_flight_file_path(self, field=None): Returns: Fully qualified path with {mode} and {ext} substitution points """ - # init_time_short = pathsafe_timestamp(self._init_time, show_millis=True) # leave mode and ext to be formatted by the file dump filename = self.fmt_filename("", field) @@ -542,7 +543,7 @@ def dump_json(self, data, time=None, mode="meta", make_dir=True): make_path(filename, from_file=True) else: if not os.path.exists(os.path.dirname(filename)): - rospy.logerr( + self.log.error( "Archiving directory not created yet and `make_dir` set to false. " "Could not write: {}".format(filename) ) @@ -577,15 +578,15 @@ def dump_log_yaml(self, data): def update_project_flight(self, project, flight, effort="", collection_mode="?"): if not project: - rospy.logwarn("Missing project string, setting to default") + self.log.warning("Missing project string, setting to default") project = "arch_core_svc_no_project" if not flight: - rospy.logwarn("Missing flight string, setting to default") + self.log.warning("Missing flight string, setting to default") flight = "00" if not effort: - rospy.logwarn("Missing effort string, setting to default") + self.log.warning("Missing effort string, setting to default") effort = "arch_core_svc_no_effort" self._project = project @@ -594,14 +595,13 @@ def update_project_flight(self, project, flight, effort="", collection_mode="?") self._collection_mode = collection_mode return project, flight, effort - def call_set_archiving(self, req): - # type: (SetArchiving) -> bool - rospy.loginfo("!! DEPRECATED !! call_set_archiving v2: {}".format(req)) - return True + def call_set_archiving(self, req, resp): + self.log.info("!! DEPRECATED !! call_set_archiving v2: {}".format(req)) + resp.success = True + return resp - def call_add_to_event_log(self, req): - # type: (AddToEventLog) -> bool - rospy.loginfo("maybe deprecating? call_add_to_event_log: {}".format(req)) + def call_add_to_event_log(self, req, resp): + self.log.info("maybe deprecating? call_add_to_event_log: {}".format(req)) self.update_project_flight( req.project, req.flight, req.effort, req.collection_mode ) @@ -615,21 +615,15 @@ def call_add_to_event_log(self, req): "collection_mode": req.collection_mode, } self.dump_log_yaml(data) - return True + resp.success = True + return resp def update_schema(self, msg): - # type: (ArchiveSchemaDispatch) -> None - rospy.loginfo("Set schema: \n{}".format(str(msg))) + self.log.info("Set schema: \n{}".format(str(msg))) self._project = msg.project fl_number = "".join([d for d in msg.flight if d.isdigit()]) self._flight = "fl{:0>2}".format(fl_number) - @staticmethod - def call_erase_disk(req): - print(req) - parent_host = rospy.get_namespace().strip("/") - rospy.logwarn("Deleting disk on {}: ".format(parent_host)) - def disk_check(self, dirname, every_nth=8): """ Run the disk check protocol and publish @@ -638,12 +632,11 @@ def disk_check(self, dirname, every_nth=8): :param every_nth: :return: """ - rospy.loginfo("disk check on {} (every {}th)".format(dirname, every_nth)) + self.log.info("disk check on {} (every {}th)".format(dirname, every_nth)) if every_nth > 1: self.counter += 1 if self.counter % every_nth: return - rospy.loginfo("checking!!!!!!!") stats = os.statvfs(dirname) bytes_free = stats.f_frsize * stats.f_bavail diskmsg = MsgUInt64() @@ -669,18 +662,17 @@ def halt_if_unmounted(self): ) self.fail(txt) else: - rospy.loginfo("Mount OK: {}".format(sentinel)) + self.log.info("Mount OK: {}".format(sentinel)) def fail(self, txt): - rospy.logwarn(txt) - # if self.is_archiving: + self.log.warning(txt) self.rawmsg_publish_err(txt) self.envoy.put("/sys/arch/is_archiving", 0) def rawmsg_publish_err(self, txt): - msg = MsgString(txt) - rospy.logerr_throttle(1, txt) - self.rawmsg_pub = rospy.Publisher("/rawmsg", MsgString, queue_size=99) + msg = MsgString() + msg.data = txt + self.log.error(txt, throttle_duration_sec=1) self.rawmsg_pub.publish(msg) @property diff --git a/src/process/view_server/CMakeLists.txt b/src/process/view_server/CMakeLists.txt deleted file mode 100644 index 162bfdc3..00000000 --- a/src/process/view_server/CMakeLists.txt +++ /dev/null @@ -1,201 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(view_server) - -## 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 - std_msgs - custom_msgs - image_view -) - -## 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 exec_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 exec_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 exec_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 nexus -# CATKIN_DEPENDS roscpp 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}/nexus.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/nexus_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_nexus.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) diff --git a/src/process/view_server/__init__.py b/src/process/view_server/__init__.py deleted file mode 100644 index faa18be5..00000000 --- a/src/process/view_server/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- diff --git a/src/process/view_server/launch/image_view_server.launch b/src/process/view_server/launch/image_view_server.launch deleted file mode 100644 index 44fa603a..00000000 --- a/src/process/view_server/launch/image_view_server.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/process/view_server/launch/image_view_server.launch.xml b/src/process/view_server/launch/image_view_server.launch.xml new file mode 100644 index 00000000..090f1111 --- /dev/null +++ b/src/process/view_server/launch/image_view_server.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/src/process/view_server/launch/web_server.launch b/src/process/view_server/launch/web_server.launch deleted file mode 100644 index 69f720f8..00000000 --- a/src/process/view_server/launch/web_server.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - diff --git a/src/process/view_server/package.xml b/src/process/view_server/package.xml index 84afe8d7..64452969 100644 --- a/src/process/view_server/package.xml +++ b/src/process/view_server/package.xml @@ -1,44 +1,25 @@ - - + + + view_server - 0.0.0 - GUIs implemented with wxPython. + 1.0.0 + Image view server (synchronization + windowed image access) - - Adam Romlein - - - - - Apache 2.0 - - - - - - - - - - - - catkin - rospy - rospkg - sensor_msgs - std_msgs - custom_msgs - cv_bridge - image_view - genpy - python-numpy + rclpy + std_msgs + sensor_msgs + custom_msgs + cv_bridge + roskv + nexus + python3-numpy + python3-opencv - - - + + ament_python diff --git a/src/process/nexus/scripts/__init__.py b/src/process/view_server/resource/view_server similarity index 100% rename from src/process/nexus/scripts/__init__.py rename to src/process/view_server/resource/view_server diff --git a/src/process/view_server/setup.cfg b/src/process/view_server/setup.cfg new file mode 100644 index 00000000..996c6633 --- /dev/null +++ b/src/process/view_server/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/view_server +[install] +install_scripts=$base/lib/view_server diff --git a/src/process/view_server/setup.py b/src/process/view_server/setup.py index 8a658e40..d2dd0aa4 100755 --- a/src/process/view_server/setup.py +++ b/src/process/view_server/setup.py @@ -1,9 +1,28 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['view_server'], - package_dir={'': 'src'}) +from setuptools import setup -setup(**d) +package_name = "view_server" + +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Image view server (synchronization + windowed image access)", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "image_view_server = view_server.image_view_server:main", + ], + }, +) diff --git a/src/process/view_server/src/__init__.py b/src/process/view_server/src/__init__.py deleted file mode 100644 index faa18be5..00000000 --- a/src/process/view_server/src/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- diff --git a/src/process/view_server/src/view_server/image_view_server.py b/src/process/view_server/src/view_server/image_view_server.py index 6f5bddd3..9e50b8a9 100755 --- a/src/process/view_server/src/view_server/image_view_server.py +++ b/src/process/view_server/src/view_server/image_view_server.py @@ -2,23 +2,22 @@ from __future__ import division, print_function import os import socket -import traceback -from contextlib import contextmanager -import numpy as np import threading +import time from collections import deque -from six.moves.queue import Queue, deque -from six import BytesIO -from hashlib import md5 +from contextlib import contextmanager +import numpy as np import cv2 # ROS imports -import rospy -import rospkg +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import ReentrantCallbackGroup from sensor_msgs.msg import CompressedImage, Image -from cv_bridge import CvBridge, CvBridgeError -from roskv.util import hash_genpy_msg +from cv_bridge import CvBridge +from roskv.util import hash_ros_msg from roskv.rendezvous import ConditionalRendezvous # Kamera Imports @@ -35,7 +34,7 @@ InMemBridge, ) -from view_server.img_nexus import Nexus +from view_server.img_nexus import Nexus, stamp_to_sec MEM_TRANSPORT_DIR = os.environ.get("MEM_TRANSPORT_DIR", False) @@ -57,24 +56,6 @@ bridge = CvBridge() -rospack = rospkg.RosPack() - -import time -import threading -from contextlib import contextmanager - - -class NopContext(object): - - @property - def nop(self): - return self.nopContext() - - @contextmanager - def nopContext(self): - yield True - - class TimeoutLock(object): def __init__(self, default_timeout=None): self._lock = threading.RLock() @@ -119,6 +100,7 @@ class ImageViewServer(object): def __init__( self, + node, sync_image_topic, rgb_service_topic=None, rgb_metadata_service_topic=None, @@ -129,6 +111,8 @@ def __init__( rgb_queue=None, ): """ + :param node: rclpy node owning the ROS interfaces + :param sync_image_topic: Topic providing SynchronizedImages messages. :type sync_image_topic: str @@ -141,23 +125,23 @@ def __init__( :type service_topic: str """ + self.node = node + self.log = node.get_logger() self.image_lock = threading.RLock() self.rgb_msg = None self.ir_msg = None self.uv_msg = None + # services must be reentrant so image requests can block while + # subscription callbacks keep flowing on the executor + self.cb_group = ReentrantCallbackGroup() self.frame2newimg = {"rgb_msg": dict(), "ir_msg": dict(), "uv_msg": dict()} self.frame2hash = {"rgb_msg": dict(), "ir_msg": dict(), "uv_msg": dict()} if rgb_queue is None: raise ValueError("You must provide a rgb_queue parameter") - self.rgb_queue = rgb_queue # type: queue.dequeue + self.rgb_queue = rgb_queue - self.queue = { - "rgb_msg": Queue(1), - "ir_msg": Queue(1), - "uv_msg": Queue(1), - } self.img_stamp_blocks = { "rgb_msg": ConditionalRendezvous(1), "ir_msg": ConditionalRendezvous(1), @@ -174,63 +158,62 @@ def __init__( hostname_ns = "/" + socket.gethostname() - # rospy.loginfo('Subscribing to SynchronizedImages topic \'{}\''.format(sync_image_topic)) - # rospy.Subscriber(sync_image_topic, SyncedImageMsg, self.sync_images_callback, queue_size=1) - self.enabled = {"rgb": True, "uv": True, "ir": True} # todo: deal with channel enable config def subscribe_to_single_image(modality="rgb"): if self.enabled[modality]: - topic = rospy.get_param( - "_topic".format(modality), - hostname_ns + "/{}/image_raw".format(modality), - ) - - rospy.loginfo("Subscribing to Images topic '{}'".format(topic)) - rospy.Subscriber( - topic, + topic = hostname_ns + "/{}/image_raw".format(modality) + self.log.info("Subscribing to Images topic '{}'".format(topic)) + node.create_subscription( Image, - self.any_queue_callback, - callback_args=modality, - queue_size=1, + topic, + lambda msg, m=modality: self.any_queue_callback(msg, m), + 1, + callback_group=self.cb_group, ) def subscribe_to_image_service(service_topic, metadata_service_topic, key): if service_topic is not None: - rospy.loginfo( + self.log.info( "Creating RequestImageView service to provide " "'%s' image views on topic '%s'" % (key, service_topic) ) - rospy.Service( - service_topic, + node.create_service( RequestImageView, - lambda req: self.image_patch_service_request(req, key, False), + service_topic, + lambda req, resp: self.image_patch_service_request( + req, resp, key, False + ), + callback_group=self.cb_group, ) - if service_topic is not None: compressed_service_topic = "%s/compressed" % service_topic - rospy.loginfo( + self.log.info( "Creating RequestCompressedImageView service to " "provide '%s' image views on topic '%s'" % (key, compressed_service_topic) ) - rospy.Service( - compressed_service_topic, + node.create_service( RequestCompressedImageView, - lambda req: self.image_patch_service_request(req, key, True), + compressed_service_topic, + lambda req, resp: self.image_patch_service_request( + req, resp, key, True + ), + callback_group=self.cb_group, ) if metadata_service_topic is not None: - rospy.loginfo( + self.log.info( "Creating RequestImageMetadata service to " "provide '%s' image metadata via " "RequestImageMetadata on topic '%s'" % (key, metadata_service_topic) ) - rospy.Service( - metadata_service_topic, + node.create_service( RequestImageMetadata, - lambda req: self.metadata_service_topic_request(req, key), + metadata_service_topic, + lambda req, resp: self.metadata_service_topic_request(req, resp, key), + callback_group=self.cb_group, ) for modality in self.enabled: @@ -248,167 +231,80 @@ def subscribe_to_image_service(service_topic, metadata_service_topic, key): @property def nop_lock(self): return self.image_lock - # return self.nopContext() - - @contextmanager - def nopContext(self): - yield True def any_queue_callback(self, msg, modality="rgb"): modality = modality.lower() + "_msg" - rospy.loginfo("image callback {}".format(modality)) + self.log.info("image callback {}".format(modality)) for frame in self.frame2newimg[modality]: try: self.frame2newimg[modality][frame][0] = True - except: + except Exception: pass with self.nop_lock: setattr(self, modality, msg) - def sync_images_callback(self, msg): - rospy.loginfo("sync images callback") - with self.nop_lock: - try: - rgb_str = "RGB=%ix%i %s" % ( - msg.image_rgb.width, - msg.image_rgb.height, - msg.image_rgb.encoding, - ) - modality = "rgb_msg" - img_rendezvous = self.img_stamp_blocks[modality] - img_rendezvous.put(msg.header.stamp) - # queue = self.queue[modality] - # if queue.empty(): - # rospy.loginfo("Enqueued: {}".format(modality)) - # queue.put(True) - # else: - # rospy.loginfo("Full or something: {}".format(modality)) - except Exception as exc: - rospy.logerr("RGB fail: {}: {}".format(exc.__class__.__name__, exc)) - rgb_str = "No RGB" - - try: - ir_str = "IR=%ix%i %s" % ( - msg.image_ir.width, - msg.image_ir.height, - msg.image_ir.encoding, - ) - modality = "ir_msg" - - img_rendezvous = self.img_stamp_blocks[modality] - img_rendezvous.put(msg.header.stamp) - # queue = self.queue[modality] - # if queue.empty(): - # rospy.loginfo("Enqueued: {}".format(modality)) - # queue.put(True) - # else: - # rospy.loginfo("Full or something: {}".format(modality)) - except Exception as exc: - rospy.logerr("IR fail {}: {}".format(exc.__class__.__name__, exc)) - ir_str = "No IR" - - try: - uv_str = "UV=%ix%i %s" % ( - msg.image_uv.width, - msg.image_uv.height, - msg.image_uv.encoding, - ) - modality = "uv_msg" - - img_rendezvous = self.img_stamp_blocks[modality] - img_rendezvous.put(msg.header.stamp) - # queue = self.queue[modality] - # if queue.empty(): - # rospy.loginfo("Enqueued: {}".format(modality)) - # queue.put(True) - # else: - # rospy.loginfo("Full or something: {}".format(modality)) - except Exception as exc: - rospy.logerr("UV fail {}: {}".format(exc.__class__.__name__, exc)) - uv_str = "No UV" - - rospy.loginfo( - "Received SynchronizedImages message with [%s] [%s] " - "[%s]" % (rgb_str, ir_str, uv_str) - ) - self.rgb_msg = msg.image_rgb - self.ir_msg = msg.image_ir - self.uv_msg = msg.image_uv - - def image_patch_service_request(self, req, modality, compress): + def image_patch_service_request(self, req, resp, modality, compress): """ - see custom_msgs/srv/RequestImagePatches.srv for more details. + see custom_msgs/srv/RequestImageView.srv for more details. :param modality: Which image stream from which to return an image view. :type modality: str {'RGB','IR','UV'} """ tic = time.time() - req_hash = hash_genpy_msg(req) - # rospy.loginfo('Requesting {} \'{}\' image view of size {} x {}: {}'.format( - # 'compressed' if compress else '', modality, - # req.output_width, req.output_height, req_hash)) + req_hash = hash_ros_msg(req) with self.nop_lock: - # rospy.logwarn('pre lock') if modality == "rgb_msg": try: img_msg = self.rgb_queue[0] except IndexError as exc: - rospy.logwarn("{}: {}".format(exc.__class__.__name__, exc)) + self.log.warning("{}: {}".format(exc.__class__.__name__, exc)) img_msg = None else: img_msg = getattr(self, modality) - # Remove cache - # setattr(self, modality, None) - # rospy.logwarn('post lock') if img_msg is None: - # rospy.logerr('exit early due to lack of encoding') - return False, Image() + resp.success = False + return resp try: stale_hash = req_hash == self.frame2hash[modality][req.frame][0] - except: + except Exception: stale_hash = False try: newimg = self.frame2newimg[modality][req.frame][0] - except: + except Exception: newimg = True if newimg or not stale_hash: try: image = membridge.imgmsg_to_cv2(img_msg, "passthrough") except Exception as exc: - rospy.logerr("{}: {}".format(exc.__class__.__name__, exc)) - return False, Image() + self.log.error("{}: {}".format(exc.__class__.__name__, exc)) + resp.success = False + return resp else: - return True, Image() + resp.success = True + return resp try: self.frame2newimg[modality][req.frame][0] = False - except: + except Exception: self.frame2newimg[modality][req.frame] = deque([False], maxlen=1) try: self.frame2hash[modality][req.frame][0] = req_hash - except: + except Exception: self.frame2hash[modality][req.frame] = deque([req_hash], maxlen=1) - # rospy.logwarn(inspect.currentframe().f_lineno) flags = get_interpolation(req.interpolation) dsize = (req.output_width, req.output_height) if modality == "ir_msg": if req.apply_clahe: - bright_px = 20 - h, w = image.shape - pxs = h * w - #top_percentile = ((pxs - bright_px) / pxs) * 100 - top_percentile = 100 - stretch_percentiles = [1, top_percentile] + stretch_percentiles = [1, 100] img = image.astype("uint16") mi = np.percentile(img, stretch_percentiles[0]) ma = np.percentile(img, stretch_percentiles[1]) normalized = (img - mi) / (ma - mi) - #normalized = np.clip(normalized, 0, 1) normalized = normalized * 255 normalized[normalized < 0] = 0 image = np.round(normalized).astype("uint8") @@ -416,109 +312,85 @@ def image_patch_service_request(self, req, modality, compress): homography = np.reshape(req.homography, (3, 3)).astype(np.float32) raw_image = cv2.warpPerspective(image, homography, dsize=dsize, flags=flags) - if modality == "ir_msg": - # Don't need mono16 for display - # raw_image = np.round(raw_image/256).astype('uint8') + if modality in ("ir_msg", "uv_msg"): image2 = cv2.cvtColor(raw_image, cv2.COLOR_GRAY2RGB) - if req.show_saturated_pixels: - maxval = 255 - saturation_mask = np.all(image2 == maxval, -1) - image2[:, :, 1][saturation_mask] = 0 - image2[:, :, 2][saturation_mask] = 0 - elif modality == "uv_msg": - image2 = cv2.cvtColor(raw_image, cv2.COLOR_GRAY2RGB) - if req.show_saturated_pixels: - maxval = 255 - saturation_mask = np.all(image2 == maxval, -1) - image2[:, :, 1][saturation_mask] = 0 - image2[:, :, 2][saturation_mask] = 0 - elif modality == "rgb_msg": - image2 = raw_image - if req.show_saturated_pixels: - maxval = 255 - saturation_mask = np.all(image2 == maxval, -1) - image2[:, :, 1][saturation_mask] = 0 - image2[:, :, 2][saturation_mask] = 0 else: image2 = raw_image + if req.show_saturated_pixels and image2.ndim == 3: + maxval = 255 + saturation_mask = np.all(image2 == maxval, -1) + image2[:, :, 1][saturation_mask] = 0 + image2[:, :, 2][saturation_mask] = 0 - # rospy.logwarn('post warp') if compress: - # raise NotImplementedError('disabled for now') out_msg = CompressedImage() out_msg.format = "jpeg" - out_msg.data = np.array(cv2.imencode(".jpg", image2)[1]).tostring() + out_msg.data = np.array(cv2.imencode(".jpg", image2)[1]).tobytes() out_msg.header = img_msg.header - # rospy.logwarn("Compressed") else: out_msg = bridge.cv2_to_imgmsg(image2, encoding="rgb8") out_msg.header = img_msg.header - # rospy.logwarn("UnCompressed") - # Cache the request hash so we can block the next time around toc = time.time() - rospy.loginfo( + self.log.info( "{:.2f} Releasing {: >3}".format( - img_msg.header.stamp.to_sec(), modality[:3] + stamp_to_sec(img_msg.header.stamp), modality[:3] ) ) print("Time to process request was %0.3fs" % (toc - tic)) - return True, out_msg + resp.success = True + resp.image = out_msg + return resp - def metadata_service_topic_request(self, req, modality): + def metadata_service_topic_request(self, req, resp, modality): """ - see custom_msgs/srv/RequestImagePatches.srv for more details. + see custom_msgs/srv/RequestImageMetadata.srv for more details. :param modality: Which image stream from which to return an image view. :type modality: str {'RGB','IR','UV'} """ - # We want to return the next image received. - rospy.logerr_throttle(1.0, "request: {} modality: {}".format(req, modality)) + self.log.error( + "request: {} modality: {}".format(req, modality), + throttle_duration_sec=1.0, + ) with self.nop_lock: img_msg0 = getattr(self, modality) if img_msg0 is None: - msg = (False, 0, 0, "") + resp.success = False + resp.height = 0 + resp.width = 0 + resp.encoding = "" else: - msg = (True, img_msg0.height, img_msg0.width, img_msg0.encoding) - # print('metadata: {}'.format(msg)) + resp.success = True + resp.height = img_msg0.height + resp.width = img_msg0.width + resp.encoding = img_msg0.encoding # invalidate cache lanes img_rendezvous = self.img_stamp_blocks[modality] if req.release: img_rendezvous.release() - return msg - + return resp -def set_up_nexus(rgb_queue): - import socket - # node = 'img_nexus' - # rospy.init_node(node) - print("Parent", rospy.get_namespace()) - node_name = rospy.get_name() - # for param in rospy.get_param_names(): - # print(param) - # /nuvo1/img_nexus/ir_topic - """ - root@nuvo0:~/kamera_ws# rosparam get /nuvo0/img_nexus -{ir_topic: ir/image_raw, max_wait: 0.9, out_topic: synched, rgb_topic: rgb/image_raw, - uv_topic: uv/image_raw, verbosity: 9}""" +def set_up_nexus(node, rgb_queue): hostname_ns = "/" + socket.gethostname() - sys_prefix = hostname_ns + "/img_nexus" - verbosity = rospy.get_param("verbosity", 9) - rgb_topic = rospy.get_param("rgb_topic", hostname_ns + "/rgb/image_raw") - ir_topic = rospy.get_param("ir_topic", hostname_ns + "/ir/image_raw") - uv_topic = rospy.get_param("uv_topic", hostname_ns + "/uv/image_raw") - # out_topic = rospy.get_param('out_topic', sys_prefix + 'synced') + verbosity = node.declare_parameter("verbosity", 9).value + rgb_topic = node.declare_parameter( + "rgb_topic", hostname_ns + "/rgb/image_raw" + ).value + ir_topic = node.declare_parameter("ir_topic", hostname_ns + "/ir/image_raw").value + uv_topic = node.declare_parameter("uv_topic", hostname_ns + "/uv/image_raw").value out_topic = hostname_ns + "/synched" - max_wait = rospy.get_param("/max_frame_period", 444) / 1000.0 - send_image_data = rospy.get_param("~send_image_data") - compress_imagery = rospy.get_param("~compress_imagery") + max_wait = node.declare_parameter("max_frame_period", 444.0).value / 1000.0 + send_image_data = node.declare_parameter("send_image_data", True).value + compress_imagery = node.declare_parameter("compress_imagery", True).value nexus = Nexus( + node, rgb_topic, ir_topic, uv_topic, @@ -532,60 +404,43 @@ def set_up_nexus(rgb_queue): return nexus -def rospy_spin(delay=1.0): - """ - Blocks until ROS node is shutdown. Yields activity to other threads. - @raise ROSInitException: if node is not in a properly initialized state - """ - - if not rospy.core.is_initialized(): - raise rospy.exceptions.ROSInitException( - "client code must call rospy.init_node() first" - ) - rospy.logdebug( - "node[%s, %s] entering spin(), pid[%s]", - rospy.core.get_caller_id(), - rospy.core.get_node_uri(), - os.getpid(), - ) - try: - while not rospy.core.is_shutdown(): - rospy.rostime.wallsleep(delay) - rospy.loginfo("spin") - # print('.', end='') - except KeyboardInterrupt: - rospy.logdebug("keyboard interrupt, shutting down") - rospy.core.signal_shutdown("keyboard interrupt") - - -def main(): - # Launch the node. - node = "image_view_server" - rospy.init_node(node, anonymous=False) - node_name = rospy.get_name() +def main(args=None): + rclpy.init(args=args) + node = Node("image_view_server") # -------------------------- Read Parameters ----------------------------- - sync_image_topic = rospy.get_param("%s/sync_image_topic" % node_name) - rgb_service_topic = rospy.get_param("%s/rgb_service_topic" % node_name) - ir_service_topic = rospy.get_param("%s/ir_service_topic" % node_name) - uv_service_topic = rospy.get_param("%s/uv_service_topic" % node_name) - - rgb_metadata_service_topic = rospy.get_param( - "%s/rgb_metadata_service_topic" % node_name - ) - ir_metadata_service_topic = rospy.get_param( - "%s/ir_metadata_service_topic" % node_name - ) - uv_metadata_service_topic = rospy.get_param( - "%s/uv_metadata_service_topic" % node_name - ) + hostname_ns = "/" + socket.gethostname() + sync_topic_default = hostname_ns + "/synched" + sync_image_topic = node.declare_parameter( + "sync_image_topic", sync_topic_default + ).value + rgb_service_topic = node.declare_parameter( + "rgb_service_topic", sync_topic_default + "/rgb_view_service" + ).value + ir_service_topic = node.declare_parameter( + "ir_service_topic", sync_topic_default + "/ir_view_service" + ).value + uv_service_topic = node.declare_parameter( + "uv_service_topic", sync_topic_default + "/uv_view_service" + ).value + + rgb_metadata_service_topic = node.declare_parameter( + "rgb_metadata_service_topic", sync_topic_default + "/rgb_metadata_service" + ).value + ir_metadata_service_topic = node.declare_parameter( + "ir_metadata_service_topic", sync_topic_default + "/ir_metadata_service" + ).value + uv_metadata_service_topic = node.declare_parameter( + "uv_metadata_service_topic", sync_topic_default + "/uv_metadata_service" + ).value # ------------------------------------------------------------------------ # Share debayered RGB images between nexus and image view rgb_queue = deque(maxlen=1) - nexus = set_up_nexus(rgb_queue) + set_up_nexus(node, rgb_queue) ImageViewServer( + node, sync_image_topic, rgb_service_topic, rgb_metadata_service_topic, @@ -596,12 +451,16 @@ def main(): rgb_queue=rgb_queue, ) - rospy_spin() + executor = MultiThreadedExecutor(num_threads=8) + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - print("Interrupt") - pass + main() diff --git a/src/process/view_server/src/view_server/image_view_server.py.bak b/src/process/view_server/src/view_server/image_view_server.py.bak deleted file mode 100755 index 64cf8c74..00000000 --- a/src/process/view_server/src/view_server/image_view_server.py.bak +++ /dev/null @@ -1,254 +0,0 @@ -#! /usr/bin/python -from __future__ import division, print_function -import os -import numpy as np -import threading -from collections import deque -import cv2 - -# ROS imports -import rospy -import rospkg -from sensor_msgs.msg import CompressedImage, Image -from cv_bridge import CvBridge, CvBridgeError - -# Kamera Imports -from custom_msgs.msg import SynchronizedImages, SyncedPathImages -from custom_msgs.srv import RequestImageMetadata, RequestCompressedImageView, \ - RequestImageView -from nexus.pathimg_bridge import PathImgBridge, ExtendedBridge, coerce_message - -MEM_TRANSPORT_DIR = os.environ.get('MEM_TRANSPORT_DIR', False) -if MEM_TRANSPORT_DIR: - membridge = PathImgBridge() - membridge.dirname = MEM_TRANSPORT_DIR - SyncedImageMsg = SyncedPathImages -else: - membridge = ExtendedBridge() - SyncedImageMsg = SynchronizedImages - -bridge = CvBridge() - - -rospack = rospkg.RosPack() - - -def get_interpolation(interpolation): - if interpolation == 4: - flags = cv2.INTER_LANCZOS4 | cv2.WARP_INVERSE_MAP - elif interpolation == 3: - flags = cv2.INTER_CUBIC | cv2.WARP_INVERSE_MAP - elif interpolation == 2: - flags = cv2.INTER_AREA | cv2.WARP_INVERSE_MAP - elif interpolation == 1: - flags = cv2.INTER_LINEAR | cv2.WARP_INVERSE_MAP - else: - flags = cv2.INTER_NEAREST | cv2.WARP_INVERSE_MAP - return flags - - -class ImageViewServer(object): - """Provides windowed or reduced-resolution image access over network. - - When a request for imagery is made, it is not returned until a new image is - received by this node. - - """ - def __init__(self, sync_image_topic, rgb_service_topic=None, - rgb_metadata_service_topic=None, ir_service_topic=None, - ir_metadata_service_topic=None, uv_service_topic=None, - uv_metadata_service_topic=None): - """ - :param sync_image_topic: Topic providing SynchronizedImages messages. - :type sync_image_topic: str - - :param _service_topic: The service topic providing access to windowed or - reduced-resolution imagery. - :type service_topic: str - - :param _metadata_service_topic: The service topic providing metadata for - the raw imagery stored on this server. - :type service_topic: str - - """ - self.image_lock = threading.RLock() - self.rgb_msg = None - self.ir_msg = None - self.uv_msg = None - - - if MEM_TRANSPORT_DIR: - sync_image_topic += '_shm' - - rospy.loginfo('Subscribing to SynchronizedImages topic \'%s\'' - % sync_image_topic) - rospy.Subscriber(sync_image_topic, SyncedImageMsg, - self.sync_images_callback, queue_size=1) - - def subscribe_to_image_service(service_topic, metadata_service_topic, - key): - if service_topic is not None: - rospy.loginfo('Creating RequestImageView service to provide ' - '\'%s\' image views on topic \'%s\'' % - (key,service_topic)) - rospy.Service(service_topic, RequestImageView, - lambda req: self.image_patch_service_request(req, - key, - False)) - - if service_topic is not None: - compressed_service_topic = '%s/compressed' % service_topic - rospy.loginfo('Creating RequestCompressedImageView service to ' - 'provide \'%s\' image views on topic \'%s\'' % - (key,compressed_service_topic)) - rospy.Service(compressed_service_topic, - RequestCompressedImageView, - lambda req: self.image_patch_service_request(req, - key, - True)) - - if metadata_service_topic is not None: - rospy.loginfo('Creating RequestImageMetadata service to ' - 'provide \'%s\' image metadata via ' - 'RequestImageMetadata on topic \'%s\'' % - (key,metadata_service_topic)) - rospy.Service(metadata_service_topic, RequestImageMetadata, - lambda req: self.metadata_service_topic_request(req, - key)) - - subscribe_to_image_service(rgb_service_topic, - rgb_metadata_service_topic, - 'rgb_msg') - - subscribe_to_image_service(ir_service_topic, - ir_metadata_service_topic, - 'ir_msg') - - subscribe_to_image_service(uv_service_topic, - uv_metadata_service_topic, - 'uv_msg') - - - def sync_images_callback(self, msg): - with self.image_lock: - try: - rgb_str = ('RGB=%ix%i %s' % (msg.image_rgb.width, - msg.image_rgb.height, - msg.image_rgb.encoding)) - except: - rgb_str = 'No RGB' - - try: - ir_str = ('IR=%ix%i %s' % (msg.image_ir.width, - msg.image_ir.height, - msg.image_ir.encoding)) - except: - ir_str = 'No IR' - - try: - uv_str = ('UV=%ix%i %s' % (msg.image_uv.width, - msg.image_uv.height, - msg.image_uv.encoding)) - except: - uv_str = 'No UV' - - rospy.loginfo('Received SynchronizedImages message with [%s] [%s] ' - '[%s]' % (rgb_str,ir_str,uv_str)) - self.rgb_msg = msg.image_rgb - self.ir_msg = msg.image_ir - self.uv_msg = msg.image_uv - - def image_patch_service_request(self, req, modality, compress): - """ - see custom_msgs/srv/RequestImagePatches.srv for more details. - - :param modality: Which image stream from which to return an image view. - :type modality: str {'RGB','IR','UV'} - - """ - rospy.loginfo('Requesting %s \'%s\' image view of size %i x %i' % - ('compressed' if compress else '',modality, - req.output_width,req.output_height)) - - # We want to return the next image received. - with self.image_lock: - img_msg0 = getattr(self, modality) - - # Get the image. - while True: - with self.image_lock: - img_msg = getattr(self, modality) - if img_msg != img_msg0: - break - - if img_msg.encoding == '': - return (False,Image(),CompressedImage()) - - image = membridge.imgmsg_to_cv2(img_msg, 'passthrough') - flags = get_interpolation(req.interpolation) - dsize = (req.output_width, req.output_height) - - homography = np.reshape(req.homography, (3,3)).astype(np.float32) - image2 = cv2.warpPerspective(image, homography, dsize=dsize, - flags=flags) - - if compress: - raise NotImplementedError('disabled for now') - compressed_msg = CompressedImage() - compressed_msg.format = "jpeg" - compressed_msg.data = np.array(cv2.imencode('.jpg', - image2)[1]).tostring() - compressed_msg.header = img_msg.header - return (True,compressed_msg) - else: - new_image_msg = bridge.cv2_to_imgmsg(image2, encoding="passthrough") - new_image_msg.header = img_msg.header - return (True,new_image_msg) - - def metadata_service_topic_request(self, req, modality): - """ - see custom_msgs/srv/RequestImagePatches.srv for more details. - - :param modality: Which image stream from which to return an image view. - :type modality: str {'RGB','IR','UV'} - - """ - # We want to return the next image received. - with self.image_lock: - img_msg0 = getattr(self, modality) - - if img_msg0 is None: - return (False,0,0,'') - - return (True,img_msg0.height,img_msg0.width,img_msg0.encoding) - - -def main(): - # Launch the node. - node = 'image_view_server' - rospy.init_node(node, anonymous=False) - node_name = rospy.get_name() - - # -------------------------- Read Parameters ----------------------------- - sync_image_topic = rospy.get_param('%s/sync_image_topic' % node_name) - rgb_service_topic = rospy.get_param('%s/rgb_service_topic' % node_name) - ir_service_topic = rospy.get_param('%s/ir_service_topic' % node_name) - uv_service_topic = rospy.get_param('%s/uv_service_topic' % node_name) - - rgb_metadata_service_topic = rospy.get_param('%s/rgb_metadata_service_topic' % node_name) - ir_metadata_service_topic = rospy.get_param('%s/ir_metadata_service_topic' % node_name) - uv_metadata_service_topic = rospy.get_param('%s/uv_metadata_service_topic' % node_name) - # ------------------------------------------------------------------------ - - ImageViewServer(sync_image_topic, rgb_service_topic, - rgb_metadata_service_topic, ir_service_topic, - ir_metadata_service_topic, uv_service_topic, - uv_metadata_service_topic) - - rospy.spin() - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/process/view_server/src/view_server/img_nexus.py b/src/process/view_server/src/view_server/img_nexus.py index 425a534b..791bcf55 100755 --- a/src/process/view_server/src/view_server/img_nexus.py +++ b/src/process/view_server/src/view_server/img_nexus.py @@ -5,28 +5,26 @@ import os import sys import json +import socket from typing import List, Tuple, Optional from datetime import datetime import threading -from functools import partial import time +import urllib.parse as urllib_parse from roskv.impl.redis_envoy import RedisEnvoy -from six.moves import urllib_parse -from six.moves.queue import deque import numpy as np import cv2 # ROS imports -import rospy from cv_bridge import CvBridge from std_msgs.msg import Bool as MsgBool, Float64 as MsgFloat64, String as MsgString from std_msgs.msg import Header from sensor_msgs.msg import Image as MsgImage from custom_msgs.msg import ( SynchronizedImages, - GSOF_EVT, - GSOF_INS, + GsofEvt, + GsofIns, SyncedPathImages, Stat, ) @@ -42,7 +40,6 @@ coerce_message, InMemBridge, ) -from libnmea_navsat_driver.gsof import GsofSpoofEventDispatch G_DO_DEBAYER = True @@ -74,45 +71,52 @@ bayer_patterns["bayer_bggr16"] = cv2.COLOR_BayerRG2RGB bayer_patterns["bayer_gbrg16"] = cv2.COLOR_BayerGR2RGB -ros_immediate = rospy.Duration(nsecs=1) +log = None # module logger, set by Nexus + + +def stamp_to_sec(stamp): + return stamp.sec + stamp.nanosec * 1e-9 + + +def stamp_key(stamp): + """Hashable epoch key from a builtin_interfaces Time (ROS2 messages are unhashable).""" + return (stamp.sec, stamp.nanosec) def rostime_to_datetime(stamp): - # type: (rospy.Time) -> datetime - t = stamp.to_sec() - return datetime.utcfromtimestamp(t) + return datetime.utcfromtimestamp(stamp_to_sec(stamp)) -def check_image_msg(msg, mode=""): - # type: (MsgImage, Optional[str]) -> Optional[np.ndarray] +def check_image_msg(msg, mode="", logger=None): + # type: (MsgImage, Optional[str], object) -> Optional[np.ndarray] """ Checks validity (presence, size, encoding) of image before sending off :param msg: Image message object :param mode: [optional] Type of message - used for logging :return: Decoded image (this step is pretty fast) or None on failure """ + logger = logger or log if not msg: - rospy.logerr("Message {} is None".format(mode)) + logger.error("Message {} is None".format(mode)) return None if not msg.encoding: - rospy.logerr("Message {} is missing encoding".format(mode)) + logger.error("Message {} is missing encoding".format(mode)) return None data = bridge.imgmsg_to_cv2(msg) # type: np.ndarray - rospy.logdebug(data.shape, data.size) if not data.shape: - rospy.logerr("Message {} has no shape".format(mode)) + logger.error("Message {} has no shape".format(mode)) return None if not data.size: - rospy.logerr("Message {} has no size".format(mode)) + logger.error("Message {} has no size".format(mode)) return None if data.ndim not in (2, 3): - rospy.logerr("Message {} has incorrect ndim: {}".format(mode, data.ndim)) + logger.error("Message {} has incorrect ndim: {}".format(mode, data.ndim)) return None if not (np.prod(data.shape)): - rospy.logerr("Message {} has null shape: {}".format(mode, data.shape)) + logger.error("Message {} has null shape: {}".format(mode, data.shape)) return None return data @@ -127,7 +131,7 @@ def dump_image_array(filename, data, verbosity=0): cv2.imwrite(filename, data, (cv2.IMWRITE_JPEG_QUALITY, 100)) end = time.time() if verbosity >= 2: - rospy.loginfo("Image Writer saved: {} in {:.3f}s".format(filename, end - start)) + print("Image Writer saved: {} in {:.3f}s".format(filename, end - start)) def dump_image_msg(filename, msg, mode="", verbosity=0): @@ -144,7 +148,7 @@ def dump_image_msg(filename, msg, mode="", verbosity=0): # in the rare event of the filename being a dupe, just tag it as such if os.path.exists(filename): - rospy.logerr("OOPS! Duplicate: {}".format(filename)) + print("OOPS! Duplicate: {}".format(filename)) fn, ext = os.path.splitext(filename) filename = fn + "_dupe" + ext @@ -154,7 +158,7 @@ def dump_image_msg(filename, msg, mode="", verbosity=0): if verbosity >= 4: print("{} {} {:.3f} sec".format(msg.encoding, data.shape, end - start)) if verbosity >= 2: - rospy.loginfo("Image Writer saved: {}".format(filename)) + print("Image Writer saved: {}".format(filename)) def debayer_image_msg(msg, do_debayer=G_DO_DEBAYER): @@ -167,33 +171,18 @@ def debayer_image_msg(msg, do_debayer=G_DO_DEBAYER): """ if not do_debayer: return msg - tic = time.time() if msg.encoding in bayer_patterns.keys(): - rospy.logdebug("DeBayering from encoding {}".format(msg.encoding)) image = bridge.imgmsg_to_cv2(msg) - - # image = self.gamma_to_linear_lut[image] image = cv2.cvtColor(image, bayer_patterns[msg.encoding]) - # image = self.linear_to_gamma_lut[image] - - # White balance - """ - RGB_rescale = [0.59987517, 1, 0.96323181] - for i in range(3): - lut = np.round(np.arange(256)*RGB_rescale[i]).astype(np.uint8) - image[:,:,i] = cv2.LUT(image[:,:,i], lut) - """ - debayered_msg = bridge.cv2_to_imgmsg(image, encoding="rgb8") - debayered_msg.header.stamp = msg.header.stamp debayered_msg.header.frame_id = msg.header.frame_id - rospy.logdebug("Debayer Time elapsed: {:.3f} s".format(time.time() - tic)) elif msg.encoding == "rgb8": # message is already decoded, just return return msg else: - rospy.logwarn("Unrecognized Bayer encoding `{}`".format(msg.encoding)) + if log is not None: + log.warning("Unrecognized Bayer encoding `{}`".format(msg.encoding)) return msg return debayered_msg @@ -234,6 +223,7 @@ class Nexus(object): def __init__( self, + node, rgb_topic, ir_topic, uv_topic, @@ -245,6 +235,8 @@ def __init__( verbosity=0, ): """ + :param node: rclpy node owning the ROS interfaces + :param rgb_topic: Topic to receive RGB ROS Image messages on. :type rgb_topic: str @@ -266,8 +258,12 @@ def __init__( :type max_wait: float """ + global log + self.node = node + self.log = node.get_logger() + log = self.log redis_host = os.environ.get("REDIS_HOST", "nuvo0") - node_host = rospy.get_namespace().strip("/") + node_host = os.environ.get("NODE_HOSTNAME") or socket.gethostname() self.envoy = RedisEnvoy(redis_host, client_name=node_host + "_img_nexus") cam_fov = self.envoy.get( os.path.join("/sys", "arch", "hosts", node_host, "fov") @@ -283,14 +279,14 @@ def __init__( if rgb_queue is None: raise ValueError("You must provide a rgb_queue parameter") - self.rgb_queue = rgb_queue # type: dequeue + self.rgb_queue = rgb_queue self.image_formats = {} for chan in ["rgb", "uv", "ir", "evt", "ins"]: self.image_formats[chan] = self.envoy.get("/sys/arch/ext_%s" % chan) max_wait = 1.0 / max_frame_rate - rospy.loginfo( + self.log.info( "node host: {} fov: {} max_wait: {:.3f}".format( node_host, cam_fov, max_wait ) @@ -298,29 +294,24 @@ def __init__( self.node_host = node_host self.cam_fov = cam_fov - self.node_name = rospy.get_name() + self.node_name = node.get_name() self.image_lock = threading.RLock() self.pub_timer = None - self._current_epoch = rospy.Time.now() + self._current_epoch = stamp_key(node.get_clock().now().to_msg()) self.epoch_dict = dict() self._msg_dict = dict() self._recent_epochs = [] self.max_wait = max_wait self.rolling_success = LowpassIIR() self.topics = { - "rgb_topic": rospy.resolve_name(rgb_topic), - "ir_topic": rospy.resolve_name(ir_topic), - "uv_topic": rospy.resolve_name(uv_topic), - "out_topic": rospy.resolve_name(out_topic), + "rgb_topic": rgb_topic, + "ir_topic": ir_topic, + "uv_topic": uv_topic, + "out_topic": out_topic, } topic_base = f"/sys/enabled/{cam_fov}" self.enabled = self.envoy.get(topic_base) - # self.enabled = { - # 'rgb': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'rgb'), True), - # 'ir': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'ir'), True), - # 'uv': rospy.get_param(os.path.join('/cfg/enabled', cam_fov, 'uv'), True) - # } self.enabled_list = [k for k, v in self.enabled.items() if v] self.full_packet_list = self.enabled_list + ["evt"] self.skip_ir = not self.enabled["ir"] @@ -328,56 +319,38 @@ def __init__( self._is_archiving = False self.verbosity = verbosity self._pub_ir_leveled = True # Outputs a stream of z-normalized IR - self.archiver = ArchiveManager(agent_name="nexus", verbosity=verbosity) - self.archiver.advertise_services() + self.archiver = ArchiveManager(node, agent_name="nexus", verbosity=verbosity) + self.archiver.advertise_services(namespace=node_host) self.stats_logger = SimpleStatsLogger(archiver=self.archiver) self.pub_missed = {} # publish when a frame is missed self.image_writers = {} - if self.enabled["rgb"]: - rospy.loginfo("Subscribing to Images topic '%s'" % rgb_topic) - rospy.Subscriber( - rgb_topic, - MsgImage, - self.any_queue_callback, - callback_args="rgb", - queue_size=1, - ) - self.pub_missed["rgb"] = rospy.Publisher("rgb/missed", Header, queue_size=5) - - if self.enabled["ir"]: - rospy.loginfo("Subscribing to Images topic '%s'" % ir_topic) - rospy.Subscriber( - ir_topic, + for mode, topic in (("rgb", rgb_topic), ("ir", ir_topic), ("uv", uv_topic)): + if not self.enabled[mode]: + continue + self.log.info("Subscribing to Images topic '%s'" % topic) + node.create_subscription( MsgImage, - self.any_queue_callback, - callback_args="ir", - queue_size=1, + topic, + lambda msg, m=mode: self.any_queue_callback(msg, m), + 1, ) - self.pub_missed["ir"] = rospy.Publisher("ir/missed", Header, queue_size=5) - - if self.enabled["uv"]: - rospy.loginfo("Subscribing to Images topic '%s'" % uv_topic) - rospy.Subscriber( - uv_topic, - MsgImage, - self.any_queue_callback, - callback_args="uv", - queue_size=1, + self.pub_missed[mode] = node.create_publisher( + Header, "%s/missed" % mode, 5 ) - self.pub_missed["uv"] = rospy.Publisher("uv/missed", Header, queue_size=5) - rospy.Subscriber( - "/event", GSOF_EVT, self.any_queue_callback, callback_args="evt" + node.create_subscription( + GsofEvt, "/event", lambda msg: self.any_queue_callback(msg, "evt"), 10 ) - self.publisher = rospy.Publisher(out_topic, SyncedImageMsg, queue_size=1) + self.publisher = node.create_publisher(SyncedImageMsg, out_topic, 1) - self.pub_status = rospy.Publisher("status", MsgString, queue_size=3) + self.pub_status = node.create_publisher(MsgString, "status", 3) - self.stat_pub = rospy.Publisher("/stat", Stat, queue_size=3) - self.pstat_pub = rospy.Publisher(self.node_name + "/stat", Stat, queue_size=3) - self.stat_counter = 0 + self.stat_pub = node.create_publisher(Stat, "/stat", 3) + self.pstat_pub = node.create_publisher( + Stat, self.node_name + "/stat", 3 + ) self.compress_imagery = compress_imagery self.send_image_data = send_image_data @@ -386,9 +359,8 @@ def msg_dict(self): """Get the most recent message dict""" return self.epoch_dict.get(self._current_epoch, {}) - def get_spoof_event(self): - rospy.logwarn("No event msg detected, generating spoof event") - return GsofSpoofEventDispatch() + def now_msg(self): + return self.node.get_clock().now().to_msg() def is_msg_dict_full(self): """Check if all requisite messages have been received (regardless of @@ -402,63 +374,25 @@ def is_msg_dict_full(self): def reset_timer(self): if self.pub_timer is not None: - self.pub_timer.shutdown() + self.pub_timer.cancel() + self.node.destroy_timer(self.pub_timer) self.pub_timer = None - def timer_writer_callback(self, timer_event=None, msg=None, mode=""): - if not msg: - raise RuntimeError("No message in timer callback, this should not happen") - if not mode: - raise RuntimeError("No mode in timer callback, this should not happen") - raise NotImplementedError("timer_writer_callback is disabled!") - - self.image_writer_callback(msg=msg, args=mode) - - def image_writer_callback(self, msg, args): - # type: (MsgImage, str) -> None - rospy.logwarn("Archiving from nexus DEPRECATED") - raise NotImplementedError("image_writer_callback is disabled!") - return - mode = args - ext = self.image_formats[mode] - - data = check_image_msg(msg, mode) - # We want to emit missed frame messages iff message is bad and we are archiving - if data is None and self.archiver.is_archiving: - self.pub_missed[mode].publish(msg.header or Header()) - - if not self.archiver.is_archiving: - return - now = datetime.utcfromtimestamp(msg.header.stamp.to_sec()) - template = self.archiver.fmt_sync_path(now) - filename = template.format(mode=mode, ext=ext) - dirname = make_path(filename, from_file=True) - try: - dump_image_msg(filename, msg, mode, verbosity=self.verbosity) - - except ImageEncodingMissingError: - pass # we logged this with check_image - except Exception: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("dump_image_msg failed: {}: {}".format(exc_type, value)) - def end_of_turn(self, stale_time=1.5): """ Finalize and publish completed packets :param stale_time: - :param timeout_time: :return: """ - stale_time = rospy.Duration.from_sec(stale_time) - now = rospy.Time.now() + now = time.time() completed = [] stale = [] with self.image_lock: for ep in self.epoch_dict: msg_dict = self.epoch_dict.get(ep) - age = now - ep + age = now - (ep[0] + ep[1] * 1e-9) if all(key in msg_dict for key in self.full_packet_list): - rospy.loginfo( + self.log.info( "[_] Comp {: >4}: {} {}".format( msg_dict["evt"].event_num, ep, msg_dict.keys() ) @@ -466,7 +400,7 @@ def end_of_turn(self, stale_time=1.5): completed.append(ep) elif age > stale_time: - rospy.logerr( + self.log.error( "[_] Messages timed out, epoch {}: {}".format( ep, msg_dict.keys() ) @@ -474,11 +408,9 @@ def end_of_turn(self, stale_time=1.5): stale.append(ep) else: pass - # rospy.loginfo("[_] Partial : {}".format(ep)) for candidate in completed + stale: msg_dict = self.epoch_dict.pop(candidate) - # rospy.loginfo("Publishing {}".format(candidate)) self._publish(msg_dict=msg_dict) self._recent_epochs = self._recent_epochs[-20:] @@ -487,9 +419,12 @@ def any_queue_callback(self, msg, modality="evt"): modality = modality.lower() urlp = urllib_parse.urlparse(msg.header.frame_id) qs = urllib_parse.parse_qs(urlp.query) - rospy.loginfo( + self.log.info( "<^>{:>3} {:>6}: {:.6f} {}".format( - modality, qs.get("eventNum", ["?"])[0], msg.header.stamp.to_sec(), qs + modality, + qs.get("eventNum", ["?"])[0], + stamp_to_sec(msg.header.stamp), + qs, ) ) @@ -503,30 +438,26 @@ def event_queue_callback(self, event_msg, modality="evt"): stat = Stat() stat.trace_header = event_msg.header stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 + stat.header.stamp = self.now_msg() stat.trace_topic = self.node_name + "/queue/" + modality self.archiver.disk_check(self.archiver._base, every_nth=4) - # rospy.loginfo('<^>{:>3} {:>6}: {:.6f}'.format(modality, event_msg.header.seq, event_msg.header.stamp.to_sec())) - # rospy.loginfo(modality + ': ' + str(image_msg.header)) with self.image_lock: - current_epoch = event_msg.header.stamp + current_epoch = stamp_key(event_msg.header.stamp) msg_dict = self.epoch_dict.get(current_epoch, {}) if len(msg_dict): # If there are already entries in the dict, that means they arrived # before this event callback, which is concerning - rospy.logwarn("Messages beat event: {}".format(msg_dict.keys())) + self.log.warning("Messages beat event: {}".format(msg_dict.keys())) msg_dict.update({"evt": event_msg}) self.epoch_dict[current_epoch] = msg_dict - rospy.loginfo( + self.log.info( "Starting {: >4}: epoch {}, epochs: {}".format( - event_msg.header.seq, current_epoch, self.epoch_dict.keys() + event_msg.event_num, current_epoch, self.epoch_dict.keys() ) ) if current_epoch in self._recent_epochs: - rospy.logerr("Duplicate event! {}".format(event_msg.header)) + self.log.error("Duplicate event! {}".format(event_msg.header)) else: self._recent_epochs.append(current_epoch) self._current_epoch = current_epoch @@ -539,24 +470,20 @@ def insert_msg(self, image_msg, modality="rgb"): stat = Stat() stat.trace_header = image_msg.header stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 + stat.header.stamp = self.now_msg() stat.trace_topic = self.node_name + "/queue/" + modality - # rospy.loginfo('<^>{:>3} {:>6}: {:.6f}'.format(modality, image_msg.header.seq, image_msg.header.stamp.to_sec())) - # rospy.loginfo(modality + ': ' + str(image_msg.header)) with self.image_lock: - epoch = image_msg.header.stamp + epoch = stamp_key(image_msg.header.stamp) msg_dict = self.epoch_dict.get(epoch, {}) if "evt" not in msg_dict: - rospy.logwarn( + self.log.warning( "{} Message beat event: {}, epochs: {}".format( modality, epoch, self.epoch_dict.keys() ) ) if modality in msg_dict: - rospy.logerr( + self.log.error( "Duplicate message {} in epoch: {}".format(modality, epoch) ) if modality == "rgb": @@ -573,108 +500,6 @@ def sync_queue_callback(self, image_msg, modality): self.insert_msg(image_msg=image_msg, modality=modality) self.end_of_turn() - def sync_queue_callback2(self, image_msg, modality): - # type: (MsgImage, str) -> None - """Method that receives messages published on self.image_topic - - :param image_msg: ROS image message. - :type image_msg: Image - - :param modality: Which image stream from which to return an image view. - :type modality: str {'EVT', 'RGB','IR','UV'} - - """ - modality = modality.lower() - stat = Stat() - stat.trace_header = image_msg.header - stat.node = self.node_name - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 - stat.trace_topic = self.node_name + "/queue/" + modality - - rospy.loginfo( - "{:>3} {:>6}: {:.6f}".format( - modality, image_msg.header.seq, image_msg.header.stamp.to_sec() - ) - ) - # rospy.loginfo(modality + ': ' + str(image_msg.header)) - with self.image_lock: - header = image_msg.header - t = header.stamp.secs + header.stamp.nsecs / 1e9 - t = datetime.utcfromtimestamp(t) - if modality == "evt": - raise NotImplementedError("Dead end! shouldn't happen") - self.stat_pub.publish(stat) - - if header.stamp != self._current_epoch: - if header.stamp in self._recent_epochs: - rospy.logerr("Stale epoch on {}: {}".format(modality, header.stamp)) - else: - rospy.logerr("Stale epoch on {}: {}".format(modality, header.stamp)) - - # rospy.loginfo('{:>3} {:>6} {}'.format(modality, image_msg.header.seq, t.isoformat()[11:24])) - - # rospy.logdebug('{:>3} {:>6} {:.3f}'.format(modality, image_msg.header.seq, image_msg.header.stamp.to_sec())) - - if modality == "rgb": - image_msg = debayer_image_msg(image_msg) - - if modality == "evt" and modality in self.msg_dict: - # oops, we got double event before buffer filled - # publish and roll over message - raise NotImplementedError("Dead end! shouldn't happen") - rospy.logwarn( - "OOPS double event! Missed packet?: {}".format(self.msg_dict.keys()) - ) - self.publish() - self.msg_dict.update({modality: image_msg}) - return - elif modality == "evt" and "ir" in self.msg_dict: - evt_time = image_msg.header.stamp.to_sec() - msg_time = self.msg_dict["ir"].header.stamp.to_sec() - rospy.logwarn("IR beat event by {}".format(evt_time - msg_time)) - if ( - abs(evt_time - msg_time) < 0.499 - ): # empirically determined IR can lead by as much as 650 ms but system capped at 2 Hz - self.msg_dict.update({modality: image_msg}) - rospy.logwarn("This is fine") - else: - rospy.logerr( - "Publishing incomplete message: {}".format(self.msg_dict.keys()) - ) - self.publish() - self.msg_dict.update({modality: image_msg}) - elif modality == "evt" and ( - "rgb" in self.msg_dict or "uv" in self.msg_dict - ): - # ok we got event but there is stuff? reset the cycle - # assume event always makes it first - rospy.logwarn( - "got event but stuff in buffer: {}".format(self.msg_dict.keys()) - ) - self.publish() - self.msg_dict.update({modality: image_msg}) - else: - self.msg_dict.update({modality: image_msg}) - - if self.verbosity > 10: - # visual symbols for fast debugging - smsg = "{} Rx {: >4} {}".format( - self.symbol_dict.get(modality), - modality, - rostime_to_datetime(image_msg.header.stamp).isoformat(), - ) - rospy.loginfo("sync msg: {}".format(smsg)) - if self.is_msg_dict_full(): - self.publish() - elif self.pub_timer is None: - # Start a new timer to publish after 'max_wait'. - self.pub_timer = rospy.Timer( - rospy.Duration(self.max_wait), self.publish, oneshot=True - ) - self.end_of_turn() - def check_success(self, msg_dict): # type: (dict) -> Tuple[list, list] """Returns list of names of all messages present and non-zero in message buffer @@ -683,19 +508,19 @@ def check_success(self, msg_dict): fail_list = [] for chan in self.enabled_list + ["evt", "ins"]: if chan not in msg_dict: - rospy.logerr("Expecting {} Message, not in msg_dict ".format(chan)) + self.log.error("Expecting {} Message, not in msg_dict ".format(chan)) fail_list.append(chan) continue if chan in ["evt", "ins"]: result = True else: - result = check_image_msg(msg_dict[chan], chan) + result = check_image_msg(msg_dict[chan], chan, self.log) if result is not None: success_list.append(chan) else: - rospy.logerr(f"Registered {chan} as a miss.") + self.log.error(f"Registered {chan} as a miss.") return success_list, fail_list @@ -709,17 +534,13 @@ def _publish(self, timer_event=None, msg_dict=None, record_stats=True): with self.image_lock: stat.node = self.node_name stat.trace_topic = self.node_name + "/" + "sync" - stat.header.stamp = rospy.Time.now() - stat.header.seq = self.stat_counter - self.stat_counter += 1 + stat.header.stamp = self.now_msg() if timer_event is not None: - rospy.logerr("Publishing due to timer callback") + self.log.error("Publishing due to timer callback") msg_dict["ins"] = self.archiver.latch_ins - rospy.logdebug("Pub'd: {}".format(msg_dict.keys())) - if not any(msg_dict): # why does this happen? - rospy.logerr("Tried to publish, but no data in buffer") + self.log.error("Tried to publish, but no data in buffer") return outmsg = SyncedImageMsg() @@ -743,16 +564,10 @@ def _publish(self, timer_event=None, msg_dict=None, record_stats=True): # keep only good data messages - this also should simplify dump_sync msg_dict = {k: msg_dict[k] for k in success_list} - s = "img_nexus.py:publish() \n" - for k, v in msg_dict.items(): - s += "||{:>3}: {:.3f}\n".format(k, v.header.stamp.to_sec()) - # rospy.loginfo(s) - # Deal with missing event, we still need a header if "evt" not in msg_dict: - rospy.loginfo("Exiting because dummy message.") + self.log.info("Exiting because dummy message.") return - msg_dict["evt"] = self.get_spoof_event() event = msg_dict.get("evt") stat.meta_json = json.dumps(record) @@ -781,9 +596,9 @@ def _publish(self, timer_event=None, msg_dict=None, record_stats=True): pathdict = self.archiver.dump_sync_image_messages(msg_dict) self.stats_logger.append(record) if self.verbosity > 3: - rospy.loginfo("pathdict: {}".format(pathdict)) + self.log.info("pathdict: {}".format(pathdict)) else: - rospy.loginfo("archived") + self.log.info("archived") # Reset all the image buffers. msg_dict = dict() @@ -804,7 +619,7 @@ def _publish(self, timer_event=None, msg_dict=None, record_stats=True): img = cv2.imdecode(cv2.imencode(".jpg", cv_img, encode_param)[1], 1) msg = bridge.cv2_to_imgmsg(img, encoding=encoding) setattr(outmsg, "image_" + mode, msg) - rospy.loginfo("All img conversions took %0.3fs." % (time.time() - tic)) + self.log.info("All img conversions took %0.3fs." % (time.time() - tic)) infostr = "SYN ({} {} {}{}) {: >3.0%}".format( "EVT" * record["have_evt"] or " ", @@ -824,4 +639,4 @@ def _publish(self, timer_event=None, msg_dict=None, record_stats=True): self.pstat_pub.publish(stat) self.pub_status.publish(infomsg) self.publisher.publish(outmsg) - rospy.loginfo(infostr) + self.log.info(infostr) diff --git a/src/process/view_server/src/view_server/web_server_node.py b/src/process/view_server/src/view_server/web_server_node.py deleted file mode 100755 index d8d5f67b..00000000 --- a/src/process/view_server/src/view_server/web_server_node.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/python3 -import io -import cv2 -import flask -import time -import numpy as np -from flask import Flask, jsonify, request, send_file - -import rospy -from cv_bridge import CvBridge -from sensor_msgs.msg import Image -from phase_one.srv import GetCompressedImageView, GetImageView - - -app = Flask(__name__) -bridge = CvBridge() - - -@app.get('/') -def index(): - return "

(∩ ` -´)⊃━━☆ﾟ.*･｡ﾟ

" - - -@app.get('/image__') -def get_image(w, h): - tic = time.time() - topic = "/StandAlone/get_image_view" - srv = rospy.ServiceProxy(topic, GetImageView, - persistent=False) - try: - resp = srv(output_width=int(w), output_height=int(h)) - except rospy.service.ServiceException as e: - print(topic) - rospy.logerr(e) - resp = None - toc = time.time() - print("Time to receive incoming image was %.3fs" % (toc - tic)) - if resp and resp.success: - cv_image = bridge.imgmsg_to_cv2(resp.image, - desired_encoding='passthrough') - image_binary = cv2.imencode(".jpeg", - cv_image[:,:,::-1])[1].tobytes() - else: - image_binary = cv2.imencode(".jpeg", np.zeros([100,100,3 - ],dtype=np.uint8))[1].tobytes() - response = flask.make_response(image_binary) - # could be png here - response.headers.set('Content-Type', 'image/jpeg') - toc = time.time() - print("Time to process incoming image was %.3fs" % (toc - tic)) - return response - - -def main(): - print("Starting ROS node.") - rospy.init_node("img_server", anonymous=False) - print("Starting App.") - app.run(host="0.0.0.0", port=5000, use_reloader=False, debug=True) - print("Finished.") - - -if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/run_scripts/entry/nexus.sh b/src/run_scripts/entry/nexus.sh deleted file mode 100755 index 9e6ef2a7..00000000 --- a/src/run_scripts/entry/nexus.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash - -# Nexus node startup script - -echo "<> <> <> NEXUS <> <> <> " -source /entry/project.sh - -pip install redis - -ROSWAIT="--wait" - -exec roslaunch "${ROSWAIT}" nexus nexus.launch \ - system_name:=${NODE_HOSTNAME} \ - verbosity:=$(/cfg/get ".verbosity") From 87cdf6d3900d4024a80bcab202af9dc57c3ad9c0 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:09:03 -0400 Subject: [PATCH 04/20] Port ins_driver to ROS2 - ins_socket_driver and spoof_events become rclpy console scripts; a background executor thread services the archiver's ROS interfaces while the main thread runs the blocking GSOF socket loop - gsof.py no longer needs rospy: dispatches build builtin_interfaces Time stamps from GPS time floats; module-level rclpy logger. header.seq assignments dropped (removed in ROS2); event identity lives in event_num / the frame_id query string - Global rosparams (/data_mount_point, spoof params) were already redundant with env vars + Redis; node params reduced to ip/port/replay/retry - Drop the legacy upstream NMEA driver stack (driver.py, parser.py, nmea_class, checksum_utils, four nmea_* scripts, ins_spoof_driver, nmea.launch): nayak/taiga only ever launch ins_socket_driver, which explicitly disabled the NMEA path - Launch files converted to ROS2 XML; ament_python build --- src/core/ins_driver/CMakeLists.txt | 21 - src/core/ins_driver/launch/ins.launch | 38 -- src/core/ins_driver/launch/ins.launch.xml | 15 + src/core/ins_driver/launch/nmea.launch | 33 -- .../ins_driver/launch/spoof_events.launch | 6 - .../ins_driver/launch/spoof_events.launch.xml | 4 + src/core/ins_driver/package.xml | 29 +- .../__init__.py => resource/ins_driver} | 0 src/core/ins_driver/scripts/ins_spoof_driver | 277 ------------- .../ins_driver/scripts/nmea_serial_driver | 63 --- .../ins_driver/scripts/nmea_socket_driver | 269 ------------- src/core/ins_driver/scripts/nmea_topic_driver | 55 --- .../scripts/nmea_topic_serial_reader | 66 ---- src/core/ins_driver/scripts/spoof_events.py | 34 -- src/core/ins_driver/setup.cfg | 4 + src/core/ins_driver/setup.py | 33 +- .../libnmea_navsat_driver/checksum_utils.py | 48 --- .../src/libnmea_navsat_driver/driver.py | 365 ------------------ .../src/libnmea_navsat_driver/gsof.py | 133 +++---- .../ins_socket_driver.py} | 299 +++++--------- .../src/libnmea_navsat_driver/nmea_class.py | 71 ---- .../src/libnmea_navsat_driver/parser.py | 300 -------------- .../src/libnmea_navsat_driver/spoof_events.py | 40 ++ 23 files changed, 268 insertions(+), 1935 deletions(-) delete mode 100644 src/core/ins_driver/CMakeLists.txt delete mode 100644 src/core/ins_driver/launch/ins.launch create mode 100644 src/core/ins_driver/launch/ins.launch.xml delete mode 100644 src/core/ins_driver/launch/nmea.launch delete mode 100644 src/core/ins_driver/launch/spoof_events.launch create mode 100644 src/core/ins_driver/launch/spoof_events.launch.xml rename src/core/ins_driver/{scripts/__init__.py => resource/ins_driver} (100%) delete mode 100755 src/core/ins_driver/scripts/ins_spoof_driver delete mode 100755 src/core/ins_driver/scripts/nmea_serial_driver delete mode 100755 src/core/ins_driver/scripts/nmea_socket_driver delete mode 100755 src/core/ins_driver/scripts/nmea_topic_driver delete mode 100755 src/core/ins_driver/scripts/nmea_topic_serial_reader delete mode 100755 src/core/ins_driver/scripts/spoof_events.py create mode 100644 src/core/ins_driver/setup.cfg delete mode 100644 src/core/ins_driver/src/libnmea_navsat_driver/checksum_utils.py delete mode 100644 src/core/ins_driver/src/libnmea_navsat_driver/driver.py rename src/core/ins_driver/{scripts/ins_socket_driver => src/libnmea_navsat_driver/ins_socket_driver.py} (52%) delete mode 100644 src/core/ins_driver/src/libnmea_navsat_driver/nmea_class.py delete mode 100644 src/core/ins_driver/src/libnmea_navsat_driver/parser.py create mode 100755 src/core/ins_driver/src/libnmea_navsat_driver/spoof_events.py diff --git a/src/core/ins_driver/CMakeLists.txt b/src/core/ins_driver/CMakeLists.txt deleted file mode 100644 index 1f57483a..00000000 --- a/src/core/ins_driver/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ins_driver) - -find_package(catkin REQUIRED) - -catkin_python_setup() -catkin_package() - -install(PROGRAMS - scripts/nmea_serial_driver - scripts/nmea_socket_driver - scripts/nmea_topic_driver - scripts/nmea_topic_serial_reader - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -#if (CATKIN_ENABLE_TESTING) -# find_package(roslint) -# roslint_python() -# roslint_add_test() -#endif() diff --git a/src/core/ins_driver/launch/ins.launch b/src/core/ins_driver/launch/ins.launch deleted file mode 100644 index da2269fa..00000000 --- a/src/core/ins_driver/launch/ins.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/core/ins_driver/launch/ins.launch.xml b/src/core/ins_driver/launch/ins.launch.xml new file mode 100644 index 00000000..de99e8e5 --- /dev/null +++ b/src/core/ins_driver/launch/ins.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/core/ins_driver/launch/nmea.launch b/src/core/ins_driver/launch/nmea.launch deleted file mode 100644 index dfc7a24b..00000000 --- a/src/core/ins_driver/launch/nmea.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/core/ins_driver/launch/spoof_events.launch b/src/core/ins_driver/launch/spoof_events.launch deleted file mode 100644 index 3d2afb0d..00000000 --- a/src/core/ins_driver/launch/spoof_events.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/src/core/ins_driver/launch/spoof_events.launch.xml b/src/core/ins_driver/launch/spoof_events.launch.xml new file mode 100644 index 00000000..691b7271 --- /dev/null +++ b/src/core/ins_driver/launch/spoof_events.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/core/ins_driver/package.xml b/src/core/ins_driver/package.xml index 2f8c2066..919758ce 100644 --- a/src/core/ins_driver/package.xml +++ b/src/core/ins_driver/package.xml @@ -1,35 +1,30 @@ - + + ins_driver - 0.5.0 + 1.0.0 - Package to parse NMEA strings and publish a very simple GPS message. Does not - require or use the GPSD deamon. + Trimble POS AVX GSOF socket driver: publishes INS and event messages. - Ed Venator - + Adam Romlein BSD - http://ros.org/wiki/nmea_navsat_driver - Eric Perko Steven Martin - catkin - - rospy + rclpy python3-serial - geometry_msgs - nmea_msgs - sensor_msgs + python3-redis + std_msgs + builtin_interfaces custom_msgs kamcore + nexus + roskv - - - + ament_python diff --git a/src/core/ins_driver/scripts/__init__.py b/src/core/ins_driver/resource/ins_driver similarity index 100% rename from src/core/ins_driver/scripts/__init__.py rename to src/core/ins_driver/resource/ins_driver diff --git a/src/core/ins_driver/scripts/ins_spoof_driver b/src/core/ins_driver/scripts/ins_spoof_driver deleted file mode 100755 index 93b75eba..00000000 --- a/src/core/ins_driver/scripts/ins_spoof_driver +++ /dev/null @@ -1,277 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Software License Agreement (BSD License) -# -# Copyright (c) 2016, Rein Appeldoorn -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -import os -import errno -import socket -from socket import error as socket_error -import sys -import time -import struct -import serial -import datetime - -import redis -import rospy -import std_msgs.msg -from libnmea_navsat_driver.gsof import (parse_gsof_stream, maybe_gsof, separate_nmea, GsofInsDispatch, - GsofEventDispatch, GsofSpoofEventDispatch, GsofSpoofInsDispatch, - GsofEvtSpoofer, GsofHeader, parse_gsof) - -from custom_msgs.msg import Stat -from msgdispatch.archive import ArchiveSchemaDispatch -from nexus.archiver_core import ArchiverBase -from libnmea_navsat_driver.stream_archive import dumpbuf, enumerate_packets -# import libnmea_navsat_driver.driver -from kamcore.datatypes import ToDictMxn, TryIntoAttrMxn, DefaultInitializer, _Stamp, _Header -from roskv.impl.redis_envoy import RedisEnvoy, redis_encode - - -# from vprint import aprint -# from vprint.base256 import b256encode - - -def rospy_spin(delay=1.0): - """ - Blocks until ROS node is shutdown. Yields activity to other threads. - @raise ROSInitException: if node is not in a properly initialized state - """ - - if not rospy.core.is_initialized(): - raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first") - rospy.logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), - os.getpid()) - try: - while not rospy.core.is_shutdown(): - rospy.rostime.wallsleep(delay) - # rospy.loginfo('spin') - # print('.', end='') - except KeyboardInterrupt: - rospy.logdebug("keyboard interrupt, shutting down") - rospy.core.signal_shutdown('keyboard interrupt') - - -class DummyDriver(object): - def add_sentence(self, sentence, frame_id, *args, **kwargs): - print('{:5d}: {}'.format(frame_id, sentence)) - - -def loginfo(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('info: {}'.format(msg)) - - -def logwarn(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('warn: {}'.format(msg)) - - -def logerr(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('err : {}'.format(msg)) - - -class Rate(object): - def __init__(self, rate=5): - self.rate = rate - - def set_rate(self, msg): - self.rate = msg.data - - @property - def period(self): - return 1.0 / self.rate - -class FailedToInitInsDriver(Exception): - def __init__(self, msg=None, host='', port=0, exc=None): - # type: (str, str, int, Exception) -> None - """ - Error for failing to initially connect to NMEA server. This is extra - bad, so we want to handle this outside regular socket errors - :param msg: custom error message - """ - if msg is None: - msg = ('Failed to initialize INS socket client on host {}:{}' - '\n Is the INS connected?'.format(host, port)) - if exc is not None: - msg += '\nOriginal exception: {}'.format(exc) - super(FailedToInitInsDriver, self).__init__(msg) - - -class AvxSpoofClient(object): - - def __init__(self): - redis_host = os.environ.get('REDIS_HOST', 'nuvo0') - self.envoy = RedisEnvoy(redis_host, client_name='ins') - - print('redis established, term: {}'.format(self.envoy.get('term'))) - self._data_mount_point = rospy.get_param('/data_mount_point', '/mnt/ins_default') - self._host = rospy.get_param('/host_center', '/nuvo0') - self._project = 'default2019' - self._flight = 'fl00' - self.archiver = ArchiverBase() - self.evt_spoofer = GsofEvtSpoofer() - - namespace = rospy.get_name() - namespace = self._host - self.archiver.advertise_services(namespace=namespace) - rospy.loginfo('Namespace: {}'.format(namespace)) - - def cb_trig_pub(self, header): - event_arrived = rospy.Time.now() - fake_packet = self.evt_spoofer.next_packet() - header = GsofHeader(fake_packet) - dispatch = parse_gsof(header, fake_packet) - dispatch.msg.sys_time = event_arrived - dispatch.publish() - ins_dispatch = GsofSpoofInsDispatch() - ins_dispatch.publish() - - def run(self, host, port, buffer_size=4096, timeout=2.0): - # disabling NMEA driver cause it's janky and running both binary and NMEA really confuses the parser - # driver = libnmea_navsat_driver.driver.RosNMEADriver() - GsofInsDispatch.add_publisher('/ins') - GsofEventDispatch.add_publisher('/event') - # GsofSpoofEventDispatch.add_publisher('event') # todo - # recv-loop: When we're connected, keep receiving stuff until that fails - counter = 0 - for rawdata in gen_packets(host, port): - event_arrived = rospy.Time.now() - if rospy.is_shutdown(): - break - - raw_ins_path = self.archiver.get_raw_ins_path() - if not (counter % 500): - rospy.loginfo('Ins path: {}'.format(raw_ins_path)) - counter += 1 - - # todo: optionally archive stream -# dumpbuf(raw_ins_path, rawdata) - - nmea_list, gsof_data = separate_nmea(rawdata) - #aprint(nmea_list) - # aprint(str(len(gsof_data)) + '[' + b256encode(gsof_data) + ']') - - # if nmea_list: - # for nmea in nmea_list: - # driver.add_sentence(nmea) - dispatches = [] - if maybe_gsof(gsof_data): - try: - dispatches = parse_gsof_stream(gsof_data) - except struct.error as err: - rospy.logerr("Gsof parse error: {}".format(err)) - except Exception as err: - rospy.logerr("Some other exception in parsing: {}".format(err)) - #aprint(dispatch.msg) - for d in dispatches: - # print(d) - d.msg.sys_time = event_arrived - d.publish() -# dumpbuf(self.archiver.get_raw_ins_path(field=d.label), d.buf) - - - # evt = GsofSpoofEventDispatch() - # evt.publish() - - - # ignoring NMEA for now - - -if __name__ == '__main__': - rospy.init_node('ins_socket_driver') - allow_serial_ins_spoof = int(os.environ.get('ALLOW_SERIAL_INS_SPOOF', 0) or 0) - try: - host = rospy.get_param('~ip', '0.0.0.0') - port = rospy.get_param('~port', 10110) - buffer_size = rospy.get_param('~buffer_size', 4096) - timeout = rospy.get_param('~timeout_sec', 2) - # spoof = rospy.get_param('spoof_rate', 0) or rospy.get_param('~spoof') - spoof_rate = max(int(os.environ.get('SPOOF_RATE', 0) or 0), 0) - - replay_path = rospy.get_param('~replay') - retry = rospy.get_param('~retry', True) - except KeyError as e: - rospy.logerr("Parameter %s not found" % e) - sys.exit(1) - - client = AvxSpoofClient() - pulse_tty = os.environ.get('PULSE_TTY', None) - if allow_serial_ins_spoof: - rospy.logwarn("ALLOW_SERIAL_INS_SPOOF ON. Serial-based spoof active") - try: - ser = serial.Serial(pulse_tty) - except Exception as exc: - ser = None - rospy.logerr('Unable to find tty: {}'.format(pulse_tty)) - sys.exit(1) - - if ser: - print('Serial connected: {}'.format(ser.name)) - client.spoof_serial(ser) - sys.exit(0) - - if spoof_rate > 0 : - rospy.logwarn("\nGlobal spoof enabled. \nSPOOF_RATE={:.3f}".format(spoof_rate)) - print("SPOOOOOOOF: {:.2f}".format(spoof_rate)) - client.spoof(spoof_rate) - sys.exit(0) - elif replay_path: - rospy.logwarn("\nReplay INS \nreplay_path={}".format(replay_path)) - client.replay(replay_path) - sys.exit(0) - - while not rospy.is_shutdown(): - try: - client.run(host, port, buffer_size, timeout) - - except FailedToInitInsDriver as err: - rospy.logerr('Failed to connect to INS: {}'.format(err)) - if retry: - rospy.logwarn('Gracefully attempting to reconnect to INS...') - time.sleep(1) - else: - rospy.logerr('Gave up trying to connect to INS, terminating') - raise err - except socket_error as err: - rospy.logerr('Other socket error trying to connect to INS: {}'.format(err)) - raise err - except (KeyboardInterrupt, SystemExit): - rospy.loginfo('User quitting') - sys.exit(130) - except Exception as err: - print(type(err)) - #import pdb; pdb.set_trace() - rospy.logerr('Encountered exception, continuing: {}'.format(err)) diff --git a/src/core/ins_driver/scripts/nmea_serial_driver b/src/core/ins_driver/scripts/nmea_serial_driver deleted file mode 100755 index 4d6f912a..00000000 --- a/src/core/ins_driver/scripts/nmea_serial_driver +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python3 - -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -import serial - -import rospy - -import libnmea_navsat_driver.driver - -if __name__ == '__main__': - rospy.init_node('nmea_serial_driver') - - serial_port = rospy.get_param('~port','/dev/ttyUSB0') - serial_baud = rospy.get_param('~baud',4800) - frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() - - try: - GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) - - try: - driver = libnmea_navsat_driver.driver.RosNMEADriver() - while not rospy.is_shutdown(): - data = GPS.readline().strip() - try: - driver.add_sentence(data, frame_id) - except ValueError as e: - rospy.logwarn("Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) - - except (rospy.ROSInterruptException, serial.serialutil.SerialException): - GPS.close() #Close GPS serial port - except serial.SerialException as ex: - rospy.logfatal("Could not open serial port: I/O error({0}): {1}".format(ex.errno, ex.strerror)) diff --git a/src/core/ins_driver/scripts/nmea_socket_driver b/src/core/ins_driver/scripts/nmea_socket_driver deleted file mode 100755 index b01fe0ce..00000000 --- a/src/core/ins_driver/scripts/nmea_socket_driver +++ /dev/null @@ -1,269 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Software License Agreement (BSD License) -# -# Copyright (c) 2016, Rein Appeldoorn -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -import os -import socket -import sys -import rospy -from libnmea_navsat_driver.gsof import (parse_gsof_stream, maybe_gsof, - separate_nmea, GsofInsDispatch, - GsofEventDispatch, GsofSpoofEventDispatch) -from msgdispatch.archive import ArchiveSchemaDispatch -from nexus.archiver_core import ArchiverBase -from libnmea_navsat_driver.stream_archive import dumpbuf, enumerate_packets -import libnmea_navsat_driver.driver - -# from vprint import aprint -# from vprint.base256 import b256encode - - -def netcat(hostname, port, content): - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - s.connect((hostname, port)) - # s.sendall(content) - s.shutdown(socket.SHUT_WR) - while 1: - data = s.recv(1024) - if data == "": - break - print("Received: {}".format(repr(data))) - print("Connection closed.") - s.close() - - -class DummyDriver(object): - def add_sentence(self, sentence, frame_id, *args, **kwargs): - print('{:5d}: {}'.format(frame_id, sentence)) - - -def loginfo(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('info: {}'.format(msg)) - - -def logwarn(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('warn: {}'.format(msg)) - - -def logerr(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) - print('err : {}'.format(msg)) - - -class FailedToInitNmeaClient(Exception): - def __init__(self, msg=None, host='', port=0, exc=None): - # type: (str, str, int, Exception) -> None - """ - Error for failing to initially connect to NMEA server. This is extra - bad, so we want to handle this outside regular socket errors - :param msg: custom error message - """ - if msg is None: - msg = ('Failed to initialize NMEA socket client on host {}:{}' - ''.format(host, port)) - if exc is not None: - msg += '\nOriginal exception: {}'.format(exc) - super(FailedToInitNmeaClient, self).__init__(msg) - - -def gen_packets(host, port, buffer_size=4096, timeout=2.0): - # type: (str, int, int, float) -> str - """ - Packet generator for streaming data from NMEA service - :param host: - :param port: - :param buffer_size: - :param timeout: - - Args: - host: hostname of NMEA device - port: port of NMEA device - buffer_size: recv() buffer size - timeout: socket timeout - - Yields: - NMEA data strings - """ - - sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - try: - rospy.loginfo('Accessing {}:{}'.format(host, port)) - sock.connect((host, port)) - sock.settimeout(timeout) - rospy.loginfo('Connected to {}:{}'.format(host, port)) - - while True: - try: - yield sock.recv(buffer_size).strip() - - except socket.error as exc: - logerr( - "Caught exception socket.error during recv: %s" % exc) - - except socket.error as exc: - logerr('Critical failure in initualization of host {}:{}'.format(host, port)) - raise FailedToInitNmeaClient(host=host, port=port, exc=exc) - - finally: - sock.close() - - -class AvxClient(object): - - def __init__(self): - self._data_mount_point = rospy.get_param('/data_mount_point', '/mnt/kamera') - self._project = 'default2019' - self._flight = 'fl00' - self.archiver = ArchiverBase() - - def run(self, host, port, buffer_size=4096, timeout=2.0): - driver = libnmea_navsat_driver.driver.RosNMEADriver() - GsofInsDispatch.add_publisher('/gis/ins') - GsofEventDispatch.add_publisher('/gis/event') - # GsofSpoofEventDispatch.add_publisher('/gis/event') # todo - - # recv-loop: When we're connected, keep receiving stuff until that fails - for rawdata in gen_packets(host, port): - if rospy.is_shutdown(): - break - - # todo: optionally archive stream - dumpbuf(self.archiver.get_ins_path(), rawdata) - - nmea_list, gsof_data = separate_nmea(rawdata) - #aprint(nmea_list) - # aprint(str(len(gsof_data)) + '[' + b256encode(gsof_data) + ']') - - if nmea_list: - for nmea in nmea_list: - driver.add_sentence(nmea) - - if maybe_gsof(gsof_data): - dispatches = parse_gsof_stream(gsof_data) - #aprint(dispatch.msg) - for d in dispatches: - # print(d) - d.publish() - - - # evt = GsofSpoofEventDispatch() - # evt.publish() - - - # ignoring NMEA for now - @staticmethod - def replay(path_to_data): - print('REPLAY MODE') - with open(path_to_data, 'rb') as fp: - raw_stream = fp.read() - - GsofEventDispatch.add_publisher('/gis/event') - GsofInsDispatch.add_publisher('/gis/ins') - # recv-loop: When we're connected, keep receiving stuff until that fails - for i, rawdata in enumerate_packets(raw_stream): - if rospy.is_shutdown(): - break - print(i, len(rawdata)) - # todo: optionally archive stream - - nmea_list, gsof_data = separate_nmea(rawdata) - # aprint(nmea_list) - # aprint(str(len(gsof_data)) + '[' + b256encode(gsof_data) + ']') - - if maybe_gsof(gsof_data): - dispatches = parse_gsof_stream(gsof_data) - # aprint(dispatch.msg) - for d in dispatches: - # print(d) - d.publish() - continue - - - @staticmethod - def spoof(frequency=5): - import time - period = 1.0 / frequency - rospy.logwarn('Going into event spoof mode!') - GsofSpoofEventDispatch.add_publisher('/gis/event') - while not rospy.is_shutdown(): - dispatch = GsofSpoofEventDispatch() - dispatch.publish() - time.sleep(period) - # rospy.loginfo(str(dispatch)) - - def update_schema(self, msg): - # type: (ArchiveSchemaDispatch) -> None - rospy.loginfo('Set schema: \n{}'.format(str(msg))) - self._project = msg.project - self._flight = msg.flight - - def rawfile_template(self): - - return os.path.join(self.basepath, self._project, self._flight, 'ins.dat') - - @property - def basepath(self): - """ - This is the root path all other subdirectories stem from. - :return: - """ - return self._data_mount_point - - - - -if __name__ == '__main__': - rospy.init_node('nmea_socket_driver') - try: - host = rospy.get_param('~ip', '0.0.0.0') - port = rospy.get_param('~port', 10110) - buffer_size = rospy.get_param('~buffer_size', 4096) - timeout = rospy.get_param('~timeout_sec', 2) - spoof = rospy.get_param('~spoof') - replay_path = rospy.get_param('~replay') - except KeyError as e: - rospy.logerr("Parameter %s not found" % e) - sys.exit(1) - - - client = AvxClient() - if spoof > 0 : - client.spoof(spoof) - elif replay_path: - client.replay(replay_path) - else: - client.run(host, port, buffer_size, timeout) - diff --git a/src/core/ins_driver/scripts/nmea_topic_driver b/src/core/ins_driver/scripts/nmea_topic_driver deleted file mode 100755 index 580bd4de..00000000 --- a/src/core/ins_driver/scripts/nmea_topic_driver +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python3 - -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -import rospy - -from nmea_msgs.msg import Sentence - -import libnmea_navsat_driver.driver - -def nmea_sentence_callback(nmea_sentence, driver): - try: - driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id, timestamp=nmea_sentence.header.stamp) - except ValueError as e: - rospy.logwarn("Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) - -if __name__ == '__main__': - rospy.init_node('nmea_topic_driver') - - driver = libnmea_navsat_driver.driver.RosNMEADriver() - - rospy.Subscriber("nmea_sentence", Sentence, nmea_sentence_callback, - driver) - - rospy.spin() diff --git a/src/core/ins_driver/scripts/nmea_topic_serial_reader b/src/core/ins_driver/scripts/nmea_topic_serial_reader deleted file mode 100755 index b7af6a9e..00000000 --- a/src/core/ins_driver/scripts/nmea_topic_serial_reader +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python3 - -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -import serial - -import rospy - -from nmea_msgs.msg import Sentence -from libnmea_navsat_driver.driver import RosNMEADriver - -if __name__ == '__main__': - rospy.init_node('nmea_topic_serial_reader') - - nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1) - - serial_port = rospy.get_param('~port','/dev/ttyUSB0') - serial_baud = rospy.get_param('~baud',4800) - - # Get the frame_id - frame_id = RosNMEADriver.get_frame_id() - - try: - GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) - while not rospy.is_shutdown(): - data = GPS.readline().strip() - - sentence = Sentence() - sentence.header.stamp = rospy.get_rostime() - sentence.header.frame_id = frame_id - sentence.sentence = data - - nmea_pub.publish(sentence) - - except rospy.ROSInterruptException: - GPS.close() #Close GPS serial port diff --git a/src/core/ins_driver/scripts/spoof_events.py b/src/core/ins_driver/scripts/spoof_events.py deleted file mode 100755 index f9ac3a7f..00000000 --- a/src/core/ins_driver/scripts/spoof_events.py +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python3 -import rospy - -import math -from custom_msgs.msg import GSOF_EVT -from std_msgs.msg import Header - -print("Creating spoof publisher.") -spoof_pub = rospy.Publisher("/event", GSOF_EVT, queue_size=1) - -def main(): - print("Initializing spoof node.") - rospy.init_node("event_spoofer") - sub = rospy.Subscriber("/trig", Header, pub) - rospy.spin() - - -def pub(hmsg): - t = hmsg.stamp - s = t.to_sec() - #ns = t.to_nsec() - #s = math.floor(ns / 1e9) - #t = rospy.Time.from_sec(s) - msg = GSOF_EVT() - msg.header.stamp = t - msg.gps_time = t - msg.sys_time = t - msg.time = s - spoof_pub.publish(msg) - rospy.loginfo("Published event msg.") - - -if __name__ == "__main__": - main() diff --git a/src/core/ins_driver/setup.cfg b/src/core/ins_driver/setup.cfg new file mode 100644 index 00000000..6b9304d0 --- /dev/null +++ b/src/core/ins_driver/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ins_driver +[install] +install_scripts=$base/lib/ins_driver diff --git a/src/core/ins_driver/setup.py b/src/core/ins_driver/setup.py index 0a1d4585..46da82d2 100644 --- a/src/core/ins_driver/setup.py +++ b/src/core/ins_driver/setup.py @@ -1,10 +1,29 @@ -#!/usr/bin/env python3 +from glob import glob + from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup -d = generate_distutils_setup( - packages=['libnmea_navsat_driver'], - package_dir={'': 'src'}, -) +package_name = "ins_driver" -setup(**d) +setup( + name=package_name, + version="1.0.0", + packages=["libnmea_navsat_driver"], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Trimble POS AVX GSOF socket driver", + license="BSD", + entry_points={ + "console_scripts": [ + "ins_socket_driver = libnmea_navsat_driver.ins_socket_driver:main", + "spoof_events = libnmea_navsat_driver.spoof_events:main", + ], + }, +) diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/checksum_utils.py b/src/core/ins_driver/src/libnmea_navsat_driver/checksum_utils.py deleted file mode 100644 index 899ae388..00000000 --- a/src/core/ins_driver/src/libnmea_navsat_driver/checksum_utils.py +++ /dev/null @@ -1,48 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - - -# Check the NMEA sentence checksum. Return True if passes and False if failed -def check_nmea_checksum(nmea_sentence): - split_sentence = nmea_sentence.split('*') - if len(split_sentence) != 2: - #No checksum bytes were found... improperly formatted/incomplete NMEA data? - return False - transmitted_checksum = split_sentence[1].strip() - - #Remove the $ at the front - data_to_checksum = split_sentence[0][1:] - checksum = 0 - for c in data_to_checksum: - checksum ^= ord(c) - - return ("%02X" % checksum) == transmitted_checksum.upper() diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/driver.py b/src/core/ins_driver/src/libnmea_navsat_driver/driver.py deleted file mode 100644 index 46b4b044..00000000 --- a/src/core/ins_driver/src/libnmea_navsat_driver/driver.py +++ /dev/null @@ -1,365 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -from typing import List -import math - -import rospy - -from sensor_msgs.msg import NavSatFix, NavSatStatus, TimeReference -from geometry_msgs.msg import TwistStamped, QuaternionStamped -from custom_msgs.msg import PASHR, EVT -from tf.transformations import quaternion_from_euler - -from libnmea_navsat_driver.checksum_utils import check_nmea_checksum -import libnmea_navsat_driver.parser -from . import nmea_class - -class PashrPub(nmea_class.NMEA): - def __init__(self, name, queue_size=1): - super(PashrPub, self).__init__(name=name, msg=PASHR, queue_size=queue_size) - - def from_dict(self, data): - pashr = self.msg() - self.format_header(pashr, data) - pashr.time = data['utc_time'] - pashr.heading = data['heading'] - pashr.roll = data['roll'] - pashr.pitch = data['pitch'] - pashr.gnss_status = data['gnss_status'] - pashr.imu_alignment_status = data['imu_alignment_status'] - return pashr - - -class EvtPub(nmea_class.NMEA): - def __init__(self, name, queue_size=1): - super(EvtPub, self).__init__(name=name, msg=EVT, queue_size=queue_size) - - def from_dict(self, data): - evt = self.msg() - self.format_header(evt, data) - evt.time = data['utc_time'] - evt.event = data['event'] - evt.event_counter = data['event_counter'] - return evt - - -class HeadingPub(nmea_class.NMEA): - def __init__(self, name, queue_size=1): - super(HeadingPub, self).__init__(name=name, msg=QuaternionStamped, queue_size=queue_size) - - def from_dict(self, data): - current_heading = self.msg() - self.format_header(current_heading, data) - heading = data.get('heading') - q = quaternion_from_euler(0, 0, math.radians(heading)) - current_heading.quaternion.x = q[0] - current_heading.quaternion.y = q[1] - current_heading.quaternion.z = q[2] - current_heading.quaternion.w = q[3] - return current_heading - - -class TimeRefPub(nmea_class.NMEA): - def __init__(self, name, queue_size=1): - super(TimeRefPub, self).__init__(name=name, msg=TimeReference, queue_size=queue_size) - - def from_dict(self, data): - timestamp = data['utc_time'] - if math.isnan(timestamp): - return None - - current_time_ref = self.msg_from_header(data) - current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) - source = data.get('time_ref_source', None) - if source: - current_time_ref.source = source - return current_time_ref - - -class RosNMEADriver(object): - - def __init__(self): - self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) - self.vel_pub = rospy.Publisher('vel', TwistStamped, queue_size=1) - self.heading_pub = HeadingPub('heading', queue_size=1) - self.time_ref_pub = rospy.Publisher('time_reference', TimeReference, queue_size=1) - self.time_ref_pub2 = TimeRefPub('time_reference', queue_size=1) - self.pashr_pub = PashrPub('pashr', queue_size=1) - self.evt_pub = EvtPub('evt', queue_size=5) - - self.time_ref_source = rospy.get_param('~time_ref_source', None) - self.use_RMC = rospy.get_param('~useRMC', False) - - # epe = estimated position error - self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000) - self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0) - self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1) - self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02) - self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0) - self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0) - self.using_receiver_epe = False - - self.lon_std_dev = float("nan") - self.lat_std_dev = float("nan") - self.alt_std_dev = float("nan") - - """Format for this dictionary is the fix type from a GGA message as the key, with - each entry containing a tuple consisting of a default estimated - position error, a NavSatStatus value, and a NavSatFix covariance value.""" - self.gps_qualities = { - # Unknown - -1: [ - self.default_epe_quality0, - NavSatStatus.STATUS_NO_FIX, - NavSatFix.COVARIANCE_TYPE_UNKNOWN - ], - # Invalid - 0: [ - self.default_epe_quality0, - NavSatStatus.STATUS_NO_FIX, - NavSatFix.COVARIANCE_TYPE_UNKNOWN - ], - # SPS - 1: [ - self.default_epe_quality1, - NavSatStatus.STATUS_FIX, - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - ], - # DGPS - 2: [ - self.default_epe_quality2, - NavSatStatus.STATUS_SBAS_FIX, - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - ], - # RTK Fix - 4: [ - self.default_epe_quality4, - NavSatStatus.STATUS_GBAS_FIX, - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - ], - # RTK Float - 5: [ - self.default_epe_quality5, - NavSatStatus.STATUS_GBAS_FIX, - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - ], - # WAAS - 9: [ - self.default_epe_quality9, - NavSatStatus.STATUS_GBAS_FIX, - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - ] - } - - def set_std_from_epe(self, default_epe): - # use default epe std_dev unless we've received a GST sentence with epes - if not self.using_receiver_epe or math.isnan(self.lon_std_dev): - self.lon_std_dev = default_epe - if not self.using_receiver_epe or math.isnan(self.lat_std_dev): - self.lat_std_dev = default_epe - if not self.using_receiver_epe or math.isnan(self.alt_std_dev): - self.alt_std_dev = default_epe * 2 - - def covar_from_hdop(self, hdop): - position_covariance = [0, ] * 9 - position_covariance[0] = (hdop * self.lon_std_dev) ** 2 - position_covariance[4] = (hdop * self.lat_std_dev) ** 2 - position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME - return position_covariance - - # Returns True if we successfully did something with the passed in - # nmea_string - def add_sentence(self, nmea_string, frame_id=None, timestamp=None): - if nmea_string[0] != '$': - return False - - if not check_nmea_checksum(nmea_string): - rospy.logwarn("Received a sentence with an invalid checksum. " + - "Sentence was: %s" % repr(nmea_string)) - print('invalid checksum') - return False - - parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) - if not parsed_sentence: - rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) - print('failed to parse: {}'.format(nmea_string)) - return False - - if frame_id is None: - frame_id = self.get_frame_id() - - # print('\n' + str(parsed_sentence)) - - if timestamp: - current_time = timestamp - else: - current_time = rospy.get_rostime() - current_fix = NavSatFix() - current_fix.header.stamp = current_time - current_fix.header.frame_id = frame_id - current_time_ref = TimeReference() - current_time_ref.header.stamp = current_time - current_time_ref.header.frame_id = frame_id - if self.time_ref_source: - current_time_ref.source = self.time_ref_source - else: - current_time_ref.source = frame_id - - header = {'stamp': current_time, 'frame_id': frame_id} - - # GGA with no RMC - if not self.use_RMC and 'GGA' in parsed_sentence: - print('Branch 1') - current_fix.position_covariance_type = \ - NavSatFix.COVARIANCE_TYPE_APPROXIMATED - - data = parsed_sentence['GGA'] - print(data) - fix_type = data['fix_type'] - if not (fix_type in self.gps_qualities): - fix_type = -1 - gps_qual = self.gps_qualities[fix_type] - default_epe = gps_qual[0] - current_fix.status.status = gps_qual[1] - current_fix.status.service = NavSatStatus.SERVICE_GPS - current_fix.position_covariance_type = gps_qual[2] - - data.update({}) - current_fix.latitude = data['latitude'] - current_fix.longitude = data['longitude'] - current_fix.altitude = data['altitude'] - - self.set_std_from_epe(default_epe) - positional_covar = self.covar_from_hdop(data['hdop']) - for n in [0, 4, 8]: - current_fix.position_covariance[n] = positional_covar[n] - - data.update({'header': header, 'time_ref_source': 'GGA'}) - self.fix_pub.publish(current_fix) - self.time_ref_pub2.publish_from_dict(data) - - - elif 'RMC' in parsed_sentence: - print('Branch 2') - data = parsed_sentence['RMC'] - - # Only publish a fix from RMC if the use_RMC flag is set. - if self.use_RMC: - if data['fix_valid']: - current_fix.status.status = NavSatStatus.STATUS_FIX - else: - current_fix.status.status = NavSatStatus.STATUS_NO_FIX - - current_fix.status.service = NavSatStatus.SERVICE_GPS - - current_fix.latitude = data['latitude'] - current_fix.longitude = data['longitude'] - - current_fix.altitude = float('NaN') - current_fix.position_covariance_type = \ - NavSatFix.COVARIANCE_TYPE_UNKNOWN - - data.update({'header': header, 'time_ref_source': 'RMC'}) - self.fix_pub.publish(current_fix) - self.time_ref_pub2.publish_from_dict(data) - - # Publish velocity from RMC regardless, since GGA doesn't provide it. - if data['fix_valid']: - current_vel = TwistStamped() - current_vel.header.stamp = current_time - current_vel.header.frame_id = frame_id - current_vel.twist.linear.x = data['speed'] * \ - math.sin(data['true_course']) - current_vel.twist.linear.y = data['speed'] * \ - math.cos(data['true_course']) - self.vel_pub.publish(current_vel) - elif 'GST' in parsed_sentence: - data = parsed_sentence['GST'] - - # Use receiver-provided error estimate if available - self.using_receiver_epe = True - self.lon_std_dev = data['lon_std_dev'] - self.lat_std_dev = data['lat_std_dev'] - self.alt_std_dev = data['alt_std_dev'] - elif 'HDT' in parsed_sentence: - data = parsed_sentence['HDT'] - data['header'] = header - self.heading_pub.publish_from_dict(data) - elif 'PASHR' in parsed_sentence: - data = parsed_sentence['PASHR'] - data.update({'header': header, 'time_ref_source': 'PASHR'}) - self.pashr_pub.publish_from_dict(data) - self.time_ref_pub2.publish_from_dict(data) - - elif 'EVT' in parsed_sentence: - data = parsed_sentence['EVT'] - data.update({'header': header, 'time_ref_source': 'EVT'}) - self.evt_pub.publish_from_dict(data) - # self.time_ref_pub2.publish_from_dict(data) - - else: - return {} - - return parsed_sentence - - def publish_packets(self, dict_of_packets): - pass - - def add_multi(self, data_list): - # type: (List[str]) -> int - """ - Add multiple data packets and publish them - Args: - data_list: - - Returns: - count of packets published - """ - for sentence in data_list: - self.add_sentence(sentence) - - @staticmethod - def get_frame_id(): - """Helper method for getting the frame_id with the correct TF prefix""" - frame_id = rospy.get_param('~frame_id', 'gps') - if frame_id[0] != "/": - """Add the TF prefix""" - prefix = "" - prefix_param = rospy.search_param('tf_prefix') - if prefix_param: - prefix = rospy.get_param(prefix_param) - if prefix[0] != "/": - prefix = "/%s" % prefix - return "%s/%s" % (prefix, frame_id) - else: - return frame_id diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/gsof.py b/src/core/ins_driver/src/libnmea_navsat_driver/gsof.py index 4ff92620..3d81b84e 100644 --- a/src/core/ins_driver/src/libnmea_navsat_driver/gsof.py +++ b/src/core/ins_driver/src/libnmea_navsat_driver/gsof.py @@ -1,19 +1,24 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- import struct +import time as _time import datetime -from typing import Tuple -import rospy +from typing import List, Tuple + +import rclpy.logging +from builtin_interfaces.msg import Time as MsgTime + from . import gps_leap_seconds from kamcore.structures import BasicStamp, BasicHeader, BasicEvent from std_msgs.msg import Empty -from custom_msgs.msg import GSOF_INS, GSOF_EVT +from custom_msgs.msg import GsofIns, GsofEvt from msgdispatch.base import DispatchBase +log = rclpy.logging.get_logger("gsof") # Constants -gps_epoch = datetime.datetime(1980,1,6) +gps_epoch = datetime.datetime(1980, 1, 6) unix_epoch = datetime.datetime(1970, 1, 1) gps_leap_td = datetime.timedelta(seconds=gps_leap_seconds.GPS_LEAP_SECONDS) @@ -41,6 +46,27 @@ TEST_GSOF_PACKET = TEST_NMEA + TEST_GSOF_INTS +def time_msg_from_sec(t): + # type: (float) -> MsgTime + """Build a builtin_interfaces Time message from unix float seconds""" + sec = int(t) + nanosec = int(round((t - sec) * 1e9)) + if nanosec >= 1000000000: + sec += 1 + nanosec -= 1000000000 + return MsgTime(sec=sec, nanosec=nanosec) + + +def time_msg_now(): + # type: () -> MsgTime + return time_msg_from_sec(_time.time()) + + +def time_msg_to_sec(msg): + # type: (MsgTime) -> float + return msg.sec + msg.nanosec * 1e-9 + + def datetime_to_float(d): # type: (datetime.datetime) -> float """ @@ -52,8 +78,7 @@ def datetime_to_float(d): Returns: unix seconds since epoch """ - total_seconds = (d - unix_epoch).total_seconds() - # + total_seconds = (d - unix_epoch).total_seconds() return total_seconds @@ -133,7 +158,7 @@ def __new__(cls, buf): return None header = struct.unpack('>9B', buf[:9]) if header[0] != START_TX: - rospy.logwarn('Start byte does not match STX') + log.warning('Start byte does not match STX') return None self = object.__new__(cls) @@ -141,13 +166,10 @@ def __new__(cls, buf): self.message_type = header[2] ln = header[3] - # try: checksum, end = struct.unpack('>BB', buf[4+ln:6+ln]) computed_checksum = sum(bytearray(buf[1:-2])) & 0xff - # except struct.error as err: - # print('failed to unpack {}'.format(err )) if end != END_TX: - rospy.logwarn('Final byte does not match ETX') + log.warning('Final byte does not match ETX') return None self.len = ln @@ -192,7 +214,7 @@ def wrap_gsof(payload, message_type=GSOF_TYPE_MSG, record_type=GSOF_TYPE_EVENT, def parse_gsof_evt(buf, cls=BasicEvent): # type: (bytes, type) -> BasicEvent """ The return type is spoofed in order to allow static type checking, this will actually return a - type `cls` e.g. GSOF_EVT message""" + type `cls` e.g. GsofEvt message""" msg = cls() # type: BasicEvent data = struct.unpack('>BHdL', buf) @@ -224,8 +246,7 @@ def inc(self): def next_msg(self, now=None): if now is None: - import time - now = time.time() + now = _time.time() self.inc() gps_week, gps_time = utc_to_gps(now) data = struct.pack('>BHdL', self.event_port, gps_week, gps_time, self.event_num) @@ -251,8 +272,8 @@ def inc(self): def ins_from_envoy(self): dd = self.envoy.get_dict('/debug/spoof/ins') - msg = GSOF_INS() - for k,v in dd.items(): + msg = GsofIns() + for k, v in dd.items(): try: setattr(msg, k, v) except AttributeError as exc: @@ -262,8 +283,7 @@ def ins_from_envoy(self): def next_struct(self, msg=None, now=None): if now is None: - import time - now = time.time() + now = _time.time() self.inc() if msg is None: @@ -293,7 +313,7 @@ def next_struct(self, msg=None, now=None): buf[19] = msg.acceleration_y buf[20] = msg.acceleration_z - data = struct.pack('>HLbbdddffffddddffffff', *buf) + data = struct.pack('>HLbbdddffffddddffffff', *buf) return data @@ -304,7 +324,7 @@ def next_packet(self, msg=None, now=None): class GsofInsDispatch(DispatchBase): counter = 0 - message_class = GSOF_INS + message_class = GsofIns pubs = {} label = 'ins' __slots__ = ( @@ -325,16 +345,15 @@ def __new__(cls, buf): data = struct.unpack('>HLbbdddffffddddffffff', buf) gps_week = data[0] - gps_time = data[1] * 1e-3 # convert ms to s + gps_time = data[1] * 1e-3 # convert ms to s utc_time = gps_to_utc(gps_week, gps_time) # as unix time - self.msg.header.stamp = rospy.Time.from_sec(utc_time) - self.msg.header.seq = self.next_id() + self.next_id() + self.msg.header.stamp = time_msg_from_sec(utc_time) self.msg.header.frame_id = 'ins' - self.msg.time = utc_time - self.msg.gps_time = rospy.Time.from_sec(utc_time) + self.msg.gps_time = time_msg_from_sec(utc_time) self.msg.align_status = data[2] self.msg.gnss_status = data[3] self.msg.latitude = data[4] @@ -359,7 +378,7 @@ def __new__(cls, buf): class GsofEventDispatch(DispatchBase): counter = 0 - message_class = GSOF_EVT + message_class = GsofEvt pubs = {} label = 'evt' __slots__ = ['header', 'time', 'event_port', 'event_num'] @@ -367,7 +386,7 @@ class GsofEventDispatch(DispatchBase): def __new__(cls, buf): self = object.__new__(cls) self.msg = self.new_message() - self.msg.header.stamp = rospy.Time.now() + self.msg.header.stamp = time_msg_now() self.buf = bytes() if buf is None: @@ -381,23 +400,21 @@ def __new__(cls, buf): gps_time = data[2] # is actually seconds, unlike INS packet utc_time = gps_to_utc(gps_week, gps_time) # as unix time - self.msg.gps_time = rospy.Time.from_sec(utc_time) + self.msg.gps_time = time_msg_from_sec(utc_time) - self.msg.time = utc_time + self.msg.time = utc_time self.msg.event_port = data[0] - self.msg.event_num = data[3] # todo: should seq id match this? - # self.msg.header.seq = self.next_id() + self.msg.event_num = data[3] self.msg.header.stamp = self.msg.gps_time self.msg.header.frame_id = '/ins_evt?eventNum={}'.format(self.event_num) - self.msg.header.seq = self.event_num # todo: probably, things get really weird if these don't match - rospy.loginfo('{} {}: {}'.format(self.msg.header.seq, self.msg.event_num, self.msg.time)) + log.info('{}: {}'.format(self.msg.event_num, self.msg.time)) return self class GsofSpoofEventDispatch(DispatchBase): counter = 0 - message_class = GSOF_EVT + message_class = GsofEvt pubs = {} label = 'evt_spoof' @@ -408,22 +425,22 @@ def __new__(cls, stamp=None): self.buf = bytes() if stamp is None: - stamp = rospy.Time.now() + stamp = time_msg_now() + seq = self.next_id() self.msg.header.stamp = stamp - self.msg.header.seq = self.next_id() self.msg.header.frame_id = 'systime' self.msg.sys_time = stamp self.msg.gps_time = stamp - self.msg.time = stamp.to_sec() + self.msg.time = time_msg_to_sec(stamp) self.msg.event_port = 23 # sentinel value - self.msg.event_num = self.msg.header.seq & 0xffff + self.msg.event_num = seq & 0xffff return self class GsofSpoofInsDispatch(DispatchBase): counter = 0 - message_class = GSOF_INS + message_class = GsofIns pubs = {} label = 'ins_spoof' @@ -433,14 +450,13 @@ def __new__(cls): self.msg = self.new_message() self.buf = bytes() - utc_time = datetime_to_float(datetime.datetime.now()) # as unix time - self.msg.header.stamp = rospy.Time.from_sec(utc_time) - self.msg.header.seq = self.next_id() + self.next_id() + self.msg.header.stamp = time_msg_from_sec(utc_time) self.msg.header.frame_id = 'systime' - self.msg.time = utc_time + self.msg.time = utc_time self.msg.altitude = 333.0 self.msg.total_speed = 75.0 self.msg.latitude = 42.864407 @@ -450,7 +466,7 @@ def __new__(cls): def stream_gsof_chunker(buf): - # type: (bytes) -> List(Tuple) + # type: (bytes) -> List[Tuple] """ Break a binary stream into header/buffer pairs chunked into message size Args: @@ -476,6 +492,7 @@ def stream_gsof_chunker(buf): NullDispatch = ClsNullDispatch() + def parse_gsof(header, buf): # type: (GsofHeader, bytes) -> DispatchBase """ @@ -491,7 +508,7 @@ def parse_gsof(header, buf): """ if header.message_type != GSOF_TYPE_MSG: - rospy.logwarn('invalid message') + log.warning('invalid message') return NullDispatch start = 9 @@ -503,7 +520,7 @@ def parse_gsof(header, buf): elif header.record_type == GSOF_TYPE_RMS: raise NotImplementedError('RMS parser not available') else: - rospy.logwarn('message type not understood') + log.warning('message type not understood') return NullDispatch @@ -542,13 +559,6 @@ def separate_nmea(buf): Returns: (list_of_nmea, binary) - - Examples: - >>> stuff = separate_nmea(TEST_GSOF_PACKET) - >>> stuff[0] - '$GNGGA,154056.00,4251.87736134,N,07346.28348206,W,1,12,1.6,118.450,M,-31.849,M,,*4A' - >>> stuff[1] - '$PASHR,154056.000,354.688,T,1.115,-2.610,,0.248,0.248,71.520,1,2*27' """ nmea_list = [] tail = bytes(buf) @@ -565,20 +575,3 @@ def separate_nmea(buf): break return nmea_list, tail - - -def run_tests(): - from pprint import pprint - nmea_list, data = separate_nmea(TEST_GSOF_PACKET) - - dispatch = parse_gsof(data) - pprint(nmea_list) - print(len(data)) - print(data.hex()) - pprint(dispatch.msg) - - -if __name__ == '__main__': - run_tests() - - diff --git a/src/core/ins_driver/scripts/ins_socket_driver b/src/core/ins_driver/src/libnmea_navsat_driver/ins_socket_driver.py similarity index 52% rename from src/core/ins_driver/scripts/ins_socket_driver rename to src/core/ins_driver/src/libnmea_navsat_driver/ins_socket_driver.py index b6b6b0bf..c2b73c12 100755 --- a/src/core/ins_driver/scripts/ins_socket_driver +++ b/src/core/ins_driver/src/libnmea_navsat_driver/ins_socket_driver.py @@ -34,57 +34,33 @@ # POSSIBILITY OF SUCH DAMAGE. import os -import errno import socket from socket import error as socket_error import sys import time import struct -import serial -import datetime +import threading import redis -import rospy +import serial + +import rclpy +from rclpy.node import Node import std_msgs.msg -from libnmea_navsat_driver.gsof import (parse_gsof_stream, maybe_gsof, separate_nmea, GsofInsDispatch, - GsofEventDispatch, GsofSpoofEventDispatch, GsofSpoofInsDispatch, - GsofEvtSpoofer, GsofHeader, parse_gsof) -from custom_msgs.msg import Stat, GSOF_INS -from msgdispatch.archive import ArchiveSchemaDispatch +from libnmea_navsat_driver.gsof import ( + parse_gsof_stream, maybe_gsof, separate_nmea, GsofInsDispatch, + GsofEventDispatch, GsofSpoofEventDispatch, GsofSpoofInsDispatch, + GsofEvtSpoofer, GsofHeader, parse_gsof, time_msg_from_sec, time_msg_to_sec) + +from custom_msgs.msg import Stat, GsofIns from nexus.archiver_core import ArchiverBase from libnmea_navsat_driver.stream_archive import dumpbuf, enumerate_packets -# import libnmea_navsat_driver.driver -from kamcore.datatypes import ToDictMxn, TryIntoAttrMxn, DefaultInitializer, _Stamp, _Header - -# from vprint import aprint -# from vprint.base256 import b256encode - - -def rospy_spin(delay=1.0): - """ - Blocks until ROS node is shutdown. Yields activity to other threads. - @raise ROSInitException: if node is not in a properly initialized state - """ - - if not rospy.core.is_initialized(): - raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first") - rospy.logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), - os.getpid()) - try: - while not rospy.core.is_shutdown(): - rospy.rostime.wallsleep(delay) - # rospy.loginfo('spin') - # print('.', end='') - except KeyboardInterrupt: - rospy.logdebug("keyboard interrupt, shutting down") - rospy.core.signal_shutdown('keyboard interrupt') def netcat(hostname, port, content): s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((hostname, port)) - # s.sendall(content) s.shutdown(socket.SHUT_WR) while 1: data = s.recv(1024) @@ -95,23 +71,15 @@ def netcat(hostname, port, content): s.close() -class DummyDriver(object): - def add_sentence(self, sentence, frame_id, *args, **kwargs): - print('{:5d}: {}'.format(frame_id, sentence)) - - def loginfo(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) print('info: {}'.format(msg)) def logwarn(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) print('warn: {}'.format(msg)) def logerr(msg, *args, **kwargs): - # rospy.loginfo(msg, *args, **kwargs) print('err : {}'.format(msg)) @@ -126,6 +94,7 @@ def set_rate(self, msg): def period(self): return 1.0 / self.rate + class FailedToInitInsDriver(Exception): def __init__(self, msg=None, host='', port=0, exc=None): # type: (str, str, int, Exception) -> None @@ -136,21 +105,15 @@ def __init__(self, msg=None, host='', port=0, exc=None): """ if msg is None: msg = ('Failed to initialize INS socket client on host {}:{}' - '\n Is the INS connected?'.format(host, port)) + '\n Is the INS connected?'.format(host, port)) if exc is not None: msg += '\nOriginal exception: {}'.format(exc) super(FailedToInitInsDriver, self).__init__(msg) -def gen_packets(host, port, buffer_size=4096, timeout=2.0): - # type: (str, int, int, float) -> str +def gen_packets(node, host, port, buffer_size=4096, timeout=2.0): """ Packet generator for streaming data from NMEA service - :param host: - :param port: - :param buffer_size: - :param timeout: - Args: host: hostname of NMEA device port: port of NMEA device @@ -163,10 +126,10 @@ def gen_packets(host, port, buffer_size=4096, timeout=2.0): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) try: - rospy.loginfo('Accessing {}:{}'.format(host, port)) + node.get_logger().info('Accessing {}:{}'.format(host, port)) sock.connect((host, port)) sock.settimeout(timeout) - rospy.loginfo('Connected to {}:{}'.format(host, port)) + node.get_logger().info('Connected to {}:{}'.format(host, port)) while True: try: @@ -186,38 +149,39 @@ def gen_packets(host, port, buffer_size=4096, timeout=2.0): class AvxClient(object): - def __init__(self): - self._data_mount_point = rospy.get_param('/data_mount_point', '/mnt/ins_default') + def __init__(self, node, rc): + self.node = node + self.log = node.get_logger() + self.rc = rc self._host = socket.gethostname() - self._project = 'default2024' - self._flight = 'fl00' - self.archiver = ArchiverBase() - namespace = rospy.get_name() + self.archiver = ArchiverBase(node) namespace = self._host self.archiver.advertise_services(namespace=namespace) - rospy.loginfo('Namespace: {}'.format(namespace)) + self.log.info('Namespace: {}'.format(namespace)) + + def now_msg(self): + return self.node.get_clock().now().to_msg() def run(self, host, port, buffer_size=4096, timeout=2.0): - # disabling NMEA driver cause it's janky and running both binary and NMEA really confuses the parser - # driver = libnmea_navsat_driver.driver.RosNMEADriver() - GsofInsDispatch.add_publisher('/ins') - GsofEventDispatch.add_publisher('/event') - # GsofSpoofEventDispatch.add_publisher('event') # todo + if '/ins' not in GsofInsDispatch.pubs: + GsofInsDispatch.add_publisher(self.node, '/ins') + GsofEventDispatch.add_publisher(self.node, '/event') # recv-loop: When we're connected, keep receiving stuff until that fails counter = 0 + rc = self.rc spoof_events = rc.get("/debug/spoof_events") if spoof_events is not None: spoof_events = int(spoof_events) else: spoof_events = 0 - for rawdata in gen_packets(host, port): - event_arrived = rospy.Time.now() - if rospy.is_shutdown(): + for rawdata in gen_packets(self.node, host, port, buffer_size, timeout): + event_arrived = self.now_msg() + if not rclpy.ok(): break raw_ins_path = self.archiver.get_raw_ins_path() if not (counter % 100): - rospy.loginfo('Ins path: {}'.format(raw_ins_path)) + self.log.info('Ins path: {}'.format(raw_ins_path)) spoof_events = rc.get("/debug/spoof_events") if spoof_events is not None: spoof_events = int(spoof_events) @@ -225,29 +189,23 @@ def run(self, host, port, buffer_size=4096, timeout=2.0): spoof_events = 0 counter += 1 - # todo: optionally archive stream dumpbuf(raw_ins_path, rawdata) nmea_list, gsof_data = separate_nmea(rawdata) - #aprint(nmea_list) - # aprint(str(len(gsof_data)) + '[' + b256encode(gsof_data) + ']') - # if nmea_list: - # for nmea in nmea_list: - # driver.add_sentence(nmea) dispatches = [] if maybe_gsof(gsof_data): try: dispatches = parse_gsof_stream(gsof_data) except struct.error as err: - rospy.logerr("Gsof parse error: {}".format(err)) + self.log.error("Gsof parse error: {}".format(err)) except Exception as err: - rospy.logerr("Some other exception in parsing: {}".format(err)) - #aprint(dispatch.msg) + self.log.error("Some other exception in parsing: {}".format(err)) for d in dispatches: d.msg.sys_time = event_arrived if isinstance(d, GsofEventDispatch) and spoof_events: - rospy.logwarn("WARNING: Not publishing events because /debug/spoof_events is true.") + self.log.warning( + "WARNING: Not publishing events because /debug/spoof_events is true.") # let the spoofer handle it continue elif isinstance(d, GsofInsDispatch): @@ -263,101 +221,61 @@ def run(self, host, port, buffer_size=4096, timeout=2.0): spoof_events = 0 d.publish() -# dumpbuf(self.archiver.get_raw_ins_path(field=d.label), d.buf) - - - # evt = GsofSpoofEventDispatch() - # evt.publish() - # ignoring NMEA for now - @staticmethod - def replay(path_to_data): + + def replay(self, path_to_data): print('REPLAY MODE') with open(path_to_data, 'rb') as fp: raw_stream = fp.read() - GsofEventDispatch.add_publisher('/event') - GsofInsDispatch.add_publisher('/ins') + if '/ins' not in GsofInsDispatch.pubs: + GsofEventDispatch.add_publisher(self.node, '/event') + GsofInsDispatch.add_publisher(self.node, '/ins') # recv-loop: When we're connected, keep receiving stuff until that fails for i, rawdata in enumerate_packets(raw_stream): - if rospy.is_shutdown(): + if not rclpy.ok(): break print(i, len(rawdata)) - # todo: optionally archive stream nmea_list, gsof_data = separate_nmea(rawdata) - # aprint(nmea_list) - # aprint(str(len(gsof_data)) + '[' + b256encode(gsof_data) + ']') if maybe_gsof(gsof_data): dispatches = parse_gsof_stream(gsof_data) - # aprint(dispatch.msg) for d in dispatches: - # print(d) d.publish() continue - @staticmethod - def run_spoofed_ins(pt1, pt2, dt, freq): - # Spoof a list of iNS messages over a given time period over - # 2 different points - rospy.logwarn("RUNNING SPOOFED INS MODE.") - import numpy as np - pub = rospy.Publisher("/ins", GSOF_INS, queue_size=10) - num_samples = int(freq * dt) - xnew = np.linspace(pt1[0], pt2[0], num=(num_samples)) - ynew = np.linspace(pt1[1], pt2[1], num=(num_samples)) - rate = rospy.Rate(freq) - for x, y in zip(xnew, ynew): - t = rospy.Time.now() - msg = GSOF_INS() - msg.latitude = x - msg.longitude = y - msg.altitude = 3000 - msg.total_speed = 100 - msg.gps_time = t - msg.time = t.to_sec() - msg.header.stamp = t - pub.publish(msg) - rate.sleep() - - @staticmethod - def spoof(frequency=5): - import time + def spoof(self, frequency=5): rate = Rate(frequency) - rospy.logwarn('Going into event spoof mode!') - GsofSpoofEventDispatch.add_publisher('/event') - trigger_sub = rospy.Subscriber('/daq/trigger_freq', - std_msgs.msg.Float64, - rate.set_rate) - while not rospy.is_shutdown(): + self.log.warning('Going into event spoof mode!') + GsofSpoofEventDispatch.add_publisher(self.node, '/event') + self.node.create_subscription( + std_msgs.msg.Float64, '/daq/trigger_freq', rate.set_rate, 10) + while rclpy.ok(): dispatch = GsofSpoofEventDispatch() dispatch.publish() time.sleep(rate.period) - rospy.loginfo(str(dispatch)) + self.log.info(str(dispatch)) - @staticmethod - def spoof_serial(ser, frequency=1000): + def spoof_serial(self, ser, frequency=1000): """Use this if you have a pulse plugged into your serial port via DSR""" - import time rate = Rate(frequency) - rospy.logwarn('Going into event PULSE mode!') - GsofEventDispatch.add_publisher('/event') - GsofSpoofEventDispatch.add_publisher('/event') - GsofSpoofInsDispatch.add_publisher('/ins') + self.log.warning('Going into event PULSE mode!') + GsofEventDispatch.add_publisher(self.node, '/event') + GsofSpoofEventDispatch.add_publisher(self.node, '/event') + GsofSpoofInsDispatch.add_publisher(self.node, '/ins') pulse = False - stat_pub = rospy.Publisher('/stat', Stat, queue_size=10) - rospy.logwarn('Stat pub engaged') + stat_pub = self.node.create_publisher(Stat, '/stat', 10) + self.log.warning('Stat pub engaged') evt_spoofer = GsofEvtSpoofer() clock_skew = float(os.environ.get('CLOCK_SKEW', 0.0)) - clock_skew = rospy.Duration.from_sec(clock_skew) - while not rospy.is_shutdown(): + while rclpy.ok(): # detect edge if not pulse: if ser.dsr: - event_arrived = rospy.Time.now() + event_arrived = self.now_msg() fake_packet = evt_spoofer.next_packet() header = GsofHeader(fake_packet) dispatch = parse_gsof(header, fake_packet) @@ -365,104 +283,95 @@ def spoof_serial(ser, frequency=1000): stat = Stat() pulse = True - dispatch.msg.sys_time = rospy.Time.now() + clock_skew + now_skewed = time_msg_to_sec(self.now_msg()) + clock_skew + dispatch.msg.sys_time = time_msg_from_sec(now_skewed) dispatch.publish() seq = dispatch.msg.event_num stat.trace_header = dispatch.msg.header - stat.node = rospy.get_name() + stat.node = self.node.get_name() stat.link = '/event/{}'.format(seq) stat.trace_topic = '/event' stat_pub.publish(stat) ins_dispatch = GsofSpoofInsDispatch() ins_dispatch.publish() - rospy.loginfo('dsr pulse {:>6} {:.3f}'.format(dispatch.msg.header.seq, dispatch.msg.header.stamp.to_sec())) - # rospy.loginfo('{}'.format(dispatch.msg)) + self.log.info('dsr pulse {:>6} {:.3f}'.format( + dispatch.msg.event_num, + time_msg_to_sec(dispatch.msg.header.stamp))) else: if not ser.dsr: pulse = False time.sleep(rate.period) - # rospy.loginfo(str(dispatch)) -if __name__ == '__main__': +def main(args=None): redis_host = os.environ.get('REDIS_HOST', 'nuvo0') rc = redis.Redis(host=redis_host, client_name='ins') print('redis established, term: {}'.format(rc.get('term'))) - rospy.init_node('ins_socket_driver') + + rclpy.init(args=args) + node = Node('ins_socket_driver') + log = node.get_logger() + allow_serial_ins_spoof = int(os.environ.get('ALLOW_SERIAL_INS_SPOOF', 0) or 0) - try: - host = rospy.get_param('~ip', '0.0.0.0') - port = rospy.get_param('~port', 10110) - buffer_size = rospy.get_param('~buffer_size', 4096) - timeout = rospy.get_param('~timeout_sec', 2) - # spoof = rospy.get_param('spoof_rate', 0) or rospy.get_param('~spoof') - spoof_rate = max(int(os.environ.get('SPOOF_RATE', 0) or 0), 0) - - replay_path = rospy.get_param('~replay') - retry = rospy.get_param('~retry', True) - except KeyError as e: - rospy.logerr("Parameter %s not found" % e) - sys.exit(1) - - client = AvxClient() - - spoof_ins = os.environ.get('SPOOF_INS', 0) - if False: #int(spoof_ins) == 1: - print("Spoof ins!!") - # wa shapefile - #pt1 = (47.907982,-121.976531) - #pt2 = (47.875874, -121.976165) - # NOAA Campus - pt1 = (47.910178, -121.965058) - pt2 = (47.881414, -121.989867) - duration_of_flight_in_s = 200 - client.run_spoofed_ins(pt1, pt2, duration_of_flight_in_s, freq=100) + host = node.declare_parameter('ip', '0.0.0.0').value + port = node.declare_parameter('port', 10110).value + buffer_size = node.declare_parameter('buffer_size', 4096).value + timeout = node.declare_parameter('timeout_sec', 2.0).value + spoof_rate = max(int(os.environ.get('SPOOF_RATE', 0) or 0), 0) + replay_path = node.declare_parameter('replay', '').value + retry = node.declare_parameter('retry', True).value + # Services/subscriptions (archiver) are handled by a background executor + # while the main thread runs the blocking socket recv loop. + spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + spin_thread.start() + + client = AvxClient(node, rc) pulse_tty = os.environ.get('PULSE_TTY', None) if allow_serial_ins_spoof: - rospy.logwarn("ALLOW_SERIAL_INS_SPOOF ON. Serial-based spoof active") + log.warning("ALLOW_SERIAL_INS_SPOOF ON. Serial-based spoof active") try: ser = serial.Serial(pulse_tty) - except Exception as exc: - ser = None - rospy.logerr('Unable to find tty: {}'.format(pulse_tty)) + except Exception: + log.error('Unable to find tty: {}'.format(pulse_tty)) sys.exit(1) - if ser: - print('Serial connected: {}'.format(ser.name)) - client.spoof_serial(ser) - sys.exit(0) + print('Serial connected: {}'.format(ser.name)) + client.spoof_serial(ser) + sys.exit(0) - if spoof_rate > 0 : - rospy.logwarn("\nGlobal spoof enabled. \nSPOOF_RATE={:.3f}".format(spoof_rate)) - print("SPOOOOOOOF: {:.2f}".format(spoof_rate)) + if spoof_rate > 0: + log.warning("\nGlobal spoof enabled. \nSPOOF_RATE={:.3f}".format(spoof_rate)) client.spoof(spoof_rate) sys.exit(0) elif replay_path: - rospy.logwarn("\nReplay INS \nreplay_path={}".format(replay_path)) + log.warning("\nReplay INS \nreplay_path={}".format(replay_path)) client.replay(replay_path) sys.exit(0) - while not rospy.is_shutdown(): + while rclpy.ok(): try: client.run(host, port, buffer_size, timeout) except FailedToInitInsDriver as err: - rospy.logerr('Failed to connect to INS: {}'.format(err)) + log.error('Failed to connect to INS: {}'.format(err)) if retry: - rospy.logwarn('Gracefully attempting to reconnect to INS...') + log.warning('Gracefully attempting to reconnect to INS...') time.sleep(1) else: - rospy.logerr('Gave up trying to connect to INS, terminating') + log.error('Gave up trying to connect to INS, terminating') raise err except socket_error as err: - rospy.logerr('Other socket error trying to connect to INS: {}'.format(err)) + log.error('Other socket error trying to connect to INS: {}'.format(err)) raise err except (KeyboardInterrupt, SystemExit): - rospy.loginfo('User quitting') + log.info('User quitting') sys.exit(130) except Exception as err: print(type(err)) - #import pdb; pdb.set_trace() - rospy.logerr('Encountered exception, continuing: {}'.format(err)) + log.error('Encountered exception, continuing: {}'.format(err)) + + +if __name__ == '__main__': + main() diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/nmea_class.py b/src/core/ins_driver/src/libnmea_navsat_driver/nmea_class.py deleted file mode 100644 index 06bdd1ad..00000000 --- a/src/core/ins_driver/src/libnmea_navsat_driver/nmea_class.py +++ /dev/null @@ -1,71 +0,0 @@ -import abc -import rospy - - -class NMEA(object): - def __init__(self, name, msg, queue_size=1): - self._name = name - self._queue_size = queue_size - self._msg = msg - self._pub = rospy.Publisher(name, msg, queue_size=queue_size) - - @abc.abstractmethod - def from_dict(self, data): - # type: (dict) -> genpy.msg - """ - This is just an example and should be overridder. - Args: - data: dict to be parsed - - Returns: - Rospy message populated from dict - """ - msg = self._msg() - msg.header.stamp = data['current_time'] - return msg - - def publish_from_dict(self, data): - # type: (dict) -> bool - msg = self.from_dict(data) - if msg is None: - return False - self.publish(msg) - return True - - @staticmethod - def format_header(msg, data_or_header): - # type: (genpy.msg, dict) -> None - """ - Populate the message with header information. - Try to extract a field 'header' from the dict. Upon failing, - treat the dict as the header structure itself. - Args: - msg: - data_or_header: - - Returns: - - """ - maybe_header = data_or_header.get('header', None) - if maybe_header is None: - header = data_or_header - else: - header = maybe_header - msg.header.stamp = header['stamp'] - msg.header.frame_id = header['frame_id'] - - def msg_from_header(self, data_or_header): - msg = self.msg() - self.format_header(msg, data_or_header) - return msg - - @property - def name(self): - return self._name - - @property - def msg(self): - return self._msg - - def publish(self, msg): - self._pub.publish(msg) diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/parser.py b/src/core/ins_driver/src/libnmea_navsat_driver/parser.py deleted file mode 100644 index c23f1334..00000000 --- a/src/core/ins_driver/src/libnmea_navsat_driver/parser.py +++ /dev/null @@ -1,300 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Eric Perko -# 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 names of the authors nor the names of their -# affiliated organizations 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 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. - -from typing import List -import re -import time -import calendar -import math -import logging -# for doctests and debugging -from pprint import pprint -logger = logging.getLogger('rosout2') - - -def safe_float(field): - try: - return float(field) - except ValueError: - return float('NaN') - - -def safe_int(field): - try: - return int(field) - except ValueError: - return 0 - - -def convert_latitude(field): - return safe_float(field[0:2]) + safe_float(field[2:]) / 60.0 - - -def convert_longitude(field): - return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0 - - -def convert_time(nmea_utc): - # Get current time in UTC for date information - utc_struct = time.gmtime() # immutable, so cannot modify this one - utc_list = list(utc_struct) - # If one of the time fields is empty, return NaN seconds - if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6]: - return float('NaN') - else: - hours = int(nmea_utc[0:2]) - minutes = int(nmea_utc[2:4]) - seconds = int(nmea_utc[4:6]) - utc_list[3] = hours - utc_list[4] = minutes - utc_list[5] = seconds - unix_time = calendar.timegm(tuple(utc_list)) - return unix_time - - -def convert_time_float(nmea_utc): - t = convert_time(nmea_utc) - if math.isnan(t): - return t - else: - return t + float(nmea_utc[6:]) - - -def convert_status_flag(status_flag): - if status_flag == "A": - return True - elif status_flag == "V": - return False - else: - return False - - -def convert_knots_to_mps(knots): - return safe_float(knots) * 0.514444444444 - - -# Need this wrapper because math.radians doesn't auto convert inputs -def convert_deg_to_rads(degs): - return math.radians(safe_float(degs)) - -"""Format for this dictionary is a sentence identifier (e.g. "GGA") as the key, with a -list of tuples where each tuple is a field name, conversion function and index -into the split sentence""" -parse_maps = { - "GGA": [ - ("fix_type", int, 6), - ("ulatitude", convert_latitude, 2), - ("latitude_direction", str, 3), - ("ulongitude", convert_longitude, 4), - ("longitude_direction", str, 5), - ("orthometric_height", safe_float, 9), - ("mean_sea_level", safe_float, 11), - ("hdop", safe_float, 8), - ("num_satellites", safe_int, 7), - ("utc_time", convert_time_float, 1), - ], - "RMC": [ - ("utc_time", convert_time_float, 1), - ("fix_valid", convert_status_flag, 2), - ("ulatitude", convert_latitude, 3), - ("latitude_direction", str, 4), - ("ulongitude", convert_longitude, 5), - ("longitude_direction", str, 6), - ("speed", convert_knots_to_mps, 7), - ("true_course", convert_deg_to_rads, 8), - ], - "GST": [ - ("utc_time", convert_time_float, 1), - ("ranges_std_dev", safe_float, 2), - ("semi_major_ellipse_std_dev", safe_float, 3), - ("semi_minor_ellipse_std_dev", safe_float, 4), - ("semi_major_orientation", safe_float, 5), - ("lat_std_dev", safe_float, 6), - ("lon_std_dev", safe_float, 7), - ("alt_std_dev", safe_float, 8), - ], - "HDT": [ - ("heading", safe_float, 1), - ], - "PASHR": [ - ("utc_time", convert_time_float, 1), - ("heading", safe_float, 2), - ("roll", safe_float, 4), - ("pitch", safe_float, 5), - ("gnss_status", int, 10), - ("imu_alignment_status", int, 11), - ], - "EVT": [ - ("utc_time", convert_time_float, 2), - ("event", int, 3), - ("event_counter", int, 4), - ], - } - - -def rectify_latlonalt(geo_data): - # type: (dict) -> dict - if 'latitude_direction' not in geo_data: - # no need to rectify - return geo_data - latitude = geo_data['ulatitude'] - if geo_data['latitude_direction'] == 'S': - latitude = -latitude - - longitude = geo_data['ulongitude'] - if geo_data['longitude_direction'] == 'W': - longitude = -longitude - geo_data.update({'latitude': latitude, 'longitude': longitude}) - - # Altitude is above ellipsoid, so adjust for mean-sea-level - ortho = geo_data.get('orthometric_height', None) - if ortho is not None: - altitude = ortho + geo_data['mean_sea_level'] - geo_data.update({'altitude': altitude}) - - return geo_data - - -def parse_sentence_type(fields): - # type: (List[str]) -> str - """ - Parse sentence type from list of fields - Args: - fields: Lists of NMEA string fields - - Returns: - proper field name - - Examples: - >>> parse_sentence_type(['$PASHR','191019.500','57.100','T','1.161']) - 'PASHR' - >>> parse_sentence_type(['$PTNL','EVT','19','1','4','2045','1','18*72']) - 'EVT' - >>> parse_sentence_type(['$GNGGA','191020.00','4251','N','07346','W']) - 'GGA' - """ - if fields[0] == "$PASHR": - sentence_type = "PASHR" - elif fields[0] == "$PTNL": - sentence_type = fields[1] - else: - # Ignore the $ and talker ID portions (e.g. GP) - sentence_type = fields[0][3:] - return sentence_type - - -def is_valid_nmea(nmea_sentence): - # type: (str) -> bool - """ - Determines if string is a valid NMEA sentence - Args: - nmea_sentence: - - Returns: - true if valid - - Examples: - >>> assert is_valid_nmea('$GNGGA,195639.00,4....2*12') - >>> assert is_valid_nmea('$GNGGA,195637.00,07.*42') - >>> assert is_valid_nmea('$PASHR,195636.000,1,2*29') - >>> assert is_valid_nmea('$PTNL,EVT,,156.490,T,1.125,-2.6,2*29') - >>> assert is_valid_nmea('$PTNL,AVR,,+157.,Yaw,-2.6292,Tilt,2.2,16*36') - - """ - match = re.match('^\$(GP|GN|GL|P).*\*[0-9A-Fa-f]{2}$', nmea_sentence) - return not not match - - -def parse_nmea_sentence(nmea_sentence): - # Check for a valid nmea sentence - nmea_sentence = nmea_sentence.strip() - if not is_valid_nmea(nmea_sentence): - logger.debug("Regex didn't match, sentence not valid NMEA? Sentence was: %s" - % repr(nmea_sentence)) - return None - stripped_sentence = nmea_sentence[:-3] # Strip checksum - fields = [field.strip(',') for field in stripped_sentence.split(',')] - sentence_type = parse_sentence_type(fields) - - if sentence_type not in parse_maps: - logger.debug("Sentence type %s not in parse map, ignoring." - % repr(sentence_type)) - return None - - parse_map = parse_maps[sentence_type] - - parsed_sentence = {} - for entry in parse_map: - parsed_sentence[entry[0]] = entry[1](fields[entry[2]]) - - if sentence_type in ['GGA', 'RMC']: - parsed_sentence = rectify_latlonalt(parsed_sentence) - - return {sentence_type: parsed_sentence} - - -def some_doctests(): - """ - Examples: - >>> evt = '$PTNL,EVT,191023.083423,1,40104,2045,1,18*7E' - >>> pprint(parse_nmea_sentence(evt)['EVT']) - {'event': 1, 'event_counter': 40104, 'utc_time': 1552936223.083423} - >>> evt = '''$PTNL,EVT,191023.083423,1,40104,2045,1,18*7E ''' - >>> pprint(parse_nmea_sentence(evt)['EVT']) - {'event': 1, 'event_counter': 40104, 'utc_time': 1552936223.083423} - Examples: - >>> pashr = '$PASHR,191022.500,58.072,T,1.165,-2.568,,0.247,0.247,77.169,1,2*10' - >>> pprint(parse_nmea_sentence(pashr)['PASHR']) - {'gnss_status': 1, - 'heading': 58.072, - 'imu_alignment_status': 2, - 'pitch': -2.568, - 'roll': 1.165, - 'utc_time': 1552936222.5} - Examples: - >>> gga = '$GNGGA,193012.00,4251.87728463,N,07346.28597204,W,1,16,0.9,109.055,M,-31.849,M,,*44' - >>> pprint(parse_nmea_sentence(gga)['GGA']) - {'altitude': 109.055, - 'fix_type': 1, - 'hdop': 0.9, - 'latitude': 42.8646214105, - 'latitude_direction': 'N', - 'longitude': 73.77143286733333, - 'longitude_direction': 'W', - 'mean_sea_level': -31.849, - 'num_satellites': 16, - 'utc_time': 1552937412} - >>> avr = '$PTNL,AVR,201140.00,+53.6362,Yaw,-2.5690,Tilt,+1.0876,Roll,0.000,1,2.2,16*0E' - - - """ - pass diff --git a/src/core/ins_driver/src/libnmea_navsat_driver/spoof_events.py b/src/core/ins_driver/src/libnmea_navsat_driver/spoof_events.py new file mode 100755 index 00000000..6ceb14f2 --- /dev/null +++ b/src/core/ins_driver/src/libnmea_navsat_driver/spoof_events.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node + +from custom_msgs.msg import GsofEvt +from std_msgs.msg import Header + + +class EventSpoofer(Node): + def __init__(self): + super().__init__("event_spoofer") + self.spoof_pub = self.create_publisher(GsofEvt, "/event", 1) + self.sub = self.create_subscription(Header, "/trig", self.pub, 10) + + def pub(self, hmsg): + t = hmsg.stamp + msg = GsofEvt() + msg.header.stamp = t + msg.gps_time = t + msg.sys_time = t + msg.time = t.sec + t.nanosec * 1e-9 + self.spoof_pub.publish(msg) + self.get_logger().info("Published event msg.") + + +def main(args=None): + print("Initializing spoof node.") + rclpy.init(args=args) + node = EventSpoofer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From c085b267a377a6fc3d6d739b38c9c952f5c5bd68 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:14:37 -0400 Subject: [PATCH 05/20] Port mcc_daq and ser_daq to ROS2 mcc_daq: - daq_node rewritten roscpp -> rclcpp: TriggerTimer/AsyncTriggerTimer/ OneShotManager now use rclcpp timers and Time/Duration; one-shot timers are wall timers cancelled+erased on fire (uint64 ids replace boost uuids, dropping the boost dependency); AsyncSpinner replaced by a MultiThreadedExecutor - ROS1-only TimerEvent replaced with a minimal local struct carrying current_real - Global params /spoof_rate & /spoof_daq (loaded via the old roscore container) replaced with the SPOOF_RATE env var; node params declared explicitly - Dropped the unused UsbDaqDummy class, the interactive chatter test node, and chatter/testusb launch files - utils.h provides rclcpp logging shims so the vendored MCC hardware code keeps its ROS_INFO-style call sites ser_daq: - ser_daq_driver ported to rclpy (ament_python console script); pulse timing uses threading.Timer instead of rospy one-shot timers; unused params/spoof plumbing removed --- src/core/mcc_daq/CMakeLists.txt | 224 +--------- src/core/mcc_daq/launch/chatter.launch | 14 - src/core/mcc_daq/launch/daq.launch | 29 -- src/core/mcc_daq/launch/daq.launch.xml | 13 + src/core/mcc_daq/launch/testusb.launch | 14 - src/core/mcc_daq/package.xml | 71 +-- src/core/mcc_daq/src/chatter.cpp | 47 -- src/core/mcc_daq/src/daq_node.cpp | 407 ++++++++---------- src/core/mcc_daq/src/daq_node.h | 143 +++--- src/core/mcc_daq/src/usbdaq.cpp | 161 +------ src/core/mcc_daq/src/usbdaq.h | 85 +--- src/core/mcc_daq/src/utils.h | 10 +- src/core/ser_daq/CMakeLists.txt | 18 - src/core/ser_daq/launch/ser_daq.launch | 26 -- src/core/ser_daq/launch/ser_daq.launch.xml | 6 + src/core/ser_daq/package.xml | 24 +- .../{scripts/__init__.py => resource/ser_daq} | 0 src/core/ser_daq/scripts/ser_daq_driver | 98 ----- src/core/ser_daq/ser_daq/__init__.py | 0 src/core/ser_daq/ser_daq/ser_daq_driver.py | 76 ++++ src/core/ser_daq/setup.cfg | 4 + src/core/ser_daq/setup.py | 32 +- 22 files changed, 443 insertions(+), 1059 deletions(-) delete mode 100644 src/core/mcc_daq/launch/chatter.launch delete mode 100644 src/core/mcc_daq/launch/daq.launch create mode 100644 src/core/mcc_daq/launch/daq.launch.xml delete mode 100644 src/core/mcc_daq/launch/testusb.launch delete mode 100644 src/core/mcc_daq/src/chatter.cpp delete mode 100644 src/core/ser_daq/CMakeLists.txt delete mode 100644 src/core/ser_daq/launch/ser_daq.launch create mode 100644 src/core/ser_daq/launch/ser_daq.launch.xml rename src/core/ser_daq/{scripts/__init__.py => resource/ser_daq} (100%) delete mode 100755 src/core/ser_daq/scripts/ser_daq_driver create mode 100644 src/core/ser_daq/ser_daq/__init__.py create mode 100755 src/core/ser_daq/ser_daq/ser_daq_driver.py create mode 100644 src/core/ser_daq/setup.cfg diff --git a/src/core/mcc_daq/CMakeLists.txt b/src/core/mcc_daq/CMakeLists.txt index 4185a2b8..2fb9d220 100644 --- a/src/core/mcc_daq/CMakeLists.txt +++ b/src/core/mcc_daq/CMakeLists.txt @@ -1,148 +1,24 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(mcc_daq) -## Compile as C++17, supported in ROS Noetic and newer set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -## 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 - std_msgs - custom_msgs - roskv -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(custom_msgs REQUIRED) +find_package(roskv REQUIRED) -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mcc_daq -# CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib -) - - - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - -find_library(LIBUSB NAMES usb-1.0 ) +find_library(LIBUSB NAMES usb-1.0) find_path(LIBUSB_INCLUDE_DIR NAMES libusb.h PATH_SUFFIXES "include" "libusb" "libusb-1.0") find_library(LIBHIDAPI NAMES hidapi hidapi-libusb) find_path(LIBHIDAPI_INCLUDE_DIR hidapi.h PATH_SUFFIXES "hidapi") -message(STATUS) message(STATUS "FINDING USB and HID: ${LIBUSB_INCLUDE_DIR} ${LIBUSB} ${LIBHIDAPI_INCLUDE_DIR} ${LIBHIDAPI}") include_directories(/usr/include/libusb-1.0/) -link_directories(${LIBUSB} ${LIBHIDAPI}) - - -## 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 exec_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 exec_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 exec_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 -# ) - - - - -########### -## 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}/mcc_daq.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/mcc_daq_node.cpp) add_executable(ctest-usb2408 src/nist.c @@ -151,89 +27,25 @@ add_executable(ctest-usb2408 src/usb-2416.c src/test-usb2408.c ) +target_link_libraries(ctest-usb2408 ${LIBUSB} ${LIBHIDAPI} m) add_executable(daq_node src/nist.c src/pmd.c src/usb-2408.c src/usb-2416.c - src/daq_node.cpp src/daq_node.h + src/daq_node.cpp src/usbdaq.cpp - src/utils.h src/utils.cpp) - -add_executable(chatter - src/chatter.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(ctest-usb2408 ${LIBUSB} ${LIBHIDAPI} m) - -target_link_libraries( chatter - PUBLIC ${catkin_LIBRARIES} - ) - -target_include_directories( daq_node PUBLIC ${Boost_INCLUDE_DIR}) -target_link_libraries( daq_node - PUBLIC ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + src/utils.cpp) +ament_target_dependencies(daq_node rclcpp std_msgs custom_msgs roskv) +target_link_libraries(daq_node ${LIBUSB} ${LIBHIDAPI} ) -############# -## 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_mcc_daq.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(TARGETS daq_node ctest-usb2408 + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/src/core/mcc_daq/launch/chatter.launch b/src/core/mcc_daq/launch/chatter.launch deleted file mode 100644 index 21d72a8a..00000000 --- a/src/core/mcc_daq/launch/chatter.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/core/mcc_daq/launch/daq.launch b/src/core/mcc_daq/launch/daq.launch deleted file mode 100644 index eb436bed..00000000 --- a/src/core/mcc_daq/launch/daq.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/core/mcc_daq/launch/daq.launch.xml b/src/core/mcc_daq/launch/daq.launch.xml new file mode 100644 index 00000000..e487d7c1 --- /dev/null +++ b/src/core/mcc_daq/launch/daq.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/src/core/mcc_daq/launch/testusb.launch b/src/core/mcc_daq/launch/testusb.launch deleted file mode 100644 index 3f1f42cd..00000000 --- a/src/core/mcc_daq/launch/testusb.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/core/mcc_daq/package.xml b/src/core/mcc_daq/package.xml index 631c068b..374d9690 100644 --- a/src/core/mcc_daq/package.xml +++ b/src/core/mcc_daq/package.xml @@ -1,72 +1,23 @@ - + + mcc_daq - 0.0.0 - The mcc_daq package + 1.0.0 + MCC USB-2408 DAQ trigger node - - - Adam Romlein Michael McDermott - - - - - Apache 2.0 + ament_cmake - - - - - - - - - - - + rclcpp + std_msgs + custom_msgs + roskv + libusb-1.0-dev - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - custom_msgs - roskv - - roscpp - std_msgs - roscpp - std_msgs - custom_msgs - roskv - - - - - - + ament_cmake diff --git a/src/core/mcc_daq/src/chatter.cpp b/src/core/mcc_daq/src/chatter.cpp deleted file mode 100644 index 72c676bb..00000000 --- a/src/core/mcc_daq/src/chatter.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include -#include - -int G_SIGINT_TRIGGERED = 0; - -int main(int argc, char** argv) -{ - std::string node_ns = "/subsys0/chatter_in"; - std::string topic = "foobar"; - ros::init(argc, argv, "test"); - - ros::NodeHandle nh("~"); - ros::Publisher p = nh.advertise (topic, 1); - ros::Publisher bus_pub = nh.advertise ("/bus", 10); - - - signal(SIGINT, [](int i) { - ROS_WARN("SIGINT triggered"); - G_SIGINT_TRIGGERED = 1; - }); - while (ros::ok() && !G_SIGINT_TRIGGERED) { - - std::string inputString; - std::cout << "Give input> "; - std::getline(std::cin, inputString); - std_msgs::String msg; - - if(!inputString.empty()) - { - /** to test interfacing with the USB DAQ, use one of the following: - * hi: set outpin high - * lo: set outpin low - * pu###: pulse outpin for ###ms - * re: read analog on inpin - */ - msg.data = inputString; - p.publish(msg); - bus_pub.publish(msg); - } - - ros::spinOnce(); - if (G_SIGINT_TRIGGERED) {break;} - } - - return 0; -} \ No newline at end of file diff --git a/src/core/mcc_daq/src/daq_node.cpp b/src/core/mcc_daq/src/daq_node.cpp index 3b6fbc11..b38d3bae 100644 --- a/src/core/mcc_daq/src/daq_node.cpp +++ b/src/core/mcc_daq/src/daq_node.cpp @@ -1,19 +1,21 @@ #include #include #include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include "usb-2408.h" @@ -22,25 +24,19 @@ #include "utils.h" using std::shared_ptr; +using namespace std::chrono_literals; uint8_t G_INFO_VERBOSITY = 2; -static const bool TRUE = true; -/* - Hello CPP - */ -#include -#include +static rclcpp::Node::SharedPtr g_node; +static rclcpp::Time now_() { + return g_node->now(); +} -ros::Time calculate_edge(ros::Duration const & granularity, ros::Duration const & thresh, ros::Duration const & offset); -ros::Time calculate_edge(ros::Duration const & granularity, ros::Duration const & thresh); -ros::Time calculate_edge(ros::Duration const & granularity); -std::string to_string(ros::Time const &); -std::string to_string(ros::Duration const &); -std::string to_string(ros::Rate const &); -int64_t to_nano64(ros::Duration const &); - +rclcpp::Time calculate_edge(rclcpp::Duration const & granularity, rclcpp::Duration const & thresh, rclcpp::Duration const & offset); +std::string to_string(rclcpp::Time const &); +std::string to_string(rclcpp::Duration const &); /** =================== helper func ============================== */ @@ -84,68 +80,49 @@ bool is_simple_frac(double d) { return m < 1e-9; } -std::string to_string(ros::Time const & t) { - return std::to_string(t.toSec()); +std::string to_string(rclcpp::Time const & t) { + return std::to_string(t.seconds()); } -std::string to_string(ros::Duration const & t) { - return std::to_string(t.toSec()); -} -std::string to_string(ros::Rate const & t) { - return std::to_string(ros::Duration(t).toSec()); -} - -ros::Time calculate_edge(ros::Duration const & granularity) { - return calculate_edge(granularity, ros::Duration{0,0}, ros::Duration{0,0}); +std::string to_string(rclcpp::Duration const & t) { + return std::to_string(t.seconds()); } -ros::Time calculate_edge(ros::Duration const & granularity, ros::Duration const & thresh) { - return calculate_edge(granularity, thresh, ros::Duration{0,0}); -} - -ros::Time calculate_edge(ros::Duration const & granularity, ros::Duration const & thresh, ros::Duration const & offset) { - auto now = ros::Time::now(); - auto nowns = now.toNSec(); - auto dt = granularity.toNSec(); +rclcpp::Time calculate_edge(rclcpp::Duration const & granularity, rclcpp::Duration const & thresh, rclcpp::Duration const & offset) { + auto now = now_(); + auto nowns = now.nanoseconds(); + auto dt = granularity.nanoseconds(); auto m = nowns % dt; auto delay_ns = dt - m; - ros::Duration delay = ros::Duration().fromNSec(delay_ns) - offset; -// ROS_INFO("Now: %s", to_string(now).c_str()); -// ROS_INFO("dt: %s", to_string(granularity).c_str()); -// ROS_INFO("delay_ns: %ld", delay_ns); -// ROS_INFO("delaydur: %s", to_string(out).c_str()); + rclcpp::Duration delay = rclcpp::Duration(std::chrono::nanoseconds(delay_ns)) - offset; if (delay < thresh) { - delay = ros::Duration{0,0}; + delay = rclcpp::Duration(0, 0); } - ros::Time edge = now + delay; + rclcpp::Time edge = now + delay; return edge; - -} - -void chatterCallback(const std_msgs::String::ConstPtr& msg) { - ROS_INFO("I heard: [%s]", msg->data.c_str()); } -TriggerTimer::TriggerTimer(ros::NodeHandlePtr node_, UsbDaq &usbDaq_) { +TriggerTimer::TriggerTimer(rclcpp::Node::SharedPtr node_, UsbDaq &usbDaq_) + : last_call(node_->now()), last_edge(node_->now()), next_edge(node_->now()) { node = node_; usbDaq = usbDaq_; - srvTrigger = node->advertiseService("set_trigger_rate", &TriggerTimer::setTriggerRate, this); + srvTrigger = node->create_service( + "set_trigger_rate", + std::bind(&TriggerTimer::setTriggerRate, this, std::placeholders::_1, std::placeholders::_2)); } void TriggerTimer::set_trigger_run(bool state) { if (state) { ROS_INFO1("Turned on timer") -// timer.start(); trigger_is_running = true; - rate = ros::Rate(freq_set); + period_sec = 1.0 / freq_set; } else { ROS_INFO1("Turned off timer") -// timer.stop(); trigger_is_running = false; - rate = ros::Rate(quick_idle_freq); + period_sec = 1.0 / quick_idle_freq; } } -void TriggerTimer::set_trigger_run(const std_msgs::Bool::ConstPtr &msg) { +void TriggerTimer::set_trigger_run(const std_msgs::msg::Bool::ConstSharedPtr &msg) { set_trigger_run(msg->data); } @@ -153,21 +130,18 @@ void TriggerTimer::set_trigger_run(const std_msgs::Bool::ConstPtr &msg) { * If set to 0 or less, disable the timer. * @param duration */ -void TriggerTimer::set_trigger_period(ros::Duration duration) { - if (duration.toSec() > 0.0) { +void TriggerTimer::set_trigger_period(rclcpp::Duration duration) { + if (duration.seconds() > 0.0) { ROS_INFO1("Start timer") -// timer.setPeriod(duration); -// timer.start(); set_trigger_run(true); } else { ROS_INFO1("Stop timer") -// timer.stop(); set_trigger_run(false); } } void TriggerTimer::set_trigger_period(double t_seconds) { - set_trigger_period(ros::Duration(t_seconds)); + set_trigger_period(rclcpp::Duration::from_seconds(t_seconds)); } void TriggerTimer::set_trigger_freq(double frequency) { @@ -175,59 +149,50 @@ void TriggerTimer::set_trigger_freq(double frequency) { if (frequency > 0) { freq_set = frequency; - rate = ros::Rate(freq_set); + period_sec = 1.0 / freq_set; } else { - rate = ros::Rate(quick_idle_freq); + period_sec = 1.0 / quick_idle_freq; } } -void TriggerTimer::set_trigger_freq(const std_msgs::Float64::ConstPtr &msg) { +void TriggerTimer::set_trigger_freq(const std_msgs::msg::Float64::ConstSharedPtr &msg) { set_trigger_freq(msg->data); } -ros::Rate TriggerTimer::get_trigger_freq() { - return ros::Rate(rate); -} -ros::Duration TriggerTimer::get_trigger_dur() { - return ros::Duration(rate); +rclcpp::Duration TriggerTimer::get_trigger_dur() { + return rclcpp::Duration::from_seconds(period_sec); } bool TriggerTimer::is_running() { return trigger_is_running; } -//bool TriggerTimer::call() {} - -void TriggerTimer::nop(const ros::TimerEvent &event) { -} - -bool TriggerTimer::setTriggerRate(custom_msgs::SetTriggerRate::Request &req, - custom_msgs::SetTriggerRate::Response &resp) { - set_trigger_freq(req.rate); - resp.success = true; - return true; +void TriggerTimer::setTriggerRate(const std::shared_ptr req, + std::shared_ptr resp) { + set_trigger_freq(req->rate); + resp->success = true; } /// Compute the next timer edge based on the last set frequency -ros::Time TriggerTimer::get_next_edge() { +rclcpp::Time TriggerTimer::get_next_edge() { // todo: bounds checking here // todo: put behind debugging env variable auto dur = get_trigger_dur(); - ros::Time next_edge; - if (is_simple_frac(dur.toSec())) { + rclcpp::Time next_edge_; + if (is_simple_frac(dur.seconds())) { ROS_INFO("simple dur: %s", to_string(dur).c_str()); - next_edge = calculate_edge(dur, ros::Duration(0.01), ros::Duration(0)); + next_edge_ = calculate_edge(dur, rclcpp::Duration::from_seconds(0.01), rclcpp::Duration(0, 0)); } else { - next_edge = ros::Time::now() + dur; + next_edge_ = now_() + dur; ROS_WARN("complex dur: %s", to_string(dur).c_str()); } - return next_edge; + return next_edge_; } /// Advance the timer void TriggerTimer::next() { - auto now = ros::Time::now(); + auto now = now_(); last_edge = next_edge; next_edge = get_next_edge(); ROS_INFO("Last: %s Next: %s Last Call: %s Now: %s dt: %s", to_string(last_edge).c_str(), to_string(next_edge).c_str(), @@ -237,62 +202,73 @@ void TriggerTimer::next() { /// sleep until the next edge. this is mostly a convenience method. void TriggerTimer::sleep_until_edge(double granularity) { - auto next_edge = calculate_edge(ros::Duration(granularity), ros::Duration(0.01), ros::Duration(0.0001)); - auto till_next_edge = next_edge - ros::Time::now(); + auto edge = calculate_edge(rclcpp::Duration::from_seconds(granularity), + rclcpp::Duration::from_seconds(0.01), + rclcpp::Duration::from_seconds(0.0001)); + auto till_next_edge = edge - now_(); ROS_WARN("Next edge: %s", to_string(till_next_edge).c_str()); - till_next_edge.sleep(); + if (till_next_edge.nanoseconds() > 0) { + std::this_thread::sleep_for(std::chrono::nanoseconds(till_next_edge.nanoseconds())); + } } /// spin once then sleep until the next period starts void TriggerTimer::spin_then_sleep() { next(); - ros::spinOnce(); - auto till_next_edge = next_edge - ros::Time::now(); - till_next_edge.sleep(); + rclcpp::spin_some(node); + auto till_next_edge = next_edge - now_(); + if (till_next_edge.nanoseconds() > 0) { + std::this_thread::sleep_for(std::chrono::nanoseconds(till_next_edge.nanoseconds())); + } } -// -void TriggerTimer::sleep_next() { - -} /** =================== AsyncTriggerTimer ============================== */ -void OneShotManager::erase(boost::uuids::uuid i) { +void OneShotManager::erase(uint64_t i) { timer_map.erase(i); -// std::cout << "erasing: " << i << " sz: " << timer_map.size() <second->cancel(); + } + TimerEvent e; + e.current_real = nhp->now(); callback(e); erase(i); }; - ros::Timer tmp = nhp->createTimer(period, cb2, true, false); + rclcpp::TimerBase::SharedPtr tmp = nhp->create_wall_timer( + std::chrono::nanoseconds(period.nanoseconds()), cb2); timer_map.emplace(i, tmp); - tmp.start(); // safety here, need to ensure it's in the map before it pops return i; } -AsyncTriggerTimer::AsyncTriggerTimer(ros::NodeHandlePtr nhp, ros::Duration period, - ros::Duration min_period, ros::Duration max_period, +AsyncTriggerTimer::AsyncTriggerTimer(rclcpp::Node::SharedPtr nhp, rclcpp::Duration period, + rclcpp::Duration min_period, rclcpp::Duration max_period, int spoof_events, std::shared_ptr envoy) -: nhp{nhp}, period_{period}, min_period_{min_period}, max_period_{max_period}, spoof_events_{spoof_events}, envoy_{envoy}{ - spoof_evt_pub = nhp->advertise ("/event", 1); +: nhp{nhp}, last_call{nhp->now()}, next_expected{nhp->now()}, + period_{period}, min_period_{min_period}, max_period_{max_period}, + spoof_events_{spoof_events}, envoy_{envoy} { + spoof_evt_pub = nhp->create_publisher("/event", 1); } -AsyncTriggerTimer::AsyncTriggerTimer(ros::NodeHandlePtr nhp, ros::Duration period) -: nhp{nhp}, period_{period} {} +AsyncTriggerTimer::AsyncTriggerTimer(rclcpp::Node::SharedPtr nhp, rclcpp::Duration period) +: nhp{nhp}, last_call{nhp->now()}, next_expected{nhp->now()}, period_{period} {} void AsyncTriggerTimer::start() { - callTick(ros::TimerEvent{}); + TimerEvent e; + e.current_real = nhp->now(); + callTick(e); } -void AsyncTriggerTimer::setPeriod(const ros::Duration &period) { +void AsyncTriggerTimer::setPeriod(const rclcpp::Duration &period) { if (period == period_) return; if (period < min_period_) { period_ = min_period_; @@ -304,94 +280,95 @@ void AsyncTriggerTimer::setPeriod(const ros::Duration &period) { } void AsyncTriggerTimer::setRate(double rate) { - auto period = ros::Duration(ros::Rate(rate)); - setPeriod(period); + setPeriod(rclcpp::Duration::from_seconds(1.0 / rate)); } -void AsyncTriggerTimer::cb_setPeriod(const std_msgs::Float64::ConstPtr &msg) { +void AsyncTriggerTimer::cb_setPeriod(const std_msgs::msg::Float64::ConstSharedPtr &msg) { ROS_INFO("& Got double: %lf, set period", msg->data); - setPeriod(ros::Duration(msg->data)); + setPeriod(rclcpp::Duration::from_seconds(msg->data)); } -void AsyncTriggerTimer::cb_setRate(const std_msgs::Float64::ConstPtr &msg) { +void AsyncTriggerTimer::cb_setRate(const std_msgs::msg::Float64::ConstSharedPtr &msg) { ROS_INFO("& Got double: %lf, set rate", msg->data); setRate(msg->data); } void AsyncTriggerTimer::call() { - ros::TimerEvent event; - event.current_real = ros::Time::now(); + TimerEvent event; + event.current_real = nhp->now(); call(event); } /// Call the bound callback -void AsyncTriggerTimer::call(const ros::TimerEvent &e) { - auto now = ros::Time::now(); - ROS_INFO("! AdjT RealDT( %lf )", (now - last_call).toSec()); +void AsyncTriggerTimer::call(const TimerEvent &e) { + auto now = nhp->now(); + ROS_INFO("! AdjT RealDT( %lf )", (now - last_call).seconds()); spoof_events_ = get_redis_int(envoy_, "/debug/spoof_events"); if (spoof_events_ == 1) { - // We're going to spoof a GSOF_EVT, so we don't depend on the + // We're going to spoof a GsofEvt, so we don't depend on the // INS always having a good sync to test the system - ROS_WARN("Spoofing GSOF_EVT message!"); - custom_msgs::GSOF_EVT msg; - msg.gps_time = now + ros::Duration(1e-4); // Add some small amount of noise to differ gps from sys + ROS_WARN("Spoofing GsofEvt message!"); + custom_msgs::msg::GsofEvt msg; + auto gps_time = now + rclcpp::Duration::from_seconds(1e-4); // Add some small amount of noise to differ gps from sys + msg.gps_time = gps_time; msg.sys_time = now; - msg.time = now.toSec(); - msg.header.stamp = now + ros::Duration(1e-4); // Header should match gps time - spoof_evt_pub.publish(msg); + msg.time = now.seconds(); + msg.header.stamp = gps_time; // Header should match gps time + spoof_evt_pub->publish(msg); + } + if (callback) { + callback(e); } - callback(e); last_call = now; } /// Bind a callback -void AsyncTriggerTimer::setCallback(const ros::TimerCallback& callback_) { +void AsyncTriggerTimer::setCallback(const TimerCallback& callback_) { callback = callback_; } /// This runs the mutually recursive loop. Enqueue the next event, then /// call the callback -void AsyncTriggerTimer::callTick(const ros::TimerEvent &e) { +void AsyncTriggerTimer::callTick(const TimerEvent &e) { std::lock_guard guard(mutex); /// "now" is actually event.current_real -// ROS_INFO("%d AdjT %s dt: %lf period: %3.2lf Evt: %s",i++, isonow().c_str(), (e.last_real - last).toSec(), period_.toSec(), to_string(e).c_str()); next_expected = e.current_real + period_; - ros::Duration nextPeriod = next_expected - ros::Time::now(); -// ROS_INFO("now: %lf last: %lf next: %lf dur: %lf", e.last_real.toSec(), last.toSec(), nextExpected.toSec(), dur.toSec()); - if (ros::ok()) { -// ROS_INFO("enqueuing next callTick"); - ros::TimerCallback nextCycle = boost::bind(&AsyncTriggerTimer::callTick, this, _1); + if (rclcpp::ok()) { + TimerCallback nextCycle = [this](const TimerEvent &ev) { callTick(ev); }; osm.addOneShot(nhp, period_, nextCycle); } call(e); } -ros::Rate AsyncTriggerTimer::get_trigger_freq() { - return ros::Rate(period_); -} -ros::Duration AsyncTriggerTimer::get_trigger_dur() { - return ros::Duration(period_); +rclcpp::Duration AsyncTriggerTimer::get_trigger_dur() { + return period_; } /** =================== daq wrapper ============================== */ -DaqWrapper::DaqWrapper(ros::NodeHandlePtr node_, UsbDaq &usbDaq_) { +DaqWrapper::DaqWrapper(rclcpp::Node::SharedPtr node_, UsbDaq &usbDaq_) { node = node_; usbDaq = usbDaq_; + readPinSrv = node->create_service( + "/daq/read_pin", + std::bind(&DaqWrapper::readPin, this, std::placeholders::_1, std::placeholders::_2)); } -bool DaqWrapper::readPin(custom_msgs::ReadPinRequest &req, - custom_msgs::ReadPinResponse &rsp) { - ROS_INFO("Starting to read on pin %d", req.pin); - rsp.value = usbDaq.voltageRead((uint8_t) req.pin, BP_10V); - return true; +void DaqWrapper::readPin(const std::shared_ptr req, + std::shared_ptr rsp) { + ROS_INFO("Starting to read on pin %d", req->pin); + rsp->value = usbDaq.voltageRead((uint8_t) req->pin, BP_10V); } /** =================== main ============================== */ int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared("daq_node"); + g_node = node; + RedisEnvoyOpts envoy_opts = RedisEnvoyOpts::from_env("daq" ); auto envoy_ = std::make_shared(envoy_opts); ROS_INFO("echo: %s", envoy_->echo("Redis connected").c_str()); @@ -399,7 +376,7 @@ int main(int argc, char** argv) { int debug = 0; int trigger_pps = 0; - // Enables publishing a GSOF_EVT on each pulse + // Enables publishing a GsofEvt on each pulse // (not reliant on INS having a good sync) int spoof_events = 0; try { @@ -413,15 +390,9 @@ int main(int argc, char** argv) { std::string node_ns = "/daq"; std::string topic = "chatter"; - double spoof_rate = 0; - double spoof_daq = false; - ros::init(argc, argv, "daq_node"); -// ros::NodeHandle nh; - ros::NodeHandle nh; - ros::NodeHandlePtr node = boost::make_shared(nh); - ros::Publisher bus_pub = node->advertise ("/bus", 10); - ros::Publisher stat_pub = node->advertise("/stat", 3); - ros::Publisher trig_pub = node->advertise("/trig", 3); + auto bus_pub = node->create_publisher("/bus", 10); + auto stat_pub = node->create_publisher("/stat", 3); + auto trig_pub = node->create_publisher("/trig", 3); double min_period = 0.1; double max_period = 10.0; @@ -436,32 +407,28 @@ int main(int argc, char** argv) { } } - bool start_running = true; // Trigger starts with node start - bool dummy_mode = false; // Use the dummy daq code - node->param("start_running", start_running, TRUE); - node->param("dummy", dummy_mode, false); - ros::Duration(0, 500000000).sleep(); // sleep briefly to give the publisher time to catch up - ros::param::get("/spoof_rate", spoof_rate); - ros::param::get("/spoof_daq", spoof_daq); + bool start_running = node->declare_parameter("start_running", true); // Trigger starts with node start + node->declare_parameter("dummy", false); // Use the dummy daq code (currently unused) + double spoof_rate = 0.0; + { + const char *env_spoof = std::getenv("SPOOF_RATE"); + if (env_spoof && env_spoof[0]) { + spoof_rate = std::stof(std::string(env_spoof)); + } + } if (spoof_rate > 0) { ROS_WARN("\nGoing into spoof mode \n"); - ros::spin(); + rclcpp::spin(node); return 0; } -// std::shared_ptr daq; -// if (dummy_mode) { -// daq = std::shared_ptr(node); -// } else { -// daq = new UsbDaq(node); -// } auto *daq = new UsbDaq(node); TriggerTimer triggerTimer = TriggerTimer(node, *daq); auto asyncTriggerTimerP = std::make_shared(node, - ros::Duration(1.0), - ros::Duration(min_period), - ros::Duration(max_period), + rclcpp::Duration::from_seconds(1.0), + rclcpp::Duration::from_seconds(min_period), + rclcpp::Duration::from_seconds(max_period), spoof_events, envoy_); DaqWrapper daqWrapper = DaqWrapper(node, *daq); @@ -469,27 +436,35 @@ int main(int argc, char** argv) { daq->digitalPulse(); - std_msgs::String msg; + std_msgs::msg::String msg; msg.data = "~~~~~ DAQ node going online ~~~~~"; - bus_pub.publish(msg); - -// ros::Subscriber sub = node->subscribe(node_ns+"/"+topic, 10, chatterCallback); - ros::Subscriber sub_blip_period = node->subscribe(node_ns+"/blip_period", 10, &UsbDaq::set_blip_micros, daq); - ros::Subscriber sub_blipper = node->subscribe(node_ns+"/"+topic, 10, &UsbDaq::switchboard, daq); - ros::Subscriber sub_pulser = node->subscribe(node_ns+"/pulse", 10, &UsbDaq::pulse, daq); - ros::Subscriber sub_trigger_freq = node->subscribe(node_ns+"/trigger_freq", 10, - &AsyncTriggerTimer::cb_setRate, &(*asyncTriggerTimerP)); - ros::Subscriber sub_trigger_run = node->subscribe(node_ns+"/trigger_run", 10, - &TriggerTimer::set_trigger_run, &triggerTimer); - ros::ServiceServer read_pin_srv_ = node->advertiseService(node_ns+"/read_pin", - &DaqWrapper::readPin, &daqWrapper) ; - - double trigger_freq = 0.5; - node->param("/trigger_freq", trigger_freq, 0.5); - - - auto nodeName = ros::this_node::getName(); - custom_msgs::Stat stat_msg; + bus_pub->publish(msg); + + auto sub_blip_period = node->create_subscription( + node_ns + "/blip_period", 10, + [daq](const std_msgs::msg::UInt32::ConstSharedPtr m) { daq->set_blip_micros(m); }); + auto sub_blipper = node->create_subscription( + node_ns + "/" + topic, 10, + [daq](const std_msgs::msg::String::ConstSharedPtr m) { daq->switchboard(m); }); + auto sub_pulser = node->create_subscription( + node_ns + "/pulse", 10, + [daq](const std_msgs::msg::UInt32::ConstSharedPtr m) { daq->pulse(m); }); + auto sub_trigger_freq = node->create_subscription( + node_ns + "/trigger_freq", 10, + [asyncTriggerTimerP](const std_msgs::msg::Float64::ConstSharedPtr m) { + asyncTriggerTimerP->cb_setRate(m); + }); + auto sub_trigger_run = node->create_subscription( + node_ns + "/trigger_run", 10, + [&triggerTimer](const std_msgs::msg::Bool::ConstSharedPtr m) { + triggerTimer.set_trigger_run(m); + }); + + double trigger_freq = node->declare_parameter("trigger_freq", 0.5); + + + auto nodeName = std::string(node->get_fully_qualified_name()); + custom_msgs::msg::Stat stat_msg; stat_msg.node = nodeName; stat_msg.trace_topic = nodeName + "/blip"; @@ -497,14 +472,12 @@ int main(int argc, char** argv) { triggerTimer.set_trigger_run(start_running); triggerTimer.set_trigger_freq(trigger_freq); - ros::Time next_edge; - ros::Duration till_next_edge; triggerTimer.sleep_until_edge(1.0); - asyncTriggerTimerP->setCallback([daq, envoy_, asyncTriggerTimerP, trig_pub](const ros::TimerEvent &event) { - std_msgs::Header header; - header.stamp = ros::Time::now(); - trig_pub.publish(header); + asyncTriggerTimerP->setCallback([daq, envoy_, asyncTriggerTimerP, trig_pub, node](const TimerEvent &event) { + std_msgs::msg::Header header; + header.stamp = node->now(); + trig_pub->publish(header); daq->blip(); ROS_DEBUG("blip"); ROS_INFO("checking redis"); @@ -522,13 +495,12 @@ int main(int argc, char** argv) { ROS_WARN("PPS enabled"); triggerTimer.set_trigger_freq(1.0); triggerTimer.sleep_until_edge(1.0); - while (ros::ok()) { - // ros::spinOnce(); + while (rclcpp::ok()) { if (triggerTimer.is_running()) { - std_msgs::Header header; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = node->now(); header.frame_id = "1.0"; - trig_pub.publish(header); + trig_pub->publish(header); daq->blip(); } else { if (G_INFO_VERBOSITY > 3) { @@ -547,12 +519,11 @@ int main(int argc, char** argv) { ROS_INFO("starting trigger seq"); - ros::AsyncSpinner spinner{0}; + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); asyncTriggerTimerP->start(); - spinner.start(); - ros::waitForShutdown(); + executor.spin(); + rclcpp::shutdown(); return 0; - /// DEPRECATED - } diff --git a/src/core/mcc_daq/src/daq_node.h b/src/core/mcc_daq/src/daq_node.h index 686f18a2..0fb60863 100644 --- a/src/core/mcc_daq/src/daq_node.h +++ b/src/core/mcc_daq/src/daq_node.h @@ -3,14 +3,17 @@ #include #include -#include -#include -#include -#include +#include +#include + +#include +#include +#include #include -#include -#include +#include +#include +#include #include "usbdaq.h" @@ -18,160 +21,134 @@ using std::shared_ptr; bool is_simple_frac(double d); +/// Minimal stand-in for ros::TimerEvent (rclcpp timers have void callbacks) +struct TimerEvent { + rclcpp::Time current_real; +}; + +using TimerCallback = std::function; + class TriggerTimer { private: - ros::NodeHandlePtr node; + rclcpp::Node::SharedPtr node; UsbDaq usbDaq; - ros::Publisher bus_pub; - ros::ServiceServer srvTrigger; + rclcpp::Service::SharedPtr srvTrigger; bool trigger_is_running = false; const double default_freq = 0.5; const double quick_idle_freq = 10; double freq_set = default_freq; - ros::Timer timer; - ros::Time last_call = ros::Time::now(); - ros::Time last_edge = ros::Time::now(); - ros::Time next_edge = ros::Time::now(); - ros::Duration last_duration = ros::Duration(0.5); - int flippy = 0; + rclcpp::Time last_call; + rclcpp::Time last_edge; + rclcpp::Time next_edge; public: - TriggerTimer(ros::NodeHandlePtr node_, UsbDaq &usbDaq1); + TriggerTimer(rclcpp::Node::SharedPtr node_, UsbDaq &usbDaq1); bool is_running(); - bool call(); - void set_trigger_run(bool state); - void set_trigger_run(const std_msgs::Bool::ConstPtr &msg); + void set_trigger_run(const std_msgs::msg::Bool::ConstSharedPtr &msg); void set_trigger_freq(double frequency); - void set_trigger_freq(const std_msgs::Float64::ConstPtr &msg); - ros::Rate get_trigger_freq(); - ros::Duration get_trigger_dur(); + void set_trigger_freq(const std_msgs::msg::Float64::ConstSharedPtr &msg); + rclcpp::Duration get_trigger_dur(); void set_trigger_period(double t_seconds); - void set_trigger_period(ros::Duration duration); - - void trigger_tic(const ros::TimerEvent &event); + void set_trigger_period(rclcpp::Duration duration); - bool setTriggerRate(custom_msgs::SetTriggerRate::Request &req, - custom_msgs::SetTriggerRate::Response &resp); + void setTriggerRate(const std::shared_ptr req, + std::shared_ptr resp); - void nop(const ros::TimerEvent &event); - ros::Time get_next_edge(); + rclcpp::Time get_next_edge(); void next(); - void sleep(); void spin_then_sleep(); void sleep_until_edge(double granularity); - void sleep_next(); - - ros::Rate rate = ros::Rate(quick_idle_freq); + /// current period, seconds + double period_sec = 1.0 / 10; }; -void cb_print(const ros::TimerEvent &e) { - ROS_INFO("default callback"); -} - class OneShotManager { public: OneShotManager() = default; - void erase(boost::uuids::uuid i); + void erase(uint64_t i); - boost::uuids::uuid addOneShot(ros::NodeHandlePtr nhp, - const ros::Duration &period, - const ros::TimerCallback& callback); + uint64_t addOneShot(rclcpp::Node::SharedPtr nhp, + const rclcpp::Duration &period, + const TimerCallback& callback); private: - std::map timer_map; + uint64_t next_id_ = 0; + std::map timer_map; }; class AsyncTriggerTimer { private: - ros::NodeHandlePtr nhp; + rclcpp::Node::SharedPtr nhp; std::mutex mutex; - ros::Publisher bus_pub; - ros::Publisher spoof_evt_pub; - ros::ServiceServer srvTrigger; - bool trigger_is_running = false; - const double default_freq = 0.5; - ros::Time last_call = ros::Time::now(); - ros::Time next_expected = ros::Time::now(); - ros::Duration last_duration = ros::Duration(0.5); - ros::Duration period_ = ros::Duration(1); /// this is the new main variable - ros::Duration min_period_ = ros::Duration(0.1); /// minimum time between triggers - ros::Duration max_period_ = ros::Duration(10.0); /// minimum time between triggers + rclcpp::Publisher::SharedPtr spoof_evt_pub; + rclcpp::Time last_call; + rclcpp::Time next_expected; + rclcpp::Duration period_ = rclcpp::Duration::from_seconds(1); /// this is the new main variable + rclcpp::Duration min_period_ = rclcpp::Duration::from_seconds(0.1); /// minimum time between triggers + rclcpp::Duration max_period_ = rclcpp::Duration::from_seconds(10.0); /// maximum time between triggers int spoof_events_ = 0; std::shared_ptr envoy_; - int flippy = 0; - ros::TimerCallback callback{cb_print}; + TimerCallback callback; OneShotManager osm; public: - AsyncTriggerTimer(ros::NodeHandlePtr nhp, ros::Duration period, - ros::Duration min_period, ros::Duration max_period, + AsyncTriggerTimer(rclcpp::Node::SharedPtr nhp, rclcpp::Duration period, + rclcpp::Duration min_period, rclcpp::Duration max_period, int spoof_events, std::shared_ptr envoy); - AsyncTriggerTimer(ros::NodeHandlePtr nhp, ros::Duration period); - bool is_running(); + AsyncTriggerTimer(rclcpp::Node::SharedPtr nhp, rclcpp::Duration period); void start(); - void set_trigger_run(bool state); - void cb_set_trigger_run(const std_msgs::Bool::ConstPtr &msg); /** This is merely a convenience wrapper around setPeriod * * @param frequency - Set the trigger frequency */ void setRate(double frequency); - void cb_setRate(const std_msgs::Float64::ConstPtr &msg); + void cb_setRate(const std_msgs::msg::Float64::ConstSharedPtr &msg); /** All timing sets should happen through here * * @param period - Set the trigger period, clipping to the min/max period */ - void setPeriod(const ros::Duration &period); - void cb_setPeriod(const std_msgs::Float64::ConstPtr &msg); + void setPeriod(const rclcpp::Duration &period); + void cb_setPeriod(const std_msgs::msg::Float64::ConstSharedPtr &msg); - void setCallback(const ros::TimerCallback &callback_); + void setCallback(const TimerCallback &callback_); - void callTick(const ros::TimerEvent &event); + void callTick(const TimerEvent &event); void call(); - void call(const ros::TimerEvent &event); - - bool setTriggerRate(custom_msgs::SetTriggerRate::Request &req, - custom_msgs::SetTriggerRate::Response &resp); + void call(const TimerEvent &event); - ros::Rate get_trigger_freq(); - ros::Duration get_trigger_dur(); + rclcpp::Duration get_trigger_dur(); }; class DaqWrapper { private: - ros::NodeHandlePtr node; + rclcpp::Node::SharedPtr node; UsbDaq usbDaq; - ros::Publisher bus_pub; - ros::ServiceServer srvTrigger; + rclcpp::Service::SharedPtr readPinSrv; public: - DaqWrapper(ros::NodeHandlePtr node_, UsbDaq &usbDaq1); - - bool call(); + DaqWrapper(rclcpp::Node::SharedPtr node_, UsbDaq &usbDaq1); - bool readPin( custom_msgs::ReadPinRequest &req, - custom_msgs::ReadPinResponse &rsp); - bool analogWrite( custom_msgs::ReadPinRequest &req, - custom_msgs::ReadPinResponse &rsp); + void readPin(const std::shared_ptr req, + std::shared_ptr rsp); }; diff --git a/src/core/mcc_daq/src/usbdaq.cpp b/src/core/mcc_daq/src/usbdaq.cpp index fc105601..4ac5a208 100644 --- a/src/core/mcc_daq/src/usbdaq.cpp +++ b/src/core/mcc_daq/src/usbdaq.cpp @@ -3,8 +3,8 @@ #include #include -#include -#include "std_msgs/String.h" +#include +#include "std_msgs/msg/string.hpp" #include "pmd.h" #include "usb-2408.h" @@ -12,133 +12,16 @@ #include "usbdaq.h" -// === === === === === === === === === === === -/** Dummy DAQ for testing without hardware */ -UsbDaqDummy::UsbDaqDummy() {} - -UsbDaqDummy::UsbDaqDummy(ros::NodeHandlePtr &_nh) { - node = _nh; - bus_pub = node->advertise ("/bus", 10); - std_msgs::String msg; - msg.data = "DAQ device going online"; - bus_pub.publish(msg); -} - -int UsbDaqDummy::init() { - int libusb_err = libusb_init(nullptr); - if (libusb_err < 0) { - throw LibUSBError(); - } - ROS_GREEN("Success, found a dummy USB 2408!"); - - buildGainTables(); -} - -void UsbDaqDummy::buildGainTables() { - usbBuildGainTable_USB2408(udev, gain_table_AIN); - ROS_GREEN("Built gain tables"); -} - -void UsbDaqDummy::set_blip_micros(uint32_t period) { - blip_on_micros = period; -} - -void UsbDaqDummy::set_blip_micros(const std_msgs::UInt32_>::ConstPtr &msg) { - set_blip_micros(msg->data); -} - -/** Write a value to a specific pin */ -void UsbDaqDummy::digitalWrite(uint8_t pin, uint8_t value) { - ROS_INFO3("Pin: %d Value: %d", pin, value); -} - -/** Toggle a pin briefly */ -const void UsbDaqDummy::blip() { - uint8_t pin = 0; - digitalWrite(pin, HIGH); - usleep(blip_on_micros); - digitalWrite(pin, LOW); -} - -/** Toggle a pin for time_us microseconds */ -void UsbDaqDummy::digitalPulse(uint8_t pin, uint32_t time_us){ - ROS_INFO1("Pulsing for %d us", time_us); - digitalWrite(pin, HIGH); - usleep(time_us); - digitalWrite(pin, LOW); -} - -void UsbDaqDummy::digitalPulse() { - digitalPulse(outpin, 250000); -} - -void UsbDaqDummy::pulse(const std_msgs::UInt32::ConstPtr &msg) { - ROS_INFO3("Pulse: [%d]", msg->data); - digitalPulse(outpin, msg->data); -} - - -void UsbDaqDummy::switchboard(const std_msgs::String::ConstPtr &msg) { - ROS_INFO3("I heard: [%s]", msg->data.c_str()); - std::string prefix = msg->data.substr(0, 2); - if (prefix == "hi") { - digitalWrite(outpin, HIGH); - } else if (prefix == "lo") { - digitalWrite(outpin, LOW); - } else if (prefix == "pu") { - int time_ms = 0; - try { - time_ms = std::stoi(msg->data.substr(2)); - } - catch (std::invalid_argument) { ROS_WARN("Could not parse that"); } - if (time_ms) { - digitalPulse(outpin, time_ms); - } - - } else if (prefix == "re") { - uint8_t chan = inpin; - int data = analogRead(chan); - std::string volt_str = std::to_string(data); - // Publishing broken until I figure out how to bind the publisher without getting compiler errors -// analog_pub.publish(volt_str); - ROS_INFO("Voltage: %d [%s]", data, volt_str.c_str()); - ROS_GREEN("Voltage"); - } -} - -int UsbDaqDummy::analogRead(uint8_t channel) { - uint8_t range, rate, mode; - range = BP_5V; - rate = HZ1000; - mode = DIFFERENTIAL; - return analogRead(channel, mode, range, rate); -} - -int UsbDaqDummy::analogRead(uint8_t channel, uint8_t mode, uint8_t range, uint8_t rate) { - int gain = 2; - int data = 1; - data = data*range; -// voltage = volts_USB2408(gain, data); - return data; -} - -double UsbDaqDummy::voltageRead(uint8_t channel, uint8_t range) { - // todo: implement gain correction -} - -// === === === === === === === === === === === - UsbDaq::UsbDaq() { // I have no idea why I need this. } -// todo: figure out proper way to set up nodeHandle references -UsbDaq::UsbDaq(ros::NodeHandlePtr &_nh) { - node = _nh; - bus_pub = node->advertise ("/bus", 10); - std_msgs::String msg; +UsbDaq::UsbDaq(rclcpp::Node::SharedPtr nh) { + node = nh; + bus_pub = node->create_publisher("/bus", 10); + std_msgs::msg::String msg; msg.data = "DAQ device going online"; - bus_pub.publish(msg); + bus_pub->publish(msg); } int UsbDaq::init() { @@ -146,9 +29,7 @@ int UsbDaq::init() { if (libusb_err < 0) { throw LibUSBError(); } - std_msgs::String msg; -// msg.data = "hiiiiiiiiiiiiiiiiii"; -// bus_pub.publish(msg); + std_msgs::msg::String msg; if ((udev = usb_device_find_USB_MCC(USB2408_PID, nullptr))) { @@ -162,11 +43,12 @@ int UsbDaq::init() { } else { ROS_ERROR("Failure, did not find a USB 2408 or 2408_2AO!\n"); msg.data = "Failure, did not find a USB 2408 or 2408_2AO!"; - bus_pub.publish(msg); + bus_pub->publish(msg); throw DeviceNotFoundError(); } - bus_pub.publish(msg); + bus_pub->publish(msg); buildGainTables(); + return 0; } /** Builds a lookup table of calibration coefficents to translate values into voltages. @@ -220,16 +102,14 @@ int UsbDaq::analogRead(uint8_t channel) { } int UsbDaq::analogRead(uint8_t channel, uint8_t mode, uint8_t range, uint8_t rate) { - int gain = 2; int data = usbAIn_USB2408(udev, channel, mode, range, rate, &flags); data = data*range; -// voltage = volts_USB2408(gain, data); return data; } int UsbDaq::analogWrite(uint8_t channel, double voltage, double table_AO[NCHAN_AO_2408][2]){ usbAOut_USB2408_2AO(udev, channel, voltage, table_AO); - + return 0; } /** @@ -246,7 +126,6 @@ double UsbDaq::voltageRead(uint8_t channel, uint8_t gain) { mode = DIFFERENTIAL; ROS_INFO("Reading device"); -// int data = usbAIn_USB2408(udev, channel, mode, gain, rate, &flags); int data = usbAIn_USB2408(udev, channel, mode, range, rate, &flags); // I have no idea why the original code does this janky cast @@ -262,7 +141,7 @@ double UsbDaq::voltageRead(uint8_t channel, uint8_t gain) { * * @param msg Uint32 Microseconds to turn on for */ -void UsbDaq::pulse(const std_msgs::UInt32::ConstPtr &msg) { +void UsbDaq::pulse(const std_msgs::msg::UInt32::ConstSharedPtr &msg) { ROS_INFO3("Pulse: [%d]", msg->data); digitalPulse(outpin, msg->data); } @@ -274,7 +153,7 @@ void UsbDaq::pulse(const std_msgs::UInt32::ConstPtr &msg) { * re: read analog on inpin * @param msg */ -void UsbDaq::switchboard(const std_msgs::String::ConstPtr &msg) { +void UsbDaq::switchboard(const std_msgs::msg::String::ConstSharedPtr &msg) { ROS_INFO3("I heard: [%s]", msg->data.c_str()); std::string prefix = msg->data.substr(0, 2); if (prefix == "hi") { @@ -286,7 +165,7 @@ void UsbDaq::switchboard(const std_msgs::String::ConstPtr &msg) { try { time_ms = std::stoi(msg->data.substr(2)); } - catch (std::invalid_argument) { ROS_WARN("Could not parse that"); } + catch (const std::invalid_argument &) { ROS_WARN("Could not parse that"); } if (time_ms) { digitalPulse(outpin, time_ms); } @@ -295,8 +174,6 @@ void UsbDaq::switchboard(const std_msgs::String::ConstPtr &msg) { uint8_t chan = inpin; int data = analogRead(chan); std::string volt_str = std::to_string(data); - // Publishing broken until I figure out how to bind the publisher without getting compiler errors -// analog_pub.publish(volt_str); ROS_INFO("Voltage: %d [%s]", data, volt_str.c_str()); ROS_GREEN("Voltage"); } @@ -312,16 +189,10 @@ void UsbDaq::bind_callback(void (*vfn)() ) { routine = vfn; } -void UsbDaq::bind_publisher(ros::NodeHandle &nh) { -// std::string topic = "analog_out"; -// analog_pub = nh.advertise (topic, 1); - -} - void UsbDaq::set_blip_micros(uint32_t period) { blip_on_micros = period; } -void UsbDaq::set_blip_micros(const std_msgs::UInt32_>::ConstPtr &msg) { +void UsbDaq::set_blip_micros(const std_msgs::msg::UInt32::ConstSharedPtr &msg) { set_blip_micros(msg->data); } diff --git a/src/core/mcc_daq/src/usbdaq.h b/src/core/mcc_daq/src/usbdaq.h index 13c2c09c..f3f25014 100644 --- a/src/core/mcc_daq/src/usbdaq.h +++ b/src/core/mcc_daq/src/usbdaq.h @@ -2,9 +2,9 @@ #define MCC_DAQ_USBDAQ_H #include -#include -#include "std_msgs/String.h" -#include "std_msgs/UInt32.h" +#include +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/u_int32.hpp" #include "usb-2408.h" @@ -13,78 +13,13 @@ enum MccDeviceType {MCC_UNDEF=0, MCC_USB2408, MCC_USB2408_AO}; -class UsbDaqBase { -public: - virtual int init() = 0; - virtual void buildGainTables() = 0; - virtual const void blip() = 0; - virtual void pulse(const std_msgs::UInt32::ConstPtr& msg) = 0; - virtual void switchboard(const std_msgs::String::ConstPtr& msg) = 0; - virtual void set_blip_micros(const std_msgs::UInt32::ConstPtr &msg) = 0; - virtual void set_blip_micros(uint32_t period) = 0; - virtual int analogRead( uint8_t channel) = 0; - virtual int analogRead( uint8_t channel, uint8_t mode, uint8_t range, uint8_t rate) = 0; - virtual double voltageRead( uint8_t channel, uint8_t range) = 0; - virtual void digitalWrite(uint8_t pin, uint8_t value) = 0; - virtual void digitalPulse(uint8_t pin, uint32_t time_us) = 0; - virtual void digitalPulse() = 0; - -}; - - -class UsbDaqDummy{ -//class UsbDaqDummy: public UsbDaqBase { -private: - libusb_device_handle *udev = nullptr; - ros::NodeHandlePtr node; -// ros::NodeHandle nh_dummy(); - MccDeviceType device_type = MCC_UNDEF; - // I have no idea what I'm doing with these silly handles. - ros::Publisher analog_pub ;//= nh_dummy.advertise ("qux", 1); // - ros::Publisher bus_pub ; - - void (*routine)(); - uint8_t outpin = 0; - uint8_t inpin = 4; - uint8_t flags = 0; - uint32_t blip_on_micros = 50000; // on-time of blip function, microseconds - - double gain_table_AIN[NGAINS_2408][2]; - -public: - UsbDaqDummy(void); - explicit UsbDaqDummy(ros::NodeHandlePtr &nh); - int init(); - void buildGainTables(); - - const void blip(); - void pulse(const std_msgs::UInt32::ConstPtr& msg); - void switchboard(const std_msgs::String::ConstPtr& msg); - - void set_blip_micros(const std_msgs::UInt32::ConstPtr &msg); - void set_blip_micros(uint32_t period); - - - int analogRead( uint8_t channel); - int analogRead( uint8_t channel, uint8_t mode, uint8_t range, uint8_t rate); - double voltageRead( uint8_t channel, uint8_t range); - void digitalWrite(uint8_t pin, uint8_t value); - void digitalPulse(uint8_t pin, uint32_t time_us); - void digitalPulse(); - -}; - class UsbDaq{ -//class UsbDaq: public UsbDaqBase { private: libusb_device_handle *udev = nullptr; - ros::NodeHandlePtr node; -// ros::NodeHandle nh_dummy(); + rclcpp::Node::SharedPtr node; MccDeviceType device_type = MCC_UNDEF; - // I have no idea what I'm doing with these silly handles. - ros::Publisher analog_pub ;//= nh_dummy.advertise ("qux", 1); // - ros::Publisher bus_pub ; + rclcpp::Publisher::SharedPtr bus_pub; void (*routine)(); uint8_t outpin = 0; @@ -96,16 +31,16 @@ class UsbDaq{ public: UsbDaq(void); - explicit UsbDaq(ros::NodeHandlePtr &nh); + explicit UsbDaq(rclcpp::Node::SharedPtr nh); int init(); void buildGainTables(); void blink(uint8_t count=3); const void blip(); - void pulse(const std_msgs::UInt32::ConstPtr& msg); - void switchboard(const std_msgs::String::ConstPtr& msg); + void pulse(const std_msgs::msg::UInt32::ConstSharedPtr& msg); + void switchboard(const std_msgs::msg::String::ConstSharedPtr& msg); - void set_blip_micros(const std_msgs::UInt32::ConstPtr &msg); + void set_blip_micros(const std_msgs::msg::UInt32::ConstSharedPtr &msg); void set_blip_micros(uint32_t period); void call(); @@ -118,8 +53,6 @@ class UsbDaq{ void digitalPulse(uint8_t pin, uint32_t time_us); void digitalPulse(); void bind_callback(void (*vfn)()); - - void bind_publisher(ros::NodeHandle &nh); }; diff --git a/src/core/mcc_daq/src/utils.h b/src/core/mcc_daq/src/utils.h index b48021a3..cfb64080 100644 --- a/src/core/mcc_daq/src/utils.h +++ b/src/core/mcc_daq/src/utils.h @@ -1,7 +1,8 @@ #ifndef MCC_DAQ_UTILS_H #define MCC_DAQ_UTILS_H -#include +#include +#include extern uint8_t G_INFO_VERBOSITY; @@ -13,6 +14,13 @@ extern uint8_t G_INFO_VERBOSITY; // No Color #define NC "\033[0m" +// rclcpp logging shims so the hardware code reads the same as before +#define ROS_INFO(...) RCLCPP_INFO(rclcpp::get_logger("mcc_daq"), __VA_ARGS__) +#define ROS_WARN(...) RCLCPP_WARN(rclcpp::get_logger("mcc_daq"), __VA_ARGS__) +#define ROS_ERROR(...) RCLCPP_ERROR(rclcpp::get_logger("mcc_daq"), __VA_ARGS__) +#define ROS_DEBUG(...) RCLCPP_DEBUG(rclcpp::get_logger("mcc_daq"), __VA_ARGS__) +#define ROS_INFO_STREAM(args) RCLCPP_INFO_STREAM(rclcpp::get_logger("mcc_daq"), args) + #define ROS_GREEN(mystr) ROS_INFO(GRN mystr NC) #define ROS_INFO1(...) if (G_INFO_VERBOSITY >= 1) {ROS_INFO(__VA_ARGS__);} #define ROS_INFO2(...) if (G_INFO_VERBOSITY >= 2) {ROS_INFO(__VA_ARGS__);} diff --git a/src/core/ser_daq/CMakeLists.txt b/src/core/ser_daq/CMakeLists.txt deleted file mode 100644 index 89c7eb33..00000000 --- a/src/core/ser_daq/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 0.0.1) -project(ser_daq) - -find_package(catkin REQUIRED) - -catkin_python_setup() -catkin_package() - -install(PROGRAMS - scripts/ser_daq_driver - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -#if (CATKIN_ENABLE_TESTING) -# find_package(roslint) -# roslint_python() -# roslint_add_test() -#endif() diff --git a/src/core/ser_daq/launch/ser_daq.launch b/src/core/ser_daq/launch/ser_daq.launch deleted file mode 100644 index 15bfc9a7..00000000 --- a/src/core/ser_daq/launch/ser_daq.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/core/ser_daq/launch/ser_daq.launch.xml b/src/core/ser_daq/launch/ser_daq.launch.xml new file mode 100644 index 00000000..fb5887cd --- /dev/null +++ b/src/core/ser_daq/launch/ser_daq.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/core/ser_daq/package.xml b/src/core/ser_daq/package.xml index bb3a197c..bbee5472 100644 --- a/src/core/ser_daq/package.xml +++ b/src/core/ser_daq/package.xml @@ -1,31 +1,21 @@ - + + ser_daq - 0.5.0 + 1.0.0 Drives I/O via serial port Adam Romlein - - - - - Apache 2.0 - Michael McDermott - - catkin - - rospy - python-serial - custom_msgs + rclpy + std_msgs + python3-serial - - - + ament_python diff --git a/src/core/ser_daq/scripts/__init__.py b/src/core/ser_daq/resource/ser_daq similarity index 100% rename from src/core/ser_daq/scripts/__init__.py rename to src/core/ser_daq/resource/ser_daq diff --git a/src/core/ser_daq/scripts/ser_daq_driver b/src/core/ser_daq/scripts/ser_daq_driver deleted file mode 100755 index 3bb91c28..00000000 --- a/src/core/ser_daq/scripts/ser_daq_driver +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import os -import errno -import socket -import sys -import time -from functools import partial - -import serial - -import rospy -import std_msgs.msg - -ros_immediate = rospy.Duration(nsecs=1) - -class Rate(object): - def __init__(self, rate=5.0): - self.rate = rate - - def set_rate(self, msg): - self.rate = msg.data - - @property - def period(self): - return 1.0 / self.rate - - -def cb_set_pin(timer_event=None, ser=None, pin='dtr', val=False): - # type: (rospy.TimerEvent, serial.Serial, str, bool) -> None - if ser is None: - raise RuntimeError("No serial object") - - setattr(ser, pin, val) - - -def cb_send_pulse(timer_event=None, ser=None, pin='dtr', duration=0.05): - # type: (rospy.TimerEvent, serial.Serial, str, float) -> None - """ - Send a pulse to the pin - :param timer_event: - :param ser: Serial interface object - :param pin: pin to use, must be RTS or DTR - :param duration: Length of pulse in seconds - :return: - """ - - setattr(ser, pin, True) - rospy.logdebug('On') - cb_off = partial(cb_set_pin, ser=ser, pin=pin, val=False) - rospy.Timer(rospy.Duration(nsecs=int(duration*1e9)), cb_off, oneshot=True) - - - -def trigger_serial(ser, pulse_frequency=2.0, spin_frequency=1000, pin='dtr'): - """Use this to send a pulse via RTS pin""" - import time - pulse_rate = Rate(pulse_frequency) - spin_rate = Rate(spin_frequency) - pulse_duration = 0.05 - pulse = False - trigger_sub = rospy.Subscriber('/daq/trigger_freq', - std_msgs.msg.Float64, - pulse_rate.set_rate) - while not rospy.is_shutdown(): - rospy.Timer(ros_immediate, - partial(cb_send_pulse, ser=ser, pin=pin, duration=pulse_duration), - oneshot=True) - rospy.sleep(pulse_rate.period) - - -if __name__ == '__main__': - rospy.init_node('daq') - try: - - buffer_size = rospy.get_param('~buffer_size', 4096) - timeout = rospy.get_param('~timeout_sec', 2) - spoof = rospy.get_param('spoof_rate', 0) or rospy.get_param('~spoof') - - except KeyError as e: - rospy.logerr("Parameter %s not found" % e) - sys.exit(1) - - - pulse_tty = os.environ.get('DAQ_TTY', None) - try: - ser = serial.Serial(pulse_tty) - except Exception as exc: - ser = None - print('Unable to find tty: {}'.format(pulse_tty)) - - if ser: # dude, gross. but it should work. - print('Serial connected: {}'.format(ser.name)) - trigger_serial(ser) - sys.exit(0) - - diff --git a/src/core/ser_daq/ser_daq/__init__.py b/src/core/ser_daq/ser_daq/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/core/ser_daq/ser_daq/ser_daq_driver.py b/src/core/ser_daq/ser_daq/ser_daq_driver.py new file mode 100755 index 00000000..a7acb1df --- /dev/null +++ b/src/core/ser_daq/ser_daq/ser_daq_driver.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import os +import sys +import time +import threading + +import serial + +import rclpy +from rclpy.node import Node +import std_msgs.msg + + +class Rate(object): + def __init__(self, rate=5.0): + self.rate = rate + + def set_rate(self, msg): + self.rate = msg.data + + @property + def period(self): + return 1.0 / self.rate + + +def send_pulse(ser, pin='dtr', duration=0.05): + # type: (serial.Serial, str, float) -> None + """ + Send a pulse to the pin + :param ser: Serial interface object + :param pin: pin to use, must be RTS or DTR + :param duration: Length of pulse in seconds + """ + setattr(ser, pin, True) + timer = threading.Timer(duration, setattr, args=(ser, pin, False)) + timer.daemon = True + timer.start() + + +def trigger_serial(node, ser, pulse_frequency=2.0, pin='dtr'): + """Use this to send a pulse via RTS/DTR pin""" + pulse_rate = Rate(pulse_frequency) + pulse_duration = 0.05 + node.create_subscription(std_msgs.msg.Float64, '/daq/trigger_freq', + pulse_rate.set_rate, 10) + while rclpy.ok(): + send_pulse(ser, pin=pin, duration=pulse_duration) + time.sleep(pulse_rate.period) + + +def main(args=None): + rclpy.init(args=args) + node = Node('daq') + + # subscriptions are serviced by a background executor while the main + # thread runs the pulse loop + spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + spin_thread.start() + + pulse_tty = os.environ.get('DAQ_TTY', None) + try: + ser = serial.Serial(pulse_tty) + except Exception: + ser = None + print('Unable to find tty: {}'.format(pulse_tty)) + + if ser: + print('Serial connected: {}'.format(ser.name)) + trigger_serial(node, ser) + sys.exit(0) + + +if __name__ == '__main__': + main() diff --git a/src/core/ser_daq/setup.cfg b/src/core/ser_daq/setup.cfg new file mode 100644 index 00000000..0df38cd7 --- /dev/null +++ b/src/core/ser_daq/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ser_daq +[install] +install_scripts=$base/lib/ser_daq diff --git a/src/core/ser_daq/setup.py b/src/core/ser_daq/setup.py index c32a5437..bd919806 100644 --- a/src/core/ser_daq/setup.py +++ b/src/core/ser_daq/setup.py @@ -1,9 +1,27 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -d = generate_distutils_setup( - packages=['ser_daq'], - package_dir={'': 'src'} -) +from setuptools import setup + +package_name = "ser_daq" -setup(**d) +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Drives I/O via serial port", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "ser_daq_driver = ser_daq.ser_daq_driver:main", + ], + }, +) From c41ce9f6757520db07508be76082a71e7d29974b Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:25:41 -0400 Subject: [PATCH 06/20] Port kw_genicam_driver (IR camera) to ROS2; add shared cam_utils package cam_utils (new package): - ROS2 port of phase_one's EventCache/parseParams/loadFile helpers, which both camera drivers previously compiled via a hardcoded /root/kamera/... path into the phase_one tree. EventCache::search now reports event_num explicitly since header.seq is gone in ROS2 kw_genicam_driver: - driver_a6750 rewritten roscpp -> rclcpp. Active path (EventHandler + CamParamHandler + executor) preserved; the unreachable legacy main loop after the early return, the unused rerange_temp() and the unbuilt legacy driver.cpp are deleted - Image fetch runs on a dedicated thread instead of self-rescheduling one-shot ROS timers; MultiThreadedExecutor services events/services - Watchdog/Trigger/parse helpers in utils ported to rclcpp types; ROS_* logging call sites kept via rclcpp shims - gige_scan/decode_error/genicam_ctl were already ROS-free - Launch files consolidated: flir_a6750/flir_a645 ROS2 XML launches named after the config.yaml camera model (the genicam_* duplicates are dropped) --- src/cams/cam_utils/CMakeLists.txt | 29 + .../include/cam_utils/event_cache.hpp | 101 +++ src/cams/cam_utils/package.xml | 23 + src/cams/cam_utils/src/event_cache.cpp | 173 ++++ src/cams/kw_genicam_driver/CMakeLists.txt | 237 +---- .../config/debug_rosconsole.conf | 7 - .../kw_genicam_driver/launch/flir_a645.launch | 1 - .../launch/flir_a645.launch.xml | 70 ++ .../launch/flir_a6750.launch | 1 - .../launch/flir_a6750.launch.xml | 78 ++ .../launch/genicam_a6750.launch | 123 --- .../launch/genicam_a6xx.launch | 119 --- .../kw_genicam_driver/launch/gige_scan.launch | 37 - src/cams/kw_genicam_driver/package.xml | 70 +- src/cams/kw_genicam_driver/src/driver.cpp | 634 ------------- .../kw_genicam_driver/src/driver_a6750.cpp | 853 +++++------------- src/cams/kw_genicam_driver/src/utils.cpp | 75 +- src/cams/kw_genicam_driver/src/utils.h | 44 +- 18 files changed, 825 insertions(+), 1850 deletions(-) create mode 100644 src/cams/cam_utils/CMakeLists.txt create mode 100644 src/cams/cam_utils/include/cam_utils/event_cache.hpp create mode 100644 src/cams/cam_utils/package.xml create mode 100644 src/cams/cam_utils/src/event_cache.cpp delete mode 100644 src/cams/kw_genicam_driver/config/debug_rosconsole.conf delete mode 120000 src/cams/kw_genicam_driver/launch/flir_a645.launch create mode 100644 src/cams/kw_genicam_driver/launch/flir_a645.launch.xml delete mode 120000 src/cams/kw_genicam_driver/launch/flir_a6750.launch create mode 100644 src/cams/kw_genicam_driver/launch/flir_a6750.launch.xml delete mode 100644 src/cams/kw_genicam_driver/launch/genicam_a6750.launch delete mode 100644 src/cams/kw_genicam_driver/launch/genicam_a6xx.launch delete mode 100644 src/cams/kw_genicam_driver/launch/gige_scan.launch delete mode 100644 src/cams/kw_genicam_driver/src/driver.cpp diff --git a/src/cams/cam_utils/CMakeLists.txt b/src/cams/cam_utils/CMakeLists.txt new file mode 100644 index 00000000..ba57ffa0 --- /dev/null +++ b/src/cams/cam_utils/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(cam_utils) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(custom_msgs REQUIRED) + +add_library(cam_utils src/event_cache.cpp) +target_include_directories(cam_utils PUBLIC + $ + $ +) +ament_target_dependencies(cam_utils rclcpp std_msgs custom_msgs) + +install(TARGETS cam_utils + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ DESTINATION include) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp std_msgs custom_msgs) +ament_package() diff --git a/src/cams/cam_utils/include/cam_utils/event_cache.hpp b/src/cams/cam_utils/include/cam_utils/event_cache.hpp new file mode 100644 index 00000000..d28c24b9 --- /dev/null +++ b/src/cams/cam_utils/include/cam_utils/event_cache.hpp @@ -0,0 +1,101 @@ +#pragma once +#ifndef CAM_UTILS_EVENT_CACHE_HPP +#define CAM_UTILS_EVENT_CACHE_HPP + +#include +#include +#include +#include + +#include +#include +#include + +const rclcpp::Duration ZERO_DURATION(0, 0); +// Used when you need something to trigger immediately, +// but where a truly zero duration may do weird things, e.g. div/0 +const rclcpp::Duration ALMOST_INSTANT{0, 10}; +const rclcpp::Duration MICROSECOND{0, 1000}; + +// Get the absolute value of a ROS duration +rclcpp::Duration rosabs(rclcpp::Duration dur); + +/* This class holds the events published from the INS upon each trigger, + indexed by the system time that event was published. When an image message + is received, this cache is `searched` for the closest event matching the + system time of the image received. These are then fused, and the header + of the image is changed to match the GPS time of the event, and the image + is saved under that GPS time. +*/ +class EventCache { +public: + // specifying some sane defaults + EventCache(); + EventCache(rclcpp::Duration tol, rclcpp::Duration delay); + + // Set the maximum amount of time allowed between an event message + // time received and an image message time received to allow a match + // between the 2. Allows for slop in estimate of delay + void set_tolerance(rclcpp::Duration const &tolerance); + + // Set the expected delay between an image trigger and the time it is + // actually received (including exposure, network transfer, etc.). + // Close to 0 for small images (e.g. IR), up to a second for longer + // exposure large imagery (e.g. Phase One) + void set_delay(double delay); + + void set_delay(rclcpp::Duration const &delay); + + // Insert event message `msg` at time `t` into this map. + void push_back(rclcpp::Time const &t, const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg); + + // Return size of this cache + int size(); + + // Print out all headers in this cache + void show(); + + // Search this cache for an event closest to `image_time`, the system + // time the image was received. + // If found, change the header `head` to the GPS time of the event, set + // `event_num` (header.seq no longer exists in ROS2) and return true. + // If `remove_when_found`, delete cached event upon a successful find. + bool search(rclcpp::Time image_time, std_msgs::msg::Header &head, + uint64_t &event_num, bool remove_when_found); + + bool search(rclcpp::Time image_time, std_msgs::msg::Header &head, + uint64_t &event_num) { + return search(image_time, head, event_num, true); + } + + // Remove all events older than `stale_time` from this cache. + void purge(); + + private: + // Tolerance allowed in the expected delay + rclcpp::Duration tol{ZERO_DURATION}; + // Expected delay from event to image + rclcpp::Duration delay{ZERO_DURATION}; + // Set time to remove messages older than when `purge` is called + rclcpp::Duration stale_time{5, 0}; + // Clock for purge aging; matches node clocks (RCL_ROS_TIME) + rclcpp::Clock clock_{RCL_ROS_TIME}; + // Lock for thread safety + std::mutex mutex_; + // Data structure containing all event messages mapped to their + // sys_time (time they were published) + std::map event_map; +}; + + +// This function takes in a ROS standard list of params, delimited by: +// param1=val1,param2=val2,param3=val3,...,paramN=valN +// and returns a map of {param1: val1, param2: val2, etc.} +std::map parseParams(std::string parameters); + +// Takes in a filename, returns a vector of strings, each one mapping +// to a line in the given file. +std::vector loadFile(std::string filename); + + +#endif //CAM_UTILS_EVENT_CACHE_HPP diff --git a/src/cams/cam_utils/package.xml b/src/cams/cam_utils/package.xml new file mode 100644 index 00000000..bd13231f --- /dev/null +++ b/src/cams/cam_utils/package.xml @@ -0,0 +1,23 @@ + + + + cam_utils + 1.0.0 + + Shared camera-driver utilities: event/image matching cache and small + parsing helpers (formerly compiled straight out of the phase_one tree). + + + Adam Romlein + Apache 2.0 + + ament_cmake + + rclcpp + std_msgs + custom_msgs + + + ament_cmake + + diff --git a/src/cams/cam_utils/src/event_cache.cpp b/src/cams/cam_utils/src/event_cache.cpp new file mode 100644 index 00000000..6c641ea3 --- /dev/null +++ b/src/cams/cam_utils/src/event_cache.cpp @@ -0,0 +1,173 @@ +#include +#include +#include + +#include +#include +#include + +static rclcpp::Logger LOG = rclcpp::get_logger("event_cache"); + +static bool stamp_is_valid(const builtin_interfaces::msg::Time &t) { + return t.sec != 0 || t.nanosec != 0; +} + + +rclcpp::Duration rosabs(rclcpp::Duration dur) { + if (dur < ZERO_DURATION) + return rclcpp::Duration(std::chrono::nanoseconds(-dur.nanoseconds())); + return dur; +} + + +EventCache::EventCache() { + this->set_tolerance(rclcpp::Duration::from_seconds(0.499)); + this->set_delay(rclcpp::Duration(0, 0)); +} + +EventCache::EventCache(rclcpp::Duration tol, rclcpp::Duration delay) { + this->set_tolerance(tol); + this->set_delay(delay); +} + +void EventCache::set_tolerance(rclcpp::Duration const &tolerance) { + std::lock_guard guard(mutex_); + this->tol = tolerance; +} + +void EventCache::set_delay(rclcpp::Duration const &delay) { + std::lock_guard guard(mutex_); + this->delay = delay; +} + +void EventCache::set_delay(double delay) { + std::lock_guard guard(mutex_); + this->delay = rclcpp::Duration::from_seconds(delay); +} + +void EventCache::push_back(rclcpp::Time const &t, const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) { + std::lock_guard guard(mutex_); + if (!stamp_is_valid(msg->sys_time)) { + RCLCPP_ERROR(LOG, "Zero/invalid sys_time encountered in EventCache::push_back()"); + return; + } + if (!stamp_is_valid(msg->gps_time)) { + RCLCPP_ERROR(LOG, "Zero/invalid gps_time encountered in EventCache::push_back()"); + return; + } + event_map.emplace(t, custom_msgs::msg::GsofEvt(*msg)); +} + +int EventCache::size() { + std::lock_guard guard(mutex_); + return (int) event_map.size(); +} + +void EventCache::show() { + std::lock_guard guard(mutex_); + for (auto it = event_map.begin(); it != event_map.end(); ++it) { + std::cout << it->second.header.stamp.sec << "." << it->second.header.stamp.nanosec + << " " << it->second.header.frame_id << "\n"; + } + std::cout << "\n---" << std::endl; +} + +bool EventCache::search(rclcpp::Time image_time, std_msgs::msg::Header &head, + uint64_t &event_num, bool remove_when_found) { + std::lock_guard guard(mutex_); + if (image_time.nanoseconds() == 0) { + RCLCPP_ERROR(LOG, "zero/invalid image_time encountered in EventCache::search()"); + return false; + } + int count = 0; + // we want the lowest corrected time + rclcpp::Duration best_time{999, 999}; + bool have_best = false; + rclcpp::Time best_sys_time; // best matching system time, aka key + RCLCPP_INFO_STREAM(LOG, "Image time is: " << image_time.seconds()); + std::map event_map_copy(event_map); + for (const auto &pair: event_map_copy) { + rclcpp::Time sys_time(pair.second.sys_time); + auto actual_delay = rosabs(sys_time - image_time); + auto corrected_delay = rosabs(actual_delay - delay); + if (corrected_delay < tol) { + std::cout << "img: " << image_time.seconds() << " sys: " << sys_time.seconds() + << " cdt: " << corrected_delay.seconds() << " ad: " << actual_delay.seconds(); + count++; + if (corrected_delay < best_time ) { + best_time = corrected_delay; + head.stamp = pair.second.gps_time; + event_num = pair.second.event_num; + best_sys_time = sys_time; + have_best = true; + std::cout << " *"; + } + std::cout << std::endl; + } + } + RCLCPP_INFO_STREAM(LOG, "Matched " << count << "/" << event_map_copy.size() << " time headers"); + if (count >= 1) { + if (remove_when_found && have_best) { + event_map.erase(best_sys_time); + } + return true; + } + return false; +} + +void EventCache::purge() { + std::lock_guard guard(mutex_); + auto now = clock_.now(); + std::map event_map_copy(event_map); + for ( const auto &pair : event_map_copy ) { + auto age = now - rclcpp::Time(pair.second.sys_time); + if (age > stale_time) { + event_map.erase(pair.first); + } + } +} + + +std::map parseParams(std::string parameters) { + // Iterate through parameters organized by name=value, separated by + // commas. Return a map of parameters to values. + std::map param_to_value; + std::string delimiter1 = ","; + std::string delimiter2 = "="; + size_t pos = 0; + std::string token; + std::string name; + std::string value; + // Always run at least once even if there's no delimiter in request + while ((pos = parameters.find(delimiter1)) != std::string::npos) { + token = parameters.substr(0, pos); + name = token.substr(0, token.find(delimiter2)); + token.erase(0, token.find(delimiter2) + delimiter2.length()); + value = token; + param_to_value[name] = value; + parameters.erase(0, pos + delimiter1.length()); + } + token = parameters; + name = token.substr(0, token.find(delimiter2)); + token.erase(0, token.find(delimiter2) + delimiter2.length()); + value = token; + param_to_value[name] = value; + return param_to_value; +}; + + +std::vector loadFile(std::string filename) { + std::vector lines; + std::ifstream inputFile(filename); + // Check if the file exists and can be opened + if (!inputFile.is_open()) { + std::cout << "File " << filename << " does not exist or cannot be opened." << std::endl; + return lines; + } else { + std::string line; + while (std::getline(inputFile, line)) { + lines.push_back(line); + } + } + return lines; +}; diff --git a/src/cams/kw_genicam_driver/CMakeLists.txt b/src/cams/kw_genicam_driver/CMakeLists.txt index 03564958..6bb2fb6e 100644 --- a/src/cams/kw_genicam_driver/CMakeLists.txt +++ b/src/cams/kw_genicam_driver/CMakeLists.txt @@ -1,27 +1,19 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(kw_genicam_driver) set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) include(CMakePrintHelpers) -## 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 - image_transport - roscpp - sensor_msgs - custom_msgs - camera_info_manager - cv_bridge - phase_one - roskv - ) - -find_package(OpenCV REQUIRED) - - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(custom_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(cam_utils REQUIRED) +find_package(roskv REQUIRED) +find_package(OpenCV REQUIRED) # GEVLIB set( GENICAM_LIB GevApi CorW32 ) @@ -55,211 +47,34 @@ set( GENICAM_LIBRARIES "GCBase_gcc${ARCH_GCCVER}_${GENICAM_PATH_VERSION}" ) -## 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 you 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 kw_genicam_driver -# CATKIN_DEPENDS image_transport roscpp sensor_msg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} - ${GENICAM_INCLUDE_DIRS} - ${KWIVER_INCLUDE_DIR} - ${OpenCV_INCLUDE_DIRS} - ${cv_bridge_INCLUDE_DIRS} - ${CMAKE_BINARY_DIR} -) - -link_directories(${GENICAM_LIBRARY_DIRS} - ${KWIVER_LIBRARY_DIR}) +include_directories(${GENICAM_INCLUDE_DIRS}) +link_directories(${GENICAM_LIBRARY_DIRS}) -## Declare a C++ library -# add_library(kw_genicam_driver -# src/${PROJECT_NAME}/kw_genicam_driver.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(kw_genicam_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable add_executable( a6750_driver_node src/decode_error.cpp src/driver_a6750.cpp - src/macros.h - src/utils.h src/utils.cpp - src/spec_a6750.h src/spec_a6750.cpp - src/genicam_ctl.cpp src/genicam_ctl.h - #TODO: import - /root/kamera/src/cams/phase_one/src/phase_one_utils.cpp) - -add_executable( gige_scan - src/gige_scan.cpp - ) - - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(kw_genicam_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against + src/genicam_ctl.cpp) +ament_target_dependencies(a6750_driver_node + rclcpp std_msgs sensor_msgs custom_msgs image_transport cv_bridge cam_utils roskv) target_link_libraries( a6750_driver_node - PUBLIC ${catkin_LIBRARIES} ${GENICAM_LIBRARIES} ${OpenCV_LIBRARIES} - ${KWIVER_LIBRARIES} ) +add_executable( gige_scan + src/gige_scan.cpp + ) target_link_libraries( gige_scan - PUBLIC ${catkin_LIBRARIES} ${GENICAM_LIBRARIES} ${OpenCV_LIBRARIES} - ${KWIVER_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 kw_genicam_driver kw_genicam_driver_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_kw_genicam_driver.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(TARGETS a6750_driver_node gige_scan + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.launch.xml") -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) -cmake_print_variables(CATKIN_PACKAGE_INCLUDE_DESTINATION catkin_LIBRARIES) -cmake_print_variables(GENICAM_LIBRARIES) -cmake_print_variables(GENICAM_INCLUDE_DIRS) -cmake_print_variables(GENICAM_LIBRARY_DIRS) +ament_package() diff --git a/src/cams/kw_genicam_driver/config/debug_rosconsole.conf b/src/cams/kw_genicam_driver/config/debug_rosconsole.conf deleted file mode 100644 index 4da42b17..00000000 --- a/src/cams/kw_genicam_driver/config/debug_rosconsole.conf +++ /dev/null @@ -1,7 +0,0 @@ -# -# You can define your own by e.g. copying this file and setting -# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file -# -log4j.logger.ros=INFO -log4j.logger.ros.kw_genicam_driver=DEBUG -log4j.logger.ros.roscpp.superdebug=WARN diff --git a/src/cams/kw_genicam_driver/launch/flir_a645.launch b/src/cams/kw_genicam_driver/launch/flir_a645.launch deleted file mode 120000 index 35ffbe61..00000000 --- a/src/cams/kw_genicam_driver/launch/flir_a645.launch +++ /dev/null @@ -1 +0,0 @@ -genicam_a6xx.launch \ No newline at end of file diff --git a/src/cams/kw_genicam_driver/launch/flir_a645.launch.xml b/src/cams/kw_genicam_driver/launch/flir_a645.launch.xml new file mode 100644 index 00000000..de1de300 --- /dev/null +++ b/src/cams/kw_genicam_driver/launch/flir_a645.launch.xml @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cams/kw_genicam_driver/launch/flir_a6750.launch b/src/cams/kw_genicam_driver/launch/flir_a6750.launch deleted file mode 120000 index 2230475f..00000000 --- a/src/cams/kw_genicam_driver/launch/flir_a6750.launch +++ /dev/null @@ -1 +0,0 @@ -genicam_a6750.launch \ No newline at end of file diff --git a/src/cams/kw_genicam_driver/launch/flir_a6750.launch.xml b/src/cams/kw_genicam_driver/launch/flir_a6750.launch.xml new file mode 100644 index 00000000..860bd7de --- /dev/null +++ b/src/cams/kw_genicam_driver/launch/flir_a6750.launch.xml @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cams/kw_genicam_driver/launch/genicam_a6750.launch b/src/cams/kw_genicam_driver/launch/genicam_a6750.launch deleted file mode 100644 index 7be1a678..00000000 --- a/src/cams/kw_genicam_driver/launch/genicam_a6750.launch +++ /dev/null @@ -1,123 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/kw_genicam_driver/launch/genicam_a6xx.launch b/src/cams/kw_genicam_driver/launch/genicam_a6xx.launch deleted file mode 100644 index 58a58578..00000000 --- a/src/cams/kw_genicam_driver/launch/genicam_a6xx.launch +++ /dev/null @@ -1,119 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/kw_genicam_driver/launch/gige_scan.launch b/src/cams/kw_genicam_driver/launch/gige_scan.launch deleted file mode 100644 index 2d42ee28..00000000 --- a/src/cams/kw_genicam_driver/launch/gige_scan.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/kw_genicam_driver/package.xml b/src/cams/kw_genicam_driver/package.xml index 14b6295a..3bf25849 100644 --- a/src/cams/kw_genicam_driver/package.xml +++ b/src/cams/kw_genicam_driver/package.xml @@ -1,64 +1,26 @@ - + + kw_genicam_driver - 0.0.1 - The kw_genicam_driver package + 1.0.0 + GenICam (Teledyne DALSA GigE-V) driver for the FLIR A6750/A6xx IR cameras - - - - jhoare - - - - - + Adam Romlein Apache 2.0 + ament_cmake - - - - - - - - - - + rclcpp + std_msgs + sensor_msgs + custom_msgs + image_transport + cv_bridge + cam_utils + roskv + libopencv-dev - - - - - - - - - - - - - catkin - image_transport - roscpp - phase_one - sensor_msgs - custom_msgs - camera_info_manager - roskv - image_transport - roscpp - phase_one - sensor_msgs - custom_msgs - camera_info_manager - roskv - - - - - + ament_cmake diff --git a/src/cams/kw_genicam_driver/src/driver.cpp b/src/cams/kw_genicam_driver/src/driver.cpp deleted file mode 100644 index ea1b75b4..00000000 --- a/src/cams/kw_genicam_driver/src/driver.cpp +++ /dev/null @@ -1,634 +0,0 @@ -#include -#include -#include -#include -#include - -// Genicam stuff -#include "GenApi/GenApi.h" //!< GenApi lib definitions. -#include "gevapi.h" //!< GEV lib definitions. - -#include -#include - -#include -#include -#include - -#include "decode_error.h" - -#define CAMERA_INDEX 0 -#define PUB_NAME "raw_image" - -/* - -ros configuration items: - -ip_addr - Camera address in dotted decilal "xx.xx.xx.xx" - Selects camera based on IP address - -camera_index - camera index number. A camera can be selected using its - SDK index. This does not work if ip_addr ias been - selected. - -num_buffers - number of application level buffers to allocate. This - value has some latency implications and interacts with - device buffering in an, as yet, unknown manner. - -frame_id - String to use to identify the image stream source. Default - value is "/cueing/right". - -frame_rate - frame rate for the camera. - -output_frame_rate - rate of published images. - -*/ - -// Number of application buffers -#define NUM_BUF 32 // 8, 32 - -// Number of frames buffered internally -#define INT_FR_BUF 16 // 4, 16 - -#define MAX_NETIF 2 -#define MAX_CAMERAS_PER_NETIF 32 -#define MAX_CAMERAS ( MAX_NETIF * MAX_CAMERAS_PER_NETIF ) - - -// --- global data areas -- -bool G_interrupt_seen(false); -GEV_CAMERA_HANDLE G_camera_handle = NULL; -GEV_DEVICE_INTERFACE G_pCamera[MAX_CAMERAS] = { 0 }; -unsigned long G_ip_addr; -UINT32 G_height = 0; -UINT32 G_width = 0; -UINT32 G_x_offset = 0; -UINT32 G_y_offset = 0; -UINT32 G_format = 0; -UINT32 G_buffer_size = 0; -PUINT8 G_buf_address[NUM_BUF]; -int G_num_buffers(NUM_BUF); -float G_frame_rate(21.0); // frame rate requested from camera -float G_output_frame_rate(10.0); // frame rate published - - -// ---------------------------------------------------------------------------- -void display_camera_info() -{ - GEV_CAMERA_INFO *info = GevGetCameraInfo(G_camera_handle); - -#define IP_ADDR(IP) (((IP) >> 24) & 0xff) << "." << (((IP) >> 16) & 0xff) << "." << (((IP) >> 8) & 0xff) << "." << (((IP) >> 0) & 0xff) - std::cout << "fIPv6: " << info->fIPv6 << std::endl - << "ipAddr: " << IP_ADDR(info->ipAddr) << std::endl - << "ipAddrLow: " << IP_ADDR(info->ipAddrLow) << std::endl - << "ipAddrHIGH: " << IP_ADDR(info->ipAddrHigh) << std::endl - << "Mac addr: " << IP_ADDR(info->macLow) << ":" << IP_ADDR(info->macHigh) << std::endl - << "Mfgr: " << info->manufacturer << std::endl - << "Model: " << info->model << std::endl - << "Serial: " << info->serial << std::endl - << "Version: " << info->version << std::endl - << "Username: " << info->username << std::endl - ; - -#undef IP_ADDR -} - - -// ---------------------------------------------------------------------------- -void display_buffer( GEV_BUFFER_OBJECT* imbuf ) -{ - std::cout << "state: " << imbuf->state << std::endl - << "status: " << imbuf->status << std::endl - << "timestamp_hi: " << imbuf->timestamp_hi << std::endl - << "timestamp_lo: " << imbuf->timestamp_lo << std::endl - << "recv_size: " << imbuf->recv_size << std::endl - << "id: " << imbuf->id << std::endl - << "height: " << imbuf->h << std::endl - << "width: " << imbuf->w << std::endl - << "x_offset: " << imbuf->x_offset << std::endl - << "y_offset: " << imbuf->y_offset << std::endl - << "x_padding: " << imbuf->x_padding << std::endl - << "y_padding: " << imbuf->y_padding << std::endl - << "bytes per pixel: " << imbuf->d << std::endl - << "format: " << decode_pixel_format( imbuf->format ) << std::endl - << "address: 0x" << std::hex << static_cast( imbuf->address) << std::endl - << std::dec - ; - -} - - -// ---------------------------------------------------------------------------- -void display_camera_options( const GEV_CAMERA_OPTIONS& opt ) -{ -#define P( F ) #F << " :" << opt.F << std::endl - - std::cout << P(numRetries) - << P(command_timeout_ms) - << P(heartbeat_timeout_ms) - << P(streamPktSize) - << P( streamPktDelay) - << P( streamNumFramesBuffered) - << P( streamMemoryLimitMax) - << P( streamMaxPacketResends) - << P( streamFrame_timeout_ms) - << P( streamThreadAffinity) - << P( serverThreadAffinity) - << P( msgChannel_timeout_ms) - ; - -#undef P -} - - -// --------------------------------------------------------------------------------- -UINT32 parseIPV4string(const std::string& ipAddress) -{ - unsigned char ipbytes[4]; - if ( sscanf(ipAddress.c_str(), "%hhu.%hhu.%hhu.%hhu", - &ipbytes[3], &ipbytes[2], &ipbytes[1], &ipbytes[0]) != 4) - { - std::cerr << "Error parsing IP address: \"" << ipAddress << "\"\n"; - return 0; - } - - return ((UINT32)ipbytes[0]) | ((UINT32)ipbytes[1]) << 8 | ((UINT32)ipbytes[2]) << 16 | ((UINT32)ipbytes[3]) << 24; -} - - -// ---------------------------------------------------------------------------- -GEV_STATUS report_if_error( GEV_STATUS status, const std::string& msg ) -{ - if (status != 0) - { - std::cerr << "Error " << msg << " - " << decode_sdk_status(status) << "\n"; - } - - return status; -} - - -// ================================================================== -void -sigint_handler( int sig ) -{ - G_interrupt_seen = true; -} - - -// ---------------------------------------------------------------------------- -bool open_camera( ros::NodeHandle pnh ) -{ - ROS_INFO( "Opening a camera..." ); - UINT16 status(0); - - // Open camera given IP address - if ( pnh.hasParam("ip_addr") ) - { - ROS_INFO( "... by IPv4 address ..." ); - - std::string ip_string; - pnh.getParam( "ip_addr", ip_string); - ROS_INFO( " - ip_addr: %s", ip_string.c_str() ); - - G_ip_addr = parseIPV4string(ip_string); - if (G_ip_addr == 0) - { - return false; - } - - status = report_if_error( GevOpenCameraByAddress( G_ip_addr, // i: ip addreess of camera - GevExclusiveMode, // i: open mode - &G_camera_handle), // o: camera handle - "opening camera by IP address" ); - } - else - { - ROS_INFO( "... by discovered camera index ..." ); - // No IP address given. Open the first camera found; - int numCamera = 0; - - // Select the first camera found if no other is specified - status = report_if_error( GevGetCameraList( G_pCamera, MAX_CAMERAS, &numCamera ), "get camera list" ); - - printf( "%d camera(s) on the network\n", numCamera ); - - if ( numCamera == 0 ) - { - return false; - } - - int camIndex = CAMERA_INDEX; - if ( pnh.hasParam("camera_index")) - { - pnh.getParam( "camera_index", camIndex); - } - - if ( camIndex >= (int)numCamera ) - { - printf( "Camera index out of range - only %d camera(s) are present\n", numCamera ); - return false; - } - - std::cout << "Selecting camera " << camIndex << std::endl; - - // Open the camera. - status = report_if_error( GevOpenCamera( &G_pCamera[camIndex], - GevExclusiveMode, // i: open mode - &G_camera_handle ), - "opening camera" ); - } - - if (status != 0) - { - return false; - } - - return true; -} - - -// ---------------------------------------------------------------------------- -bool allocate_buffers(ros::NodeHandle pnh, UINT32 format) -{ - // number of image buffers. If there are too few, images will be - // dropped. If there are too many, it increases latency. - if ( pnh.hasParam("num_buffers")) - { - // pnh.getParam( "num_buffers", G_num_buffers); - } - - // Allocate and format image buffers - UINT32 maxDepth = GetPixelSizeInBytes( format ); - - // Allocate image buffers - G_buffer_size = maxDepth * G_width * G_height; - for ( int i = 0; i < G_num_buffers; i++ ) - { - G_buf_address[i] = (PUINT8) malloc( G_buffer_size ); - memset( G_buf_address[i], 0, G_buffer_size ); - } - - return true; -} - - - -// ---------------------------------------------------------------------------- -bool get_camera_info() -{ - UINT16 status(0); - - //===================================================================== - // Get the GenICam FeatureNodeMap object and access the camera features. - static GenApi::CNodeMapRef* Camera = static_cast< GenApi::CNodeMapRef* >( GevGetFeatureNodeMap( G_camera_handle ) ); - - if ( Camera ) - { - // Access some features using the bare GenApi interface methods - try - { - //Mandatory features.... - GenApi::CIntegerPtr ptrIntNode = Camera->_GetNode( "Width" ); - G_width = (UINT32) ptrIntNode->GetValue(); - - ptrIntNode = Camera->_GetNode( "Height" ); - G_height = (UINT32) ptrIntNode->GetValue(); - - GenApi::CEnumerationPtr ptrEnumNode = Camera->_GetNode( "PixelFormat" ); - G_format = (UINT32)ptrEnumNode->GetIntValue(); - - - GenApi::CFloatPtr ptrFloatNode = Camera->_GetNode( "AcquisitionFrameRate" ); - if (ptrFloatNode.IsValid()) - { - double rate = (float) ptrFloatNode->GetValue(); - std::cout << "Acquisition rate: " << rate << "\n"; - ptrFloatNode->SetValue(G_frame_rate); - rate = (float) ptrFloatNode->GetValue(); - std::cout << "Acquisition new rate: " << rate << "\n"; - } - else - { - std::cout << "Acquisition frame rate not available\n"; - } - } - // Catch all possible exceptions from a node access. - CATCH_GENAPI_ERROR( status ); - } - - if (status != 0) - { - std::cerr << "Caught exception\n"; - return false; - } - - return true; -} - - -// ---------------------------------------------------------------------------- -bool get_camera_XML() -{ - UINT16 status(0); - - // Initiliaze access to GenICam features via Camera XML File - // Not sure if all this XML stuff is needed - status = GevInitGenICamXMLFeatures( G_camera_handle, // i: the handle - false ); // i: TRUE updates XML file - if ( status == GEVLIB_OK ) - { - // Get the name of XML file name back (example only - in case you need it somewhere). - char xmlFileName[MAX_PATH] = { 0 }; - status = GevGetGenICamXML_FileName( G_camera_handle, (int)sizeof( xmlFileName ), xmlFileName ); - if ( status == GEVLIB_OK ) - { - std::cout << "XML stored as - " << xmlFileName << std::endl; - } - - // can use GevGetFeatureValue() and GevSetFeatureValue() to work with features. - - /* - // Code to get device temp to do high temp warning - int type; - float val; - status = GevGetFeatureValue( G_camera_handle, "DeviceTemperature", &type, sizeof( val ), &val ); - if (status != GEVLIB_OK ) - { - // print error message - } - // If temp is above threshold, print message. - */ - } - - if ( status != GEVLIB_OK ) - { - std::cerr << "Error getting XML features - " << decode_sdk_status(status) << "\n"; - return false; - } - - { // limit scope of camera options - // Adjust camera options - GEV_CAMERA_OPTIONS camOptions = { 0 }; - - // Adjust the camera interface options if desired (see the manual) - GevGetCameraInterfaceOptions( G_camera_handle, &camOptions ); - - camOptions.heartbeat_timeout_ms = 3000; // 10000 initially - - // Some tuning can be done here. (see the manual) - camOptions.streamNumFramesBuffered = INT_FR_BUF; // Buffer frames internally. (4) - camOptions.numRetries = 10; - - int sfto = (camOptions.streamNumFramesBuffered -1) * (1/G_frame_rate) * 1000; - camOptions.streamFrame_timeout_ms = 3001; // Internal timeout for frame reception. (1001) - - camOptions.streamMemoryLimitMax = 64 * 1024 * 1024; // Adjust packet memory buffering limit. (64m) - camOptions.streamPktSize = 9180; // Adjust the GVSP packet size. (9180) - camOptions.streamPktDelay = 10; // Add usecs between packets to pace arrival at NIC. - - // Assign specific CPUs to threads (affinity) - if required for better performance. - if (0) - { - int numCpus = _GetNumCpus(); - if ( numCpus > 1 ) - { - camOptions.streamThreadAffinity = numCpus - 1; - camOptions.serverThreadAffinity = numCpus - 2; - } - } - - // Write the adjusted interface options back. - GevSetCameraInterfaceOptions( G_camera_handle, &camOptions ); - - std::cout << "-- From camera --\n"; - display_camera_options( camOptions ); - - } // end camera options - - return true; -} - - -// ============================================================================ -int main(int argc, char**argv) -{ - ros::init(argc, argv, "kw_genicam_driver"); - ros::NodeHandle pnh("~"); - - // camera specific data areas - UINT16 status(0); - - report_if_error( GevApiInitialize(), "API initialize" ); - - // Set default options for the library. - { - GEVLIB_CONFIG_OPTIONS options = { 0 }; - - GevGetLibraryConfigOptions( &options ); - // options.logLevel = GEV_LOG_LEVEL_OFF; - options.logLevel = GEV_LOG_LEVEL_TRACE; - // options.logLevel = GEV_LOG_LEVEL_NORMAL; - GevSetLibraryConfigOptions( &options ); - } - - // Start with default value - std::string C_frame_id( "/cueing/right"); - if ( pnh.hasParam("frame_id")) - { - pnh.getParam( "frame_id", C_frame_id); - } - - if ( pnh.hasParam("frame_rate")) - { - pnh.getParam( "frame_rate", G_frame_rate); - } - - if ( pnh.hasParam("output_frame_rate")) - { - pnh.getParam( "output_frame_rate", G_output_frame_rate); - } - - std::string C_output_topic_name_color( PUB_NAME ); - if ( pnh.hasParam( "output_topic_name_color" ) ) - { - pnh.getParam( "output_topic_name_color", C_output_topic_name_color ); - } - - std::string C_output_topic_name_bayer( "bayer_" PUB_NAME ); - if ( pnh.hasParam( "output_topic_name_bayer" ) ) - { - pnh.getParam( "output_topic_name_bayer", C_output_topic_name_bayer ); - } - - // Set up our outbound image transport - image_transport::ImageTransport it(pnh); - image_transport::Publisher it_pub = it.advertise(C_output_topic_name_color, 1); - - image_transport::ImageTransport bayer_it(pnh); - image_transport::Publisher bayer_it_pub = it.advertise(C_output_topic_name_bayer, 1); - - // counters used for reducing the output data rate - const int frame_rate_divisor( G_frame_rate / G_output_frame_rate ); - int frame_counter(0); - - ROS_INFO( "Overriding SIGINT signal handler" ); - signal( SIGINT, sigint_handler ); - - // find a camera - if ( ! open_camera( pnh ) ) - { - ROS_ERROR( "Failed to open camera" ); - goto exit_spot; - } - - // Initialize camera. Quit if init fails. - if ( ! get_camera_XML() ) - { - goto exit_spot; - } - - if ( ! get_camera_info() ) - { - goto exit_spot; - } - - std::cout << "Output frame rate: " << G_output_frame_rate << std::endl; - std::cout << "Frame rate divisor: " << frame_rate_divisor << std::endl; - - display_camera_info(); //+ temp ---------------- - - allocate_buffers(pnh, G_format); - - // Initialize a transfer with synchronous buffer handling. - status = GevInitImageTransfer( G_camera_handle, // i: camera handle - SynchronousNextEmpty, // i: buffer handling mode - G_num_buffers, // i: number of buffers - G_buf_address ); // i: buffer address list - - if (status != GEVLIB_OK) - { - std::cerr << "Error initializing image transfer - " << decode_sdk_status(status) << std::endl; - goto exit_spot; - } - - // clear the buffers - may not be needed since they are cleared when created. - for ( int i = 0; i < G_num_buffers; i++ ) - { - memset( G_buf_address[i], 0, G_buffer_size ); - } - - status = GevStartImageTransfer( G_camera_handle, -1 ); - if ( status != 0 ) - { - std::cerr << "Error starting grab - " << decode_sdk_status(status) << std::endl; - goto exit_spot; - } - - // -------- Read image - publish image - repeat -------- - while(ros::ok()) - { - if (G_interrupt_seen) break; - - static sensor_msgs::Image ros_image; // the message - GEV_BUFFER_OBJECT* image_object_ptr = NULL; - - status = GevWaitForNextImage (G_camera_handle, // i: camera handle - &image_object_ptr, // o: the image object - 2000); // i: timeout in msec - - if ( ( image_object_ptr == NULL ) || ( status != GEVLIB_OK ) ) - { - // log this event and continue - std::cout << "Timeout waiting for image - " << decode_sdk_status(status) << std::endl; - continue; - } - - if ( image_object_ptr->status != 0 ) - { - // Image had an error (incomplete (timeout/overflow/lost)). - // Do any handling of this condition necessary. - std::cout << "Got image with error: " << image_object_ptr->status - << " - " << decode_sdk_status(image_object_ptr->status) <<"\n"; - - // Release the buffer back to the image transfer process. - GevReleaseImage( G_camera_handle, image_object_ptr ); - - continue; - } - - std::cout << "-- Got new image " << image_object_ptr->id << std::endl; - - // display_buffer( image_object_ptr ); //+ ------- temp - - // See if we are to publish the image - if (++frame_counter >= frame_rate_divisor) - { - frame_counter = 0; - - // - // Publish colour image - // - if ( it_pub.getNumSubscribers() > 0 ) - { - // Copy the data into an OpenCV Mat structure - cv::Mat raw_image(G_height, G_width, CV_8UC1, image_object_ptr->address ); - - // Convert Bayer to BGR8 format - cv::Mat bgr_image(G_height, G_width, CV_8UC3); - cv::cvtColor(raw_image, // i: input image - bgr_image, // o: converted image - cv::COLOR_BayerBG2BGR, 3 ); // i: conversion specification - - // Publish the image. - cv_bridge::CvImagePtr cv_ptr( new cv_bridge::CvImage ); - cv_ptr->image = bgr_image; - cv_ptr->encoding = "bgr8"; - - cv_ptr->header.stamp = ros::Time::now(); - cv_ptr->header.frame_id = C_frame_id; - it_pub.publish( cv_ptr->toImageMsg() ); - - std::cout << "Publish id: " << image_object_ptr->id << "\n"; - } - - // Publish the unconverted original smokey-the-Baer image - if ( bayer_it_pub.getNumSubscribers() > 0 ) - { - // Copy the data into an OpenCV Mat structure - cv::Mat raw_image(G_height, G_width, CV_8UC1, image_object_ptr->address ); - - // Publish the image. - cv_bridge::CvImagePtr cv_ptr( new cv_bridge::CvImage ); - cv_ptr->image = raw_image; - cv_ptr->encoding = "mono8"; - - cv_ptr->header.stamp = ros::Time::now(); - cv_ptr->header.frame_id = C_frame_id; - bayer_it_pub.publish( cv_ptr->toImageMsg() ); - } - } - - // Release the buffer back to the image transfer process. - GevReleaseImage( G_camera_handle, image_object_ptr ); - - } // end while - -exit_spot: - if (G_camera_handle != NULL) - { - GevStopImageTransfer( G_camera_handle ); - - GevAbortImageTransfer( G_camera_handle ); - GevFreeImageTransfer( G_camera_handle ); - GevCloseCamera( &G_camera_handle ); - } - - // Close down the API. - GevApiUninitialize(); - - // Close socket API - _CloseSocketAPI(); // must close API even on error - - return 0; -} // main diff --git a/src/cams/kw_genicam_driver/src/driver_a6750.cpp b/src/cams/kw_genicam_driver/src/driver_a6750.cpp index 96229533..91b3d9a7 100644 --- a/src/cams/kw_genicam_driver/src/driver_a6750.cpp +++ b/src/cams/kw_genicam_driver/src/driver_a6750.cpp @@ -2,20 +2,21 @@ #include #include #include +#include #include #include #include #include +#include // ROS stuff -#include -#include "std_msgs/UInt8.h" -#include "std_msgs/Int8.h" - -#include +#include +#include "std_msgs/msg/u_int8.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_msgs/msg/header.hpp" #include -#include +#include #include #include #include @@ -26,12 +27,12 @@ // Includes from: /usr/dalsa/GigeV/include/ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "utils.h" @@ -45,28 +46,18 @@ enum FirmwareMode { Bayer=0, Color=1, Mono=2, Mono8=3, Mono16=4}; /// Forward declarations -void cb_request_shutdown(std_msgs::Int8 const &msg) { - ROS_INFO("Requesting clean shutdown: %d", msg.data); - ros::requestShutdown(); -} - class Transporter { public: - Transporter(ros::NodeHandlePtr nhp, const std::string &out_topic) : - nhp_{nhp}, - it_raw(*nhp), + Transporter(rclcpp::Node::SharedPtr nhp, const std::string &out_topic) : + it_raw(nhp), it_raw_pub(it_raw.advertise(out_topic, 1)) {} image_transport::ImageTransport it_raw; image_transport::Publisher it_raw_pub; -private: - ros::NodeHandlePtr nhp_; }; -typedef std::pair TimeMat; - -int purge_stale(std::map &image_map, ros::Time t) +int purge_stale(std::map &image_map, rclcpp::Time t) { int stale = 0; for (const auto pair: image_map) { @@ -80,61 +71,41 @@ int purge_stale(std::map &image_map, ros::Time }; -std::string dumpImageMessage(const sensor_msgs::ImagePtr & received_image, const std::string &filename) +std::string dumpImageMessage(const sensor_msgs::msg::Image::SharedPtr & received_image, const std::string &filename) { std::vector compression_params; compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); compression_params.push_back(100); cv_bridge::CvImagePtr cvPtr; - auto start_db = ros::Time::now(); + auto start_db = steady_clock::now(); cvPtr = cv_bridge::toCvCopy(received_image, received_image->encoding); - ROS_INFO("debayered %ld in %2.3f ", (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), (ros::Time::now()-start_db).toSec()); - cv::Mat undist = cvPtr->image; - auto now = ros::WallTime::now(); - auto nodeName = ros::this_node::getName(); + ROS_INFO("debayered %ld in %2.3f ", (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), + duration(steady_clock::now() - start_db).count()); - start_db = ros::Time::now(); + start_db = steady_clock::now(); - boost::filesystem::path path_filename{filename}; - boost::filesystem::create_directories(path_filename.parent_path()); + std::filesystem::path path_filename{filename}; + std::filesystem::create_directories(path_filename.parent_path()); cv::imwrite(filename, cvPtr->image, compression_params); -// cv::imwrite(filename, cvPtr->image); - ROS_INFO("dumped %ld in %2.3f ", (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), (ros::Time::now()-start_db).toSec()); - start_db = ros::Time::now(); -// auto filename2 = std::string("/mnt/ram/miketest/driverdump/") + nodeName + "/" + std::to_string(now.toSec()) + ".jpg"; -// cv::imwrite(filename2, cvPtr->image, compression_params); -// auto out = cv::imread(filename, cv::IMREAD_UNCHANGED); -// ROS_INFO("read %ld in %2.3f ", (long int)(out.total() * out.elemSize()), (ros::Time::now()-start_db).toSec()); + ROS_INFO("dumped %ld in %2.3f ", (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), + duration(steady_clock::now() - start_db).count()); return filename; - -// return cvPtr->toImageMsg(); } class CameraTimeSync { public: - static ros::Time timestampToRos(uint32_t timehi, uint32_t timelo) { + static rclcpp::Time timestampToRos(uint32_t timehi, uint32_t timelo) { return timestampToRos(timehi, timelo, 1000000000); } - static ros::Time timestampToRos(uint32_t timehi, uint32_t timelo, uint32_t freq) { + static rclcpp::Time timestampToRos(uint32_t timehi, uint32_t timelo, uint32_t freq) { uint64_t utime = ((uint64_t )timehi) << 32; utime = utime + (uint64_t )timelo; double dtime = double (utime) / (double) freq; -// double dtime = (double)(((uint64_t )timehi) << 32); - return ros::Time{dtime}; + return rclcpp::Time{(int64_t)(dtime * 1e9), RCL_ROS_TIME}; } }; -class DriverHandlerOpts { -public: - DriverHandlerOpts(int rawImageCVMatType) - : - rawImageCVMatType{rawImageCVMatType} - {} - int rawImageCVMatType; - std::string output_topic_raw; -}; - // ---------------------------------------------------------------------------- // Global Variables @@ -150,57 +121,10 @@ void signalHandler( int signum ) { G_SIGINT_TRIGGERED = true; } - -/** Rescale the image pixel values according to a specified temperature range. - * Will set the output range approximately such that - * minTempK ~= min(dst) = -N-sigma and maxTempK ~= max(dst) = +N-sigma - * Note: the A6750 in mono16/temp10mK mode has min/max values of 0/65535, even though the average is ~30000. - * This prevents naive approach to min-max-scaling. We want the lower / bounds on the histogram (+/- N-sigma). - * # in progress! # - * - * @param src - input matrix - * @param dest - destination matrix - * @param minTempK - expected min temperature, in Kelvin - * @param maxTempK - expected max temperature, in Kelvin - * @param rtype - output matrix data type - * @param nSigma - lower/upper threshold. - */ -void rerange_temp(cv::Mat src, cv::Mat dest, double minTempK=273, double maxTempK=303, int rtype=CV_16UC1, - double nSigma=3.0) { - double imax; - if (rtype == CV_16UC1) { - imax = 65535; - } else if (rtype == CV_8UC1) { - imax = 255; - } else { - ROS_WARN("Unsupported matrix data type. Falling back to mono8"); - imax = 255; - rtype = CV_8UC1; - } - double mean, minVal, maxVal; - double countRange = (maxTempK - minTempK) / 0.01; // adjust for 10mK/count - double scaler = 4; //imax / countRange / 2.0f; - cv::Scalar tmp; - cv::minMaxLoc(src, &minVal, &maxVal); - tmp = cv::mean(src); - mean = tmp.val[0]; - ROS_WARN("Input Min/Max/Mean: %.1f %.1f %.1f", minVal, maxVal, mean); - cv::Scalar tmpx = cv::mean(src); - mean = (float) tmpx.val[0]; - cv::Mat srcD(src.rows, src.cols, CV_32FC1); - src.convertTo(srcD, CV_32FC1); -// srcD -= mean/2.0; - srcD *= 0.01; // Convert to Kelvin - srcD -= 273.15; // Convert to C - cv::minMaxLoc(srcD, &minVal, &maxVal); - tmp = cv::mean(srcD); - mean = tmp.val[0]; - ROS_WARN("Celcius Min/Max/Mean: %.1f %.1f %.1f", minVal, maxVal, mean); - srcD.convertTo(dest, CV_16UC1, 1500.); // rescale into 16 bit range - cv::minMaxLoc(dest, &minVal, &maxVal); - tmp = cv::mean(dest); - mean = tmp.val[0]; - ROS_WARN("Output Min/Max/Mean: %.1f %.1f %.1f", minVal, maxVal, mean); +void cb_request_shutdown(std_msgs::msg::Int8 const &msg) { + ROS_INFO("Requesting clean shutdown: %d", msg.data); + G_SIGINT_TRIGGERED = true; + rclcpp::shutdown(); } @@ -227,6 +151,7 @@ struct NodeSettings { uint32_t uint_cam_ip_addr = 0; CameraIdentifier camera_id; + rclcpp::Node::SharedPtr node_; std::shared_ptr envoy_; /** XML Feature settings parameters @@ -252,11 +177,11 @@ struct NodeSettings { output_image_crop_bot_row; std::string frame_id; // Frame ID string to set in output messages. - ros::Subscriber brightness_sub; // handle to subscriber - ros::Subscriber event_sub; // handle to gps events - ros::Subscriber shutdown_sub; // sub for clean shutdown - ros::Publisher stat_pub; // handle to tracing stat - ros::Publisher errstat_pub; // handle to error tracing + rclcpp::Subscription::SharedPtr brightness_sub; // handle to subscriber + rclcpp::Subscription::SharedPtr event_sub; // handle to gps events + rclcpp::Subscription::SharedPtr shutdown_sub; // sub for clean shutdown + rclcpp::Publisher::SharedPtr stat_pub; // handle to tracing stat + rclcpp::Publisher::SharedPtr errstat_pub; // handle to error tracing std::string output_topic_raw; // Name of topic to output raw image to. float output_frame_rate; // Output frame-rate (hz). std::string output_topic_debayer; // Topic to output debayered image to. @@ -265,17 +190,17 @@ struct NodeSettings { std::string rawImageCVEncoding; int rawImageCVMatType; - std_msgs::Header last_published_; // Last header which was successfully published - custom_msgs::GSOF_EVT event_; // store the last received event + std_msgs::msg::Header last_published_; // Last header which was successfully published + custom_msgs::msg::GsofEvt event_; // store the last received event /** - * Construct settings from a node handle (usually the private handle). + * Construct settings from the node. * * \throws ConfigurationError Failed extracting setting values appropriately. */ - NodeSettings(ros::NodeHandle const &nh) { + NodeSettings(rclcpp::Node::SharedPtr node) : node_{node} { bool failed(false); - int tmp_int; + rclcpp::Node &nh = *node; G_INFO_VERBOSITY = static_cast( parse_pos_int(nh, "info_verbosity", failed, 0) ); ROS_INFO("Info Verbosity set to %d", G_INFO_VERBOSITY); @@ -299,11 +224,11 @@ struct NodeSettings { rawImageCVEncoding = FirmModeToCVEnc[firmwareMode]; rawImageCVMatType = FirmModeToCVMat[firmwareMode]; - nh.param("camera_username", camera_id.username, std::string()); - nh.param("camera_manufacturer", camera_id.manufacturer, std::string()); - nh.param("camera_ip_addr", camera_id.ip_addr, std::string()); - nh.param("camera_serial", camera_id.serial, std::string()); - nh.param("camera_mac", camera_id.mac, std::string()); + camera_id.username = nh.declare_parameter("camera_username", std::string()); + camera_id.manufacturer = nh.declare_parameter("camera_manufacturer", std::string()); + camera_id.ip_addr = nh.declare_parameter("camera_ip_addr", std::string()); + camera_id.serial = nh.declare_parameter("camera_serial", std::string()); + camera_id.mac = nh.declare_parameter("camera_mac", std::string()); if (G_INFO_VERBOSITY >= 2) { print_camera_id(camera_id); } @@ -323,20 +248,19 @@ struct NodeSettings { } // Optional path to an XML settings file to load and use. - nh.param("xmlFeatures_filepath", xmlFeatures_filepath, std::string()); + xmlFeatures_filepath = nh.declare_parameter("xmlFeatures_filepath", std::string()); - nh.param("xmlFeatures_autoBrightness", xmlFeatures_autoBrightness, false); - nh.param("xmlFeatures_autoBrightnessTarget", tmp_int, 128); - xmlFeatures_autoBrightnessTarget = tmp_int; + xmlFeatures_autoBrightness = nh.declare_parameter("xmlFeatures_autoBrightness", false); + xmlFeatures_autoBrightnessTarget = nh.declare_parameter("xmlFeatures_autoBrightnessTarget", 128); // 0 - Off, 1 - On Demand, 2 - Periodic - nh.param("xmlFeatures_BalanceWhiteAuto", xmlFeatures_BalanceWhiteAuto, 0); + xmlFeatures_BalanceWhiteAuto = nh.declare_parameter("xmlFeatures_BalanceWhiteAuto", 0); imageTransfer_numImageBuffers = static_cast( parse_pos_int(nh, "imageTransfer_numImageBuffers", failed) ); nextImage_timeout = static_cast( parse_pos_int(nh, "nextImage_timeout", failed) ); output_frame_rate = parse_pos_float(nh, "output_frame_rate", failed); - nh.param("output_image_crop_top_row", output_image_crop_top_row, -1); - nh.param("output_image_crop_bot_row", output_image_crop_bot_row, -1); + output_image_crop_top_row = nh.declare_parameter("output_image_crop_top_row", -1); + output_image_crop_bot_row = nh.declare_parameter("output_image_crop_bot_row", -1); if (output_image_crop_bot_row >= 0 && output_image_crop_bot_row <= output_image_crop_top_row) { failed = true; @@ -344,17 +268,19 @@ struct NodeSettings { output_image_crop_bot_row, output_image_crop_top_row); } - if (!nh.getParam("frame_id", frame_id)) { + frame_id = nh.declare_parameter("frame_id", std::string()); + if (frame_id.empty()) { failed = true; ROS_ERROR("No frame ID provided!"); } - if (!nh.getParam("output_topic_raw", output_topic_raw)) { + output_topic_raw = nh.declare_parameter("output_topic_raw", std::string()); + if (output_topic_raw.empty()) { failed = true; ROS_ERROR("No output topic string provided"); } // Debayer output is optional // - Debayering is undefined if the firmware is not set to bayer mode. - nh.param("output_topic_debayer", output_topic_debayer, std::string()); + output_topic_debayer = nh.declare_parameter("output_topic_debayer", std::string()); if (output_topic_debayer.size() > 0 && firmware_mode != FirmwareMode::Bayer) { failed = true; @@ -420,7 +346,7 @@ struct NodeSettings { * @param width Pixel width of the image to crop. * @roi Output cv::Rect to set the crop ROI to. */ - cv::Rect makeRoi(int height, int width, cv::Rect &roi) { + void makeRoi(int height, int width, cv::Rect &roi) { roi.x = 0; roi.y = 0; roi.width = width; @@ -471,24 +397,6 @@ struct NodeSettings { * @param[out] cam_opts Camera options structure to set values to. */ void set_camera_options(GEV_CAMERA_OPTIONS &cam_opts) { - /** - typedef struct - { // Defaults: - UINT32 numRetries; // 3 - UINT32 command_timeout_ms; // 2000 - UINT32 heartbeat_timeout_ms; // 10000 - UINT32 streamPktSize; // algorithmic - UINT32 streamPktDelay; // 0 - UINT32 streamNumFramesBuffered; // 4 - UINT32 streamMemoryLimitMax; // ??? - UINT32 streamMaxPacketResends; // 100 - UINT32 streamFrame_timeout_ms; // 1000 - INT32 streamThreadAffinity; // -1 - INT32 serverThreadAffinity; // -1 - UINT32 msgChannel_timeout_ms; // 1000 - } GEV_CAMERA_OPTIONS, *PGEV_CAMERA_OPTIONS; - */ - /** Transferring values from example/previous driver. * The following states there is 32MB of onboard memory for acquisitions: * http://info.teledynedalsa.com/acton/attachment/14932/f-054e/1/-/-/l-0042/l-0042/Genie%20Nano%20Series%20User%20Manual.pdf @@ -537,17 +445,6 @@ struct NodeSettings { // Global GenApi exception handling UINT16 genapi_exception_status(0); try { - GenApi::CFloatPtr float_node_ptr; - GenApi::CIntegerPtr int_node_ptr; - GenApi::CEnumerationPtr enum_node_ptr; - - /** Auto-brightness, AB target, and AWB not currently used */ -// push_node_ptr(feature_node_map_ptr, "autoBrightnessMode", (int) xmlFeatures_autoBrightness); -// push_node_ptr(feature_node_map_ptr, "autoBrightnessTarget", ((int) xmlFeatures_autoBrightnessTarget) & 0xff); -// push_node_ptr(feature_node_map_ptr, "BalanceWhiteAuto", (int) xmlFeatures_BalanceWhiteAuto); -// push_node_ptr(feature_node_map_ptr, "autoBrightnessAlgoConvergenceTime", (float) 15.0); - - /** Set the GenICam camera parameters. See `docs/devices/genicam.rst` for more info * TriggerMode = { FreeRun, TriggeredFreeRun, TriggeredSequence, TriggeredPresetAdvance } * TriggerSelector not used by A6750 @@ -557,7 +454,6 @@ struct NodeSettings { * */ if (camType == "6750") { push_node_ptr(feature_node_map_ptr, "TriggerMode", "FreeRun"); -// push_node_ptr(feature_node_map_ptr, "TriggerSelector", "FrameStart"); push_node_ptr(feature_node_map_ptr, "TriggerSource", triggerSource); push_node_ptr(feature_node_map_ptr, "FrameSyncSource", frameSyncSource); @@ -586,7 +482,7 @@ struct NodeSettings { // ROS Subscriber callback methods. /** Accepts a message of expected type. */ - void update_autobrightness(const std_msgs::UInt8 &msg) { + void update_autobrightness(const std_msgs::msg::UInt8 &msg) { xmlFeatures_autoBrightnessTarget = msg.data; ROS_INFO_STREAM("Received new brightness target: " << xmlFeatures_autoBrightnessTarget); @@ -595,31 +491,37 @@ struct NodeSettings { } /** Set listener callbacks for this node. */ - void set_callback(ros::NodeHandle &nh) { + void set_callback(rclcpp::Node::SharedPtr nh) { // Setup listener for camera_brightness topic - brightness_sub = nh.subscribe(std::string("camera_brightness"), 1, &NodeSettings::update_autobrightness, this); - event_sub = nh.subscribe(std::string("/event"), 5, &NodeSettings::eventCallback, this); - shutdown_sub = nh.subscribe("/shutdown", 1, cb_request_shutdown); - stat_pub = nh.advertise(std::string("/stat"), 5); - errstat_pub = nh.advertise(std::string("/errstat"), 5); + brightness_sub = nh->create_subscription( + "camera_brightness", 1, + [this](const std_msgs::msg::UInt8::ConstSharedPtr msg) { update_autobrightness(*msg); }); + event_sub = nh->create_subscription( + "/event", 5, + [this](const custom_msgs::msg::GsofEvt::ConstSharedPtr msg) { eventCallback(msg); }); + shutdown_sub = nh->create_subscription( + "/shutdown", 1, + [](const std_msgs::msg::Int8::ConstSharedPtr msg) { cb_request_shutdown(*msg); }); + stat_pub = nh->create_publisher("/stat", 5); + errstat_pub = nh->create_publisher("/errstat", 5); } - void eventCallback (const custom_msgs::GSOF_EVTConstPtr& msg) + void eventCallback (const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) { - ROS_INFO("<^> eventCallback <> %2.2f", msg->gps_time.toSec()); + ROS_INFO("<^> eventCallback <> %2.2f", rclcpp::Time(msg->gps_time).seconds()); event_ = *msg; - auto nodeName = ros::this_node::getName(); - custom_msgs::Stat stat_msg; + auto nodeName = std::string(node_->get_name()); + custom_msgs::msg::Stat stat_msg; std::stringstream link; - stat_msg.header.stamp = ros::Time::now(); + stat_msg.header.stamp = node_->now(); stat_msg.trace_header = (*msg).header; stat_msg.trace_topic = nodeName + "/eventCallback"; stat_msg.node = nodeName; - link << nodeName << "/event/" << event_.header.seq; // link this trace to the event trace + link << nodeName << "/event/" << event_.event_num; // link this trace to the event trace stat_msg.link = link.str(); - stat_pub.publish(stat_msg); + stat_pub->publish(stat_msg); trigger.fire_cond(); } @@ -627,115 +529,118 @@ struct NodeSettings { class CamParamHandler { public: - CamParamHandler(ros::NodeHandlePtr nhp, GEV_CAMERA_HANDLE camera_handle, const boost::shared_ptr settings) + CamParamHandler(rclcpp::Node::SharedPtr nhp, GEV_CAMERA_HANDLE camera_handle, const std::shared_ptr settings) : + settings{settings}, nhp_{nhp}, - camera_handle_{camera_handle}, - settings{settings} { + camera_handle_{camera_handle} { genapi_ = std::make_shared(camera_handle_); - get_camera_attr_srv_ = nhp_->advertiseService("get_camera_attr", &CamParamHandler::getCameraAttr, this); - set_camera_attr_srv_ = nhp_->advertiseService("set_camera_attr", &CamParamHandler::setCameraAttr, this); - get_attr_list_srv_ = nhp->advertiseService("get_attr_list", &CamParamHandler::getAttrList, this); - - nuc_srv_ = nhp_->advertiseService("nuc", &CamParamHandler::nuc, this); + get_camera_attr_srv_ = nhp_->create_service( + "get_camera_attr", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { getCameraAttr(req, rsp); }); + set_camera_attr_srv_ = nhp_->create_service( + "set_camera_attr", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { setCameraAttr(req, rsp); }); + get_attr_list_srv_ = nhp_->create_service( + "get_attr_list", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { getAttrList(req, rsp); }); + nuc_srv_ = nhp_->create_service( + "nuc", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { nuc(req, rsp); }); } - bool getCameraAttr(custom_msgs::CamGetAttrRequest &req, custom_msgs::CamGetAttrResponse &rsp) { - ROS_INFO(" getCameraAttr(%s)", req.name.c_str()); - bool stat{false}; - int feature_type; + void getCameraAttr(const std::shared_ptr req, + std::shared_ptr rsp) { + ROS_INFO(" getCameraAttr(%s)", req->name.c_str()); + int feature_type = 0; try { - stat = genapi_->getCamAttr(req.name, rsp.value, &feature_type); + genapi_->getCamAttr(req->name, rsp->value, &feature_type); } catch (std::exception &e) { - rsp.value = "error:" + std::string(e.what()); + rsp->value = "error:" + std::string(e.what()); } - ROS_INFO(" %s[%d]: %s.", req.name.c_str(), feature_type, rsp.value.c_str()); - return stat; + ROS_INFO(" %s[%d]: %s.", req->name.c_str(), feature_type, rsp->value.c_str()); } - bool setCameraAttr(custom_msgs::CamSetAttrRequest &req, custom_msgs::CamSetAttrResponse &rsp) { - ROS_INFO(" setCameraAttr(%s)", req.name.c_str()); - ROS_INFO(" setCameraVal(%s)", req.value.c_str()); - if (! is_number(req.value) ) { + void setCameraAttr(const std::shared_ptr req, + std::shared_ptr rsp) { + ROS_INFO(" setCameraAttr(%s)", req->name.c_str()); + ROS_INFO(" setCameraVal(%s)", req->value.c_str()); + if (! is_number(req->value) ) { std::string error = "Invalid value for attribute, must be a number."; - ROS_ERROR(error.c_str()); - rsp.value = error; - return false; + ROS_ERROR("%s", error.c_str()); + rsp->value = error; + return; } - std::string tmp; - int feature_type; - bool success = genapi_->getCamAttr(req.name, tmp, &feature_type); - if (!success) { - ROS_ERROR("Camera Attribute %s does not exist.", req.name.c_str()); - return false; - } - ROS_INFO(" %s[%d]: %s.", req.name.c_str(), feature_type, tmp.c_str()); - bool stat = genapi_->setCamAttr(req.name, req.value, tmp); - genapi_->getCamAttr(req.name, rsp.value, &feature_type); - ROS_INFO(" %s[%d]: %s.", req.name.c_str(), feature_type, rsp.value.c_str()); - return stat; + std::string tmp; + int feature_type = 0; + bool success = genapi_->getCamAttr(req->name, tmp, &feature_type); + if (!success) { + ROS_ERROR("Camera Attribute %s does not exist.", req->name.c_str()); + return; + } + ROS_INFO(" %s[%d]: %s.", req->name.c_str(), feature_type, tmp.c_str()); + genapi_->setCamAttr(req->name, req->value, tmp); + genapi_->getCamAttr(req->name, rsp->value, &feature_type); + ROS_INFO(" %s[%d]: %s.", req->name.c_str(), feature_type, rsp->value.c_str()); } - bool getAttrList(custom_msgs::StrListRequest &req, custom_msgs::StrListResponse &rsp) { + void getAttrList(const std::shared_ptr req, + std::shared_ptr rsp) { + (void) req; std::vector paramList; - bool status = get_attr_list(camera_handle_, paramList); + get_attr_list(camera_handle_, paramList); if (!paramList.size()) { ROS_ERROR("failed to populate param list"); - return false; + return; } - for (int i = 0; i < paramList.size(); i++) { - rsp.values.push_back(paramList[i]); + for (size_t i = 0; i < paramList.size(); i++) { + rsp->values.push_back(paramList[i]); } - return true; } - bool nuc(custom_msgs::CamSetAttrRequest &req, custom_msgs::CamSetAttrResponse &rsp) { -// return genapi_->tryNuc(); - /* This always seems to report 1, so can't use as a method - std::string tmp; - int feature_type; - genapi_->getCamAttr("CorrectionAutoInProgress", tmp, &feature_type); - int status = std::stoi(tmp); - if ( status != 1 ) { - ROS_ERROR("Correction already in progress, aborting."); - rsp.value = "ERROR"; - return false; - }*/ + void nuc(const std::shared_ptr req, + std::shared_ptr rsp) { + (void) req; ROS_INFO("Initiating a camera NUC via service call."); - rsp.value = tryNucCam(camera_handle_); - return true; + rsp->value = tryNucCam(camera_handle_) ? "OK" : "ERROR"; } private: - const boost::shared_ptr settings; - ros::NodeHandlePtr nhp_; + const std::shared_ptr settings; + rclcpp::Node::SharedPtr nhp_; GEV_CAMERA_HANDLE camera_handle_ = NULL; // void* type std::shared_ptr genapi_; - ros::ServiceServer get_camera_attr_srv_; - ros::ServiceServer set_camera_attr_srv_; - ros::ServiceServer get_attr_list_srv_; - ros::ServiceServer nuc_srv_; + rclcpp::Service::SharedPtr get_camera_attr_srv_; + rclcpp::Service::SharedPtr set_camera_attr_srv_; + rclcpp::Service::SharedPtr get_attr_list_srv_; + rclcpp::Service::SharedPtr nuc_srv_; }; /** * */ class EventHandler { public: - EventHandler(ros::NodeHandlePtr nhp, GEV_CAMERA_HANDLE camera_handle, CameraImageInfo cam_image_info, - const boost::shared_ptr settings, std::shared_ptr genapi) : - nhp_{nhp}, + EventHandler(rclcpp::Node::SharedPtr nhp, GEV_CAMERA_HANDLE camera_handle, CameraImageInfo cam_image_info, + const std::shared_ptr settings, std::shared_ptr genapi) : camera_handle{camera_handle}, cam_image_info{cam_image_info}, - settings{settings}, genapi_{genapi}, + settings{settings}, + nhp_{nhp}, xport{nhp, settings->output_topic_raw} { - postprocSub = nhp->subscribe(settings->output_topic_raw, 10, - &EventHandler::postProcessImage, this, ros::TransportHints().reliable()); - watchdog.setFailCallback([camera_handle](ros::TimerEvent const &e) { + postprocSub = nhp->create_subscription( + settings->output_topic_raw, 10, + [this](const sensor_msgs::msg::Image::SharedPtr msg) { postProcessImage(msg); }); + watchdog.setFailCallback([camera_handle]() { ROS_ERROR("Failed health check, attempting to safely shut down camera"); safe_exit(13, camera_handle); }); watchdog.pet(); // initial pet to give it a head start to avoid crib death. + init(); } @@ -744,50 +649,42 @@ class EventHandler { * if events aren't being received, we ought to be able to trigger off of the event itself * @param msg Event message received */ - void eventCallback(const custom_msgs::GSOF_EVTConstPtr& msg) { + void eventCallback(const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) { { std::lock_guard lck(event_mutex); - t_event_received_ = ros::Time::now(); + t_event_received_ = nhp_->now(); event_ = *msg; - event_cache.push_back(msg->sys_time, msg); + event_cache.push_back(rclcpp::Time(msg->sys_time), msg); event_cache.purge(); event_cache.show(); } - ROS_INFO("%2.4f <> eventCallback <> ", msg->gps_time.toSec()); -// if (synced) { -// fetchImage(ros::Time{}); -// } -// bool ok = checkSync(); + ROS_INFO("%2.4f <> eventCallback <> ", rclcpp::Time(msg->gps_time).seconds()); processImages(); } - void fetchImageRecurrent(const ros::TimerEvent &e) { + void fetchLoop() { ROS_INFO("fetch loop"); - fetchImage(ros::Time{}); -// checkSync(); - if (running && ros::ok()) { - fetchTimer_ = nhp_->createTimer(ros::Duration(0.001), - &EventHandler::fetchImageRecurrent, this, true, true); + while (running && rclcpp::ok() && !G_SIGINT_TRIGGERED) { + fetchImage(); } - } - void fetchImage(const ros::Time &eventTime) { - GEV_BUFFER_OBJECT *img_buff_obj_ptr; // Also the same stuct as GEVBUF_ENTRY and GEVBUF_HEADER ROS_INFO("<> eventCallback <> %2.2f", msg->header.stamp.toSec()); + + void fetchImage() { + GEV_BUFFER_OBJECT *img_buff_obj_ptr; // Also the same stuct as GEVBUF_ENTRY and GEVBUF_HEADER /// timeout is ms std::lock_guard lck(buffer_mutex); GEV_STATUS call_status = GevWaitForNextImage(camera_handle, &img_buff_obj_ptr, 10000); - ros::Time t_image_received = ros::Time::now(); // maybe - - auto nodeName = ros::this_node::getName(); - custom_msgs::Stat stat_msg; - stat_msg.header.stamp = ros::Time::now(); - stat_msg.trace_header = std_msgs::Header(); - stat_msg.trace_topic = nodeName + "/publishImage"; - stat_msg.node = nodeName; + rclcpp::Time t_image_received = nhp_->now(); // maybe + + auto nodeName = std::string(nhp_->get_name()); + custom_msgs::msg::Stat stat_msg; + stat_msg.trace_header = std_msgs::msg::Header(); + stat_msg.trace_topic = nodeName + "/publishImage"; + stat_msg.node = nodeName; stat_msg.header.stamp = t_image_received; stat_msg.trace_header.stamp = t_image_received; @@ -800,7 +697,7 @@ class EventHandler { if (img_buff_obj_ptr) { t_image_received_ = t_image_received; - ROS_INFO("%2.4f Got image", t_image_received.toSec()); + ROS_INFO("%2.4f Got image", t_image_received.seconds()); } else { ROS_WARN("null image pointer"); return endOfTurn(img_buff_obj_ptr); @@ -808,17 +705,15 @@ class EventHandler { if (validate_image(img_buff_obj_ptr, &cam_image_info)) { -// ROS_INFO("image good"); - ros::Time camTime = CameraTimeSync::timestampToRos(img_buff_obj_ptr->timestamp_hi, - img_buff_obj_ptr->timestamp_lo); + rclcpp::Time camTime = CameraTimeSync::timestampToRos(img_buff_obj_ptr->timestamp_hi, + img_buff_obj_ptr->timestamp_lo); ROS_INFO2("Received image with ID %d (%d x %d) ", img_buff_obj_ptr->id, img_buff_obj_ptr->w, img_buff_obj_ptr->h ); // Check if we're NUCing, and flag as such - bool stat{false}; - int feature_type; + int feature_type = 0; std::string rsp; try { - stat = genapi_->getCamAttr("CorrectionAutoInProgress", rsp, &feature_type); + genapi_->getCamAttr("CorrectionAutoInProgress", rsp, &feature_type); } catch (std::exception &e) { rsp = "error:" + std::string(e.what()); @@ -828,15 +723,14 @@ class EventHandler { std::string msg; msg.resize(128); - sprintf(&msg[0], R"({"cam": %f, "recv": %f, "hi": %u, "lo": %u})", - camTime.toSec(), t_image_received.toSec(), img_buff_obj_ptr->timestamp_hi, img_buff_obj_ptr->timestamp_lo); + snprintf(&msg[0], msg.size(), R"({"cam": %f, "recv": %f, "hi": %u, "lo": %u})", + camTime.seconds(), t_image_received.seconds(), img_buff_obj_ptr->timestamp_hi, img_buff_obj_ptr->timestamp_lo); std::cout << msg << "," << std::endl; { cv::Mat raw_image(img_buff_obj_ptr->h, img_buff_obj_ptr->w, settings->rawImageCVMatType, img_buff_obj_ptr->address); cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); -// cv_ptr->header.frame_id = this_frame_id.str(); cv_ptr->encoding = settings->rawImageCVEncoding; cv_ptr->image = raw_image; cv_ptr->header.frame_id = "?nucing=" + std::string(rsp.c_str()); @@ -852,62 +746,54 @@ class EventHandler { ROS_INFO("Cropped top row, new: (%d x %d)", cv_ptr->image.cols, cv_ptr->image.rows); } - std::cout << "!> cv_ptr: type: " << settings->rawImageCVMatType << cv_ptr->header << std::endl; - std::cout << "!> img: dims:" << raw_image.dims << " size: "<< raw_image.size - << " step: "<< raw_image.step << " rows: "<< raw_image.rows << " cols: "<< raw_image.cols - << " type: "<< raw_image.type() << std::endl; auto imgp = cv_ptr->toImageMsg(); ROS_INFO("success at making image message"); std::string link = "/" + nodeName + "/event/NA"; // link this trace to the event trace - stat_msg.trace_header.seq = 0; stat_msg.link = link; stat_msg.note = "success"; image_map.emplace(t_image_received, imgp); } -// cv::Mat raw_image(img_buff_obj_ptr->h, img_buff_obj_ptr->w, -// opts.rawImageCVMatType, -// img_buff_obj_ptr->address); processImages(); } else { stat_msg.note = "failure"; - } + } // Publish status msg, FPS is tracked from this in UI - settings->stat_pub.publish(stat_msg); + settings->stat_pub->publish(stat_msg); endOfTurn(img_buff_obj_ptr); } void start() { running = true; - fetchTimer_ = nhp_->createTimer(ros::Duration(0.001), - &EventHandler::fetchImageRecurrent, this, true, true); + fetch_thread_ = std::thread([this]() { fetchLoop(); }); } void stop() { running = false; + if (fetch_thread_.joinable()) { + fetch_thread_.join(); + } } void shutdown() { ROS_WARN("Shutting down the event handler"); stop(); - event_sub.shutdown(); + event_sub.reset(); } void processImages() { -// image_map. -// auto test = cam_image_info; std::lock_guard lck(event_mutex); - ros::Time evt_key; - ros::Time img_key; + rclcpp::Time img_key; bool hit = false; - std_msgs::Header gps_header; + std_msgs::msg::Header gps_header; + uint64_t event_num = 0; for ( const auto pair: image_map ) { // iterate through every image in the map to find // nearest event to image received - bool success = event_cache.search(pair.first, gps_header); - if (success == 1) { + bool success = event_cache.search(pair.first, gps_header, event_num); + if (success) { // found a hit img_key = pair.first; hit = true; @@ -920,13 +806,14 @@ class EventHandler { std::stringstream this_frame_id; this_frame_id << settings->frame_id; - sensor_msgs::ImagePtr img = image_map[img_key]; + sensor_msgs::msg::Image::SharedPtr img = image_map[img_key]; - this_frame_id << "?lock=1&eventNum=" << gps_header.seq << "&eventTime" << gps_header.stamp.toSec() << img->header.frame_id ; - ROS_INFO_STREAM("Timestamp: " << gps_header.stamp.toSec()); + this_frame_id << "?lock=1&eventNum=" << event_num << "&eventTime" + << rclcpp::Time(gps_header.stamp).seconds() << img->header.frame_id ; + ROS_INFO_STREAM("Timestamp: " << rclcpp::Time(gps_header.stamp).seconds()); img->header = gps_header; img->header.frame_id = this_frame_id.str(); - ROS_INFO(" !!! Found matching %2.4f %2.4f !!! ", gps_header.stamp.toSec(), img_key.toSec()); + ROS_INFO(" !!! Found matching %2.4f %2.4f !!! ", rclcpp::Time(gps_header.stamp).seconds(), img_key.seconds()); /// remove the image so we don't get confused later image_map.erase(img_key); ROS_INFO("Remaining evt %u img %lu ", event_cache.size(), image_map.size()); @@ -934,28 +821,27 @@ class EventHandler { watchdog.pet(); } // end process_img - void postProcessImage(const sensor_msgs::ImagePtr &msg) { + void postProcessImage(const sensor_msgs::msg::Image::SharedPtr &msg) { auto is_archiving = ArchiverHelper::get_is_archiving(settings->envoy_, "/sys/arch/is_archiving"); if (!is_archiving) { return; } long int sec = msg->header.stamp.sec; - long int nsec = msg->header.stamp.nsec; + long int nsec = msg->header.stamp.nanosec; std::string filename = ArchiverHelper::generateFilename(settings->envoy_, arch_opts_, sec, nsec); auto filename_written = dumpImageMessage(msg, filename); - ROS_INFO("dumped #%d %s",msg->header.seq, filename_written.c_str()); + ROS_INFO("dumped %s", filename_written.c_str()); } - ros::Time timeLastEventReceived() { - ros::Time last; + rclcpp::Time timeLastEventReceived() { + rclcpp::Time last; { std::lock_guard lck(event_mutex); -// last = event_.header.stamp(); last = t_event_received_; } return last; } - ros::Time timeLastImageReceived() { + rclcpp::Time timeLastImageReceived() { std::lock_guard lck(image_mutex); return t_image_received_; } @@ -964,48 +850,47 @@ class EventHandler { int rawImageCVMatType_; GEV_CAMERA_HANDLE camera_handle = NULL; // void* type CameraImageInfo cam_image_info; - ros::Subscriber event_sub; + rclcpp::Subscription::SharedPtr event_sub; std::shared_ptr genapi_; private: bool synced = false; - bool running = false; - const boost::shared_ptr settings; - custom_msgs::GSOF_EVT event_; /// todo: deprecated? + std::atomic running{false}; + const std::shared_ptr settings; + custom_msgs::msg::GsofEvt event_; /// todo: deprecated? cv::Mat raw_image_; EventCache event_cache; - std::map image_map; - ros::Time t_event_received_; - ros::Time t_image_received_; - ros::NodeHandlePtr nhp_; + std::map image_map; + rclcpp::Time t_event_received_; + rclcpp::Time t_image_received_; + rclcpp::Node::SharedPtr nhp_; Transporter xport; std::mutex event_mutex; std::mutex image_mutex; std::mutex buffer_mutex; Watchdog watchdog; ArchiverOpts arch_opts_ = ArchiverOpts::from_env(); - ros::Subscriber postprocSub; // handle to subscriber - ros::Timer fetchTimer_; /// kicks off image fetch async events + rclcpp::Subscription::SharedPtr postprocSub; // handle to subscriber + std::thread fetch_thread_; /// runs blocking image fetch loop - // GEV_BUFFER_OBJECT *img_buff_obj_ptr; // Also the same stuct as GEVBUF_ENTRY and GEVBUF_HEADER void init() { static double min_image_delay = 0.05; event_cache.set_delay(min_image_delay); - event_cache.set_tolerance(ros::Duration(0.49)); + event_cache.set_tolerance(rclcpp::Duration::from_seconds(0.49)); } void endOfTurn() { /// event/image messages are kept around for this amount of time, after that the expire /// which means they didn't get paired in the grace period - ros::Duration lookback_period{10.0}; - ros::Time old = ros::Time::now() - lookback_period; + rclcpp::Duration lookback_period{10, 0}; + rclcpp::Time old = nhp_->now() - lookback_period; int losses = 0; losses += purge_stale(image_map, old); for (auto i = 0; i < losses; i++ ) { watchdog.kick(); } - if ( event_cache.size() > image_map.size() ) { + if ( event_cache.size() > (int) image_map.size() ) { int diff = event_cache.size() - image_map.size(); for (int i = 0; i < diff; ++i) { watchdog.kick(); @@ -1026,27 +911,27 @@ class EventHandler { int main(int argc, char **argv) { /** Setup signal handler to kick out of run loop for a clean shutdown. -- Initializing before GEV stuff in order to be sure we don't interrupt - communication to the hardware in case something goes wrong. - -- We are not calling ros::shutdown() here so as to keep logging - functional throughout the exit process. - update: it seems with the new async stuff, roslaunch just isn't propagating signals properly - */ + communication to the hardware in case something goes wrong. */ signal(SIGINT, signalHandler); signal(SIGTERM, signalHandler); // ROS Node initialization + params - ros::init(argc, argv, "a6750_driver_node"); - ros::NodeHandle nh, - nhp("~"); // for parameters + rclcpp::init(argc, argv); + auto node = std::make_shared("a6750_driver_node"); // ROS/GEV Input Parameters - boost::shared_ptr node_settings = boost::make_shared(nhp); + std::shared_ptr node_settings; + try { + node_settings = std::make_shared(node); + } catch (ConfigurationError const &) { + return 1; + } CameraImageInfo cam_image_info; GEV_CAMERA_HANDLE camera_handle = NULL; // void* type - node_settings->set_callback(nh); + node_settings->set_callback(node); // Initialize GEV API { @@ -1064,7 +949,6 @@ int main(int argc, char **argv) { // Pass through option values from NodeSettings as appropriate. node_settings->set_library_config_options(options); - // Report ROS_INFO1("Setting library config options:"); log_config_options("-- ", options); GEV_STATUS s(GevSetLibraryConfigOptions( &options )); @@ -1078,7 +962,6 @@ int main(int argc, char **argv) { // Log information of camera we just connected to. { -// ROS_INFO("Connected to camera:"); ROS_GREEN("SUCCESS! Connected to camera:"); GEV_CAMERA_INFO *ci = GevGetCameraInfo(camera_handle); log_camera_interface(*ci, "-- "); @@ -1105,18 +988,8 @@ int main(int argc, char **argv) { catch (CameraConnectionError const &) { return safe_exit(1, camera_handle); } catch (CameraInUseError const &) { return safe_exit(1, camera_handle); } -// tryBootstrap(camera_handle); - /** Set up feature access using the XML retrieved from the camera. */ GenApi::CNodeMapRef *cam_node_map_ptr = NULL; - cam_node_map_ptr = static_cast< GenApi::CNodeMapRef * >( - GevGetFeatureNodeMap(camera_handle) - ); - std::shared_ptr nodemap(new GenApi::CNodeMapRef); -// cam_node_map_sptr = std::make_shared(*cam_node_map_ptr); - { - - } auto genapi = std::make_shared(camera_handle); genapi->initNodeMap(); std::string tmp; @@ -1136,7 +1009,6 @@ int main(int argc, char **argv) { } else { // true flags saving the XML to disk in the directory: // "$GIGEV_XML_DOWNLOAD/xml/download/" - // TODO: Optionalize output of XML features file ROS_GREEN("Loading XML from camera"); GEV_STATUS s(GevInitGenICamXMLFeatures(camera_handle, true)); RETURN_ON_FAILURE(GevInitGenICamXMLFeatures, @@ -1152,22 +1024,14 @@ int main(int argc, char **argv) { } tellFeatureValue(camera_handle, "TriggerMode"); get_node_val(cam_node_map_ptr, "TriggerMode"); -// tryBootstrap(camera_handle); tellFeatureValue(camera_handle, "GevTimestampTickFrequency"); tellFeatureValue(camera_handle, "FlagState"); - std::string tmp; - //genapi->setCamAttr("CorrectionAutoEnabled", "1", tmp); tellFeatureValue(camera_handle, "CorrectionAutoEnabled"); tellFeatureValue(camera_handle, "CorrectionAutoUseDeltaTemp"); tellFeatureValue(camera_handle, "CorrectionAutoUseDeltaTime"); tellFeatureValue(camera_handle, "CorrectionAutoDeltaTemp"); tellFeatureValue(camera_handle, "CorrectionAutoDeltaTime"); - //tellfeaturevalue(camera_handle, "correctionautoinprogress"); - //trynuc(cam_node_map_ptr); - //tellfeaturevalue(camera_handle, "correctionautoinprogress"); - - } // cam pointer stuff @@ -1215,7 +1079,7 @@ int main(int argc, char **argv) { node_settings->imageTransfer_numImageBuffers, buffer_size, cam_image_info.width, cam_image_info.height, cam_image_info.depth()); - for (int i = 0; i < node_settings->imageTransfer_numImageBuffers; ++i) { + for (unsigned int i = 0; i < node_settings->imageTransfer_numImageBuffers; ++i) { ROS_INFO1("-- buffer %d", i); image_buffer_array[i] = (PUINT8) calloc(buffer_size, sizeof(UINT8)); } @@ -1237,262 +1101,27 @@ int main(int argc, char **argv) { s, GEVLIB_OK, 1, camera_handle); } - /** ROS broadcast publishers */ - ROS_INFO1("Creating ROS image-transport publisher for raw image output..."); - image_transport::ImageTransport it_raw(nh); - image_transport::Publisher it_raw_pub( - it_raw.advertise(node_settings->output_topic_raw, 1) - ); - ros::Publisher missed_frames_pub_( - nh.advertise("/missed_frames", 3) - ); - - image_transport::ImageTransport it_db(nh); - image_transport::Publisher it_db_pub; - if (node_settings->debayer_enabled()) { - ROS_WARN("Creating ROS image-transport publisher for debayered image output..."); - it_db_pub = it_db.advertise(node_settings->output_topic_debayer, 1); - } - - /** ROS Broadcast loop */ - ROS_INFO1("Starting run-loop"); - GEV_STATUS call_status(0); - cv::Rect roi; - // Command node reference to manually trigger image acquisition. -// GenApi::CCommandPtr trigger_cmd_node_ptr = cam_node_map_ptr->_GetNode("TriggerSoftware"); trigger.bind_node_action(cam_node_map_ptr, node_settings->triggerNodeName.c_str()); - GEV_BUFFER_OBJECT *img_buff_obj_ptr; // Also the same stuct as GEVBUF_ENTRY and GEVBUF_HEADER - ros::Rate ros_rate(node_settings->output_frame_rate); // Acquisition Rate - int counter = 0; - bool report_timings = false; // Enable print timing info - // Loop component timers - double deltaT; // elapsed time - double lpLoopTime = 1.0; // low-passed loop time - double lpGamma = 0.1; // low pass coeff - system_clock::time_point t_trigger_start, t_trigger_end, t_wait_return, t_after_error_checks, - t_make_raw_image, t_yuv_bgr_conversion, t_raw_crop, t_published_raw_img, t_published_bayer_img, - t_image_buf_released, t_ros_spin_once, t_brightness_update; - - unsigned int frames_dropped_total_ = 0; - - auto nodeName = ros::this_node::getName(); - custom_msgs::Stat stat_msg; - stat_msg.header.stamp = ros::Time::now(); - stat_msg.trace_header = std_msgs::Header(); - stat_msg.trace_topic = nodeName + "/publishImage"; - stat_msg.node = nodeName; - - /** ======================================= alt loop ========================================================== */ - if (false) { - ROS_WARN("exiting early for the sake of cam param testing (optional)"); - return safe_exit(0, camera_handle); - } - if (true) { -// DriverHandlerOpts dopts{node_settings->rawImageCVMatType}; - /// !!! doing the nodehandleptr thing because I don't really know how to solve it - ros::NodeHandlePtr nhptr = ros::NodeHandlePtr(new ros::NodeHandle); - EventHandler handler{nhptr, camera_handle, cam_image_info, node_settings, genapi}; - CamParamHandler paramHandler{nhptr, camera_handle, node_settings}; -// handler.syncUp(nh, 0.0); - handler.event_sub = nh.subscribe(std::string("/event"), 5, &EventHandler::eventCallback, &handler); - ros::AsyncSpinner spinner{3}; - spinner.start(); - handler.start(); - - auto terminator = nh.createTimer(ros::Duration(0.25), [&](const ros::TimerEvent &e) { - if (G_SIGINT_TRIGGERED) { - ROS_WARN("shutting down spinner"); - spinner.stop(); - handler.shutdown(); - } - }, false, true); - ros::waitForShutdown(); -// while (ros::ok() && !G_SIGINT_TRIGGERED) { -// } - return safe_exit(0, camera_handle); - } - - /** ======================================= main loop ========================================================== */ - while (ros::ok() && !G_SIGINT_TRIGGERED) { - std::stringstream link; - std::stringstream this_frame_id; - - // Log message delineation - ROS_INFO3("==============================================="); - img_buff_obj_ptr = NULL; // Reset loop variables - report_timings = false; - ros::Time t_pre_trigger(ros::Time::now()); // Record image timestamp as time of acquisition request - stat_msg.header.stamp = ros::Time::now(); - stat_msg.trace_header.seq = counter; - - t_trigger_start = system_clock::now(); - trigger.spin_until_trigger(); -// trigger.fire_cond(node_settings->triggeredBySoftware); // fires off software trigger, assuming that feature exists - t_trigger_end = system_clock::now(); - - // Get the next image - ROS_INFO2("Waiting for next image..."); - // IR is not remotely rate-limiting, so we will spin a lot to ensure event is captured - ros::spinOnce(); // allow most recent event to be received - call_status = GevWaitForNextImage(camera_handle, &img_buff_obj_ptr, - node_settings->nextImage_timeout); - t_wait_return = system_clock::now(); - ros::Time t_image_received = ros::Time::now(); - stat_msg.trace_header.stamp = t_image_received; - ROS_WARN("before"); - WARN_ON_FAILURE(GevWaitForNextImage, call_status, GEVLIB_OK); - ROS_WARN("after"); - ros::spinOnce(); // allow most recent event to be received - - // bind image to most recent - std_msgs::Header gps_header; - bool success = false; - - - - if (validate_image(img_buff_obj_ptr, &cam_image_info)) { - t_after_error_checks = system_clock::now(); - - /** Successful image buffer retrieval. */ - ROS_INFO2("Received image with ID %d", img_buff_obj_ptr->id); - cv::Mat raw_image(img_buff_obj_ptr->h, img_buff_obj_ptr->w, - node_settings->rawImageCVMatType, - img_buff_obj_ptr->address); - t_make_raw_image = system_clock::now(); - t_yuv_bgr_conversion = system_clock::now(); - - /** Crop image to configured output dims */ - node_settings->makeRoi(img_buff_obj_ptr->h, img_buff_obj_ptr->w, roi); - ROS_INFO2("Cropping to ROI %dx%d+%d+%d", roi.width, roi.height, roi.x, roi.y); - if (roi.area() == 0) { - ROS_ERROR("Crop ROI has 0 area with raw image height of %d pixels.", - img_buff_obj_ptr->h); - return safe_exit(1, camera_handle); - } - raw_image = raw_image(roi); - t_raw_crop = system_clock::now(); - - if (it_raw_pub.getNumSubscribers() > 0) { - ros::spinOnce(); // allow most recent event to be received - ROS_INFO2("Publishing raw image..."); - cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); - // Set the image timestamp to match the event that actually triggered it - if (success) { - cv_ptr->header = gps_header; - } -// cv_ptr->header.frame_id = this_frame_id.str(); - cv_ptr->encoding = node_settings->rawImageCVEncoding; - cv_ptr->image = raw_image; - link << nodeName << "/event/" << node_settings->event_.header.seq; // link this trace to the event trace - stat_msg.link = link.str(); - stat_msg.note = "success"; - std::cout << "!> cv_ptr: type: " << node_settings->rawImageCVMatType << cv_ptr->header << std::endl; - std::cout << "!> img: dims:" << raw_image.dims << " size: "<< raw_image.size - << " step: "<< raw_image.step << " rows: "<< raw_image.rows << " cols: "<< raw_image.cols - << " type: "<< raw_image.type() << std::endl; - - - ROS_WARN(" evt: %.2f %.2f", cv_ptr->header.stamp.toSec(), node_settings->event_.gps_time.toSec()); - it_raw_pub.publish(cv_ptr->toImageMsg()); - } - t_published_raw_img = system_clock::now(); - - t_published_bayer_img = system_clock::now(); - report_timings = true; // All time_points have been successfully populated. - } else { - ROS_WARN("taking the nasty path, ptr: %p", img_buff_obj_ptr); - std_msgs::Header msg = std_msgs::Header(node_settings->event_.header); - std::stringstream status_int, status_name, note; - - if (img_buff_obj_ptr) { - status_int << img_buff_obj_ptr->status; - status_name << decode_sdk_status(img_buff_obj_ptr->status); - } else { - this_frame_id << "&status=" << 999 << "&error=" - << "nil pointer in buffer"; - } - this_frame_id << "&status=" << status_int.str() << "&error=" << status_name.str(); - note << "status " << status_int.str() << ": " << status_name.str(); - link << nodeName << "/event/" << node_settings->event_.header.seq; // link this trace to the event trace - stat_msg.link = link.str(); - stat_msg.note = note.str(); - msg.seq = ++frames_dropped_total_; - msg.frame_id = this_frame_id.str(); - ROS_WARN("about to publish"); - missed_frames_pub_.publish(msg); - node_settings->errstat_pub.publish(stat_msg); + EventHandler handler{node, camera_handle, cam_image_info, node_settings, genapi}; + CamParamHandler paramHandler{node, camera_handle, node_settings}; + handler.event_sub = node->create_subscription( + "/event", 5, + [&handler](const custom_msgs::msg::GsofEvt::ConstSharedPtr msg) { handler.eventCallback(msg); }); + + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 3); + executor.add_node(node); + handler.start(); + + auto terminator = node->create_wall_timer(std::chrono::milliseconds(250), [&]() { + if (G_SIGINT_TRIGGERED) { + ROS_WARN("shutting down spinner"); + handler.shutdown(); + rclcpp::shutdown(); } - node_settings->stat_pub.publish(stat_msg); - - - // Release used image buffer back to GEV acquisition process. - GevReleaseImage(camera_handle, img_buff_obj_ptr); - t_image_buf_released = system_clock::now(); - - // Allow messages to be received - ros::spinOnce(); - t_ros_spin_once = system_clock::now(); - - // Check for updated brightness setting - if (G_NEW_BRIGHTNESS) { - // activate new brightness target - ROS_INFO2("Setting new brightness value"); - if (!node_settings->set_brightness_target(cam_node_map_ptr)) { - return safe_exit(1, camera_handle); - } - G_NEW_BRIGHTNESS = false; - } - - t_brightness_update = system_clock::now(); - - if (report_timings) { - ROS_INFO2("Loop Timings:"); - ROS_INFO2("-- Before trigger -> After trigger : %f ms", - milliseconds_between(t_trigger_start, t_trigger_end)); - ROS_INFO2("-- After trigger -> Wait return : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_wait_return), - milliseconds_between(t_trigger_end, t_wait_return)); - if (G_TIMING_VERBOSE) { - ROS_INFO2("-- Wait return -> After checks : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_after_error_checks), - milliseconds_between(t_wait_return, t_after_error_checks)); - ROS_INFO2("-- After checks -> Make raw image : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_make_raw_image), - milliseconds_between(t_after_error_checks, t_make_raw_image)); - ROS_INFO2("-- Make raw image -> YUV2BGR cvt : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_yuv_bgr_conversion), - milliseconds_between(t_make_raw_image, t_yuv_bgr_conversion)); - ROS_INFO2("-- YUV2BGR cvt -> Crop Image : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_raw_crop), - milliseconds_between(t_yuv_bgr_conversion, t_raw_crop)); - ROS_INFO2("-- Crop Image -> Publish raw : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_published_raw_img), - milliseconds_between(t_raw_crop, t_published_raw_img)); - ROS_INFO2("-- Publish raw -> Publish debay : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_published_bayer_img), - milliseconds_between(t_published_raw_img, t_published_bayer_img)); - ROS_INFO2("-- Publish debay -> Release image : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_image_buf_released), - milliseconds_between(t_published_bayer_img, t_image_buf_released)); - ROS_INFO2("-- Release image -> ROS Spin Once : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_ros_spin_once), - milliseconds_between(t_image_buf_released, t_ros_spin_once)); - ROS_INFO2("-- ROS Spin Once -> Update brightness : %f ms (+%f ms)", - milliseconds_between(t_trigger_start, t_brightness_update), - milliseconds_between(t_ros_spin_once, t_brightness_update)); - - } - deltaT = milliseconds_between(t_trigger_start, t_brightness_update); - lpLoopTime = (lpLoopTime * (1-lpGamma)) + (deltaT * lpGamma); - ROS_INFO2("-- Overall Cycle Time (rolling avg) : %f ms (%f ms)", deltaT, lpLoopTime); - - } - - counter++; - ros_rate.sleep(); - } - + }); + executor.spin(); + handler.shutdown(); return safe_exit(0, camera_handle); } diff --git a/src/cams/kw_genicam_driver/src/utils.cpp b/src/cams/kw_genicam_driver/src/utils.cpp index 199c7bc5..41aecfa6 100644 --- a/src/cams/kw_genicam_driver/src/utils.cpp +++ b/src/cams/kw_genicam_driver/src/utils.cpp @@ -2,7 +2,8 @@ #include #include #include -#include +#include +#include // Local installs #include #include @@ -16,9 +17,9 @@ using std::string; using std::map; -void cb_fail_shutdown(ros::TimerEvent e) { +void cb_fail_shutdown() { ROS_WARN("Shutting down due to unhealthy"); - ros::requestShutdown(); + rclcpp::shutdown(); } /// === === === new improved interface to GenApi - now with RAII! @@ -142,19 +143,22 @@ bool GenApiConnector::tryNuc() { Watchdog::Watchdog() : failCallback_{cb_fail_shutdown} {} Watchdog::Watchdog(double lookback_period, double health_threshold) : - lookback_period_{lookback_period}, + lookback_period_{rclcpp::Duration::from_seconds(lookback_period)}, health_threshold{health_threshold}, failCallback_{cb_fail_shutdown} {} +rclcpp::Time Watchdog::now() { + return clock_.now(); +} -void Watchdog::push_back(ros::Time const &t, double val) { +void Watchdog::push_back(rclcpp::Time const &t, double val) { std::lock_guard guard(mutex_); - if (!t.isValid()) { + if (t.nanoseconds() == 0) { ROS_ERROR("zero/invalid time encountered in Watchdog::push_back()"); return; } - array.emplace_back(std::pair(t, val)); + array.emplace_back(std::pair(t, val)); } int Watchdog::size() { @@ -165,14 +169,14 @@ int Watchdog::size() { void Watchdog::show() { std::lock_guard guard(mutex_); for (const auto pair : array) { - std::cout << pair.first << ": " << pair.second << std::endl; + std::cout << pair.first.seconds() << ": " << pair.second << std::endl; } std::cout << "\n---" << std::endl; } void Watchdog::purge() { std::lock_guard guard(mutex_); - auto now = ros::Time::now(); + auto now = clock_.now(); std::size_t index = 0; for (const auto pair : array) { auto age = now - pair.first ; @@ -186,12 +190,12 @@ void Watchdog::purge() { void Watchdog::pet() { ROS_INFO("Pet the watchdog"); - this->push_back(ros::Time::now(), 1.0); + this->push_back(now(), 1.0); } void Watchdog::kick() { ROS_WARN("kicked the watchdog"); - this->push_back(ros::Time::now(), -1.0); + this->push_back(now(), -1.0); } void Watchdog::check() { @@ -212,14 +216,13 @@ double Watchdog::computeHealth() { return accu / total; } -void Watchdog::setFailCallback(ros::TimerCallback callback) { +void Watchdog::setFailCallback(std::function callback) { failCallback_ = callback; } void Watchdog::callFail() { ROS_ERROR("failed health check"); - ros::TimerEvent e; - failCallback_(e); + failCallback_(); } @@ -290,19 +293,25 @@ void Trigger::bind_node_action(GenApi::CNodeMapRef *cam_node_map_ptr, const char trigger_cmd_node_ptr = cam_node_map_ptr->_GetNode(trigger_node_name); } -/** Hold up processing until trigger received */ +/** Hold up processing until trigger received. + * Callbacks are serviced by an executor in another thread in ROS2, so + * this just waits for the flag. */ void Trigger::spin_until_trigger() { if (!setToFire) return; - while (!ready.load()) { - ros::spinOnce(); + while (!ready.load() && rclcpp::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } ready = false; } // ---------------------------------------------------------------------------- -string parse_validate_arg(ros::NodeHandle const &nh, string param, map validmap, bool &failed) { +string parse_validate_arg(rclcpp::Node &nh, string param, map validmap, bool &failed) { string tmp_str; - if( ! nh.getParam( param, tmp_str ) ) { + if (!nh.has_parameter(param)) { + nh.declare_parameter(param, std::string("")); + } + nh.get_parameter(param, tmp_str); + if (tmp_str.empty()) { failed = true; ROS_ERROR( "No param %s of type 'string' provided.", param.c_str() ); } @@ -333,34 +342,38 @@ bool is_number(string entry) { } // I have no idea why it's failing with with default parameters so for now just regular overloading -int parse_pos_int(ros::NodeHandle const &nh, string param, bool &failed) { +int parse_pos_int(rclcpp::Node &nh, string param, bool &failed) { return parse_pos_int(nh, param, failed, 1); } -int parse_pos_int(ros::NodeHandle const &nh, string param, bool &failed, int minval) { - int tmp_int; - if( ! nh.getParam( param, tmp_int ) ) { +int parse_pos_int(rclcpp::Node &nh, string param, bool &failed, int minval) { + int tmp_int = minval - 1; + if (!nh.has_parameter(param)) { + nh.declare_parameter(param, tmp_int); + } + if( ! nh.get_parameter( param, tmp_int ) ) { failed = true; ROS_ERROR( "No param %s of type 'int' provided.", param.c_str() ); } else if (tmp_int < minval) { failed = true; ROS_ERROR("%s=%d. It must be greater than or equal to %d.", param.c_str(), tmp_int, minval ); - } else { - return tmp_int; } + return tmp_int; } -float parse_pos_float(ros::NodeHandle const &nh, string param, bool &failed) { - float tmp; - if( ! nh.getParam( param, tmp ) ) { +float parse_pos_float(rclcpp::Node &nh, string param, bool &failed) { + double tmp = 0.0; + if (!nh.has_parameter(param)) { + nh.declare_parameter(param, tmp); + } + if( ! nh.get_parameter( param, tmp ) ) { failed = true; ROS_ERROR( "No param %s provided.", param.c_str() ); } else if (tmp <= 0) { failed = true; ROS_ERROR("%s must be a positive value.", param.c_str() ); - } else { - return tmp; } + return (float) tmp; } @@ -520,7 +533,7 @@ int safe_exit(int exit_code, GEV_CAMERA_HANDLE cam_handle = NULL) { //_CloseSocketAPI(); } - ros::shutdown(); + rclcpp::shutdown(); return exit_code; } diff --git a/src/cams/kw_genicam_driver/src/utils.h b/src/cams/kw_genicam_driver/src/utils.h index a87949ef..354880a6 100644 --- a/src/cams/kw_genicam_driver/src/utils.h +++ b/src/cams/kw_genicam_driver/src/utils.h @@ -6,21 +6,13 @@ #include #include #include +#include #include -#include +#include // Local installs #include #include -#ifndef _GEVAPI_H_ -/// stupid shims for clion IDE -#include "../../../../../../../github.com/zcpp/DALSA/GigeV/include/gev_linux.h" -#include "../../../../../../../github.com/zcpp/DALSA/GigeV/include/gevapi.h" -#include "../../../../../../../github.com/ros/rc_genicam_api/genicam/library/CPP/include/GenApi/GenApi.h" -#include "../../../../src/core/roskv/include/roskv/envoy.h" - -#endif - #include "decode_error.h" #include "spec_a6750.h" #include "macros.h" @@ -35,9 +27,18 @@ using namespace std::chrono; extern uint8_t G_INFO_VERBOSITY; +// rclcpp logging shims to keep ROS1-style call sites +#define ROS_INFO(...) RCLCPP_INFO(rclcpp::get_logger("kw_genicam_driver"), __VA_ARGS__) +#define ROS_WARN(...) RCLCPP_WARN(rclcpp::get_logger("kw_genicam_driver"), __VA_ARGS__) +#define ROS_ERROR(...) RCLCPP_ERROR(rclcpp::get_logger("kw_genicam_driver"), __VA_ARGS__) +#define ROS_DEBUG(...) RCLCPP_DEBUG(rclcpp::get_logger("kw_genicam_driver"), __VA_ARGS__) +#define ROS_INFO_STREAM(args) RCLCPP_INFO_STREAM(rclcpp::get_logger("kw_genicam_driver"), args) +#define ROS_WARN_STREAM(args) RCLCPP_WARN_STREAM(rclcpp::get_logger("kw_genicam_driver"), args) +#define ROS_ERROR_STREAM(args) RCLCPP_ERROR_STREAM(rclcpp::get_logger("kw_genicam_driver"), args) + enum GeniAttributes { GA_MANUFACTURER, GA_MODEL, GA_SERIAL, GA_USERNAME, GA_MAC }; -void cb_fail_shutdown(ros::TimerEvent e); +void cb_fail_shutdown(); // ---------------------------------------------------------------------------- /** Structure containing the camera image output metadata. */ @@ -90,7 +91,7 @@ class Watchdog { Watchdog(double lookback_period, double health_threshold); - void push_back(ros::Time const &t, double val); + void push_back(rclcpp::Time const &t, double val); int size(); @@ -107,19 +108,22 @@ class Watchdog { double computeHealth(); - void setFailCallback(ros::TimerCallback callback); + void setFailCallback(std::function callback); void callFail(); + rclcpp::Time now(); + private: /// lookback period in seconds to consider events - ros::Duration lookback_period_{30}; + rclcpp::Duration lookback_period_{30, 0}; /// 0 = balanced 50/50 double health_threshold{0.0}; + rclcpp::Clock clock_{RCL_ROS_TIME}; std::mutex mutex_; - std::vector> array; - ros::TimerCallback failCallback_; + std::vector> array; + std::function failCallback_; }; @@ -128,10 +132,10 @@ double milliseconds_between(system_clock::time_point const &t1, system_clock::ti std::string enum2str(int en, std::map mymap, std::string errmsg); std::string enum2str(int en, std::map mymap); std::string mac2str(uint32_t LOW, uint32_t HIGH); -std::string parse_validate_arg(ros::NodeHandle const &nh, std::string param, std::map validmap, bool &failed); -int parse_pos_int(ros::NodeHandle const &nh, std::string param, bool &failed); -int parse_pos_int(ros::NodeHandle const &nh, std::string param, bool &failed, int minval); -float parse_pos_float(ros::NodeHandle const &nh, std::string param, bool &failed); +std::string parse_validate_arg(rclcpp::Node &nh, std::string param, std::map validmap, bool &failed); +int parse_pos_int(rclcpp::Node &nh, std::string param, bool &failed); +int parse_pos_int(rclcpp::Node &nh, std::string param, bool &failed, int minval); +float parse_pos_float(rclcpp::Node &nh, std::string param, bool &failed); int arg2enum(std::string arg, std::map validmap); bool is_number(std::string entry); From fc96bc1ac0f2dba92a383f2a9dd170b793d378d3 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:36:10 -0400 Subject: [PATCH 07/20] Port prosilica_camera (RGB/UV) to ROS2 - The 1800-line ProsilicaNodelet becomes a plain rclcpp node (prosilica_node): nodelets don't exist in ROS2 and the nodelet was always run standalone under its own manager anyway - dynamic_reconfigure replaced by node parameters applied at startup; the GainMode/GainValue launch params (previously declared but never read - auto-gain always won) now actually map onto gain settings - Dropped ROS1-only plumbing with no ROS2 equivalent or no consumers: polled_camera request_image, diagnostic_updater state reporting, self_test, the disabled view_server_nodelet, and generic/streaming launch variants - Event/image fusion now uses the shared cam_utils EventCache with explicit event_num (header.seq is gone in ROS2); frame drop accounting is keyed on event_num - Published image now gets its frame_id set before publish (the nodelet set it on the buffer copy after publishing - stale id) - libprosilica Watchdog/CvtPvTimestamp ported to rclcpp types; unused OneShotManager removed; ROS_* call sites kept via rclcpp shims - prosilica_gige_sdk vendored SDK exported as an ament imported target instead of the catkin copy hack --- src/cams/prosilica_camera/CMakeLists.txt | 128 +- .../prosilica_camera/cfg/ProsilicaCamera.cfg | 45 - src/cams/prosilica_camera/debugging.log | 59 - .../include/prosilica/prosilica.h | 60 +- .../prosilica_camera/ProsilicaCameraConfig.h | 805 -------- .../prosilica_camera/launch/generic.launch | 78 - .../prosilica_camera/launch/prosilica.launch | 92 - .../launch/prosilica.launch.xml | 40 + src/cams/prosilica_camera/package.xml | 68 +- .../plugins/nodelet_plugins.xml | 13 - src/cams/prosilica_camera/profiling.txt | 74 - .../src/libprosilica/prosilica.cpp | 64 +- .../src/nodes/prosilica_node.cpp | 1474 +++++++++++++- .../src/nodes/prosilica_nodelet.cpp | 1787 ----------------- .../src/nodes/view_server_nodelet.cpp | 214 -- src/cams/prosilica_camera/streaming.launch | 15 - 16 files changed, 1607 insertions(+), 3409 deletions(-) delete mode 100755 src/cams/prosilica_camera/cfg/ProsilicaCamera.cfg delete mode 100644 src/cams/prosilica_camera/debugging.log delete mode 100644 src/cams/prosilica_camera/include/prosilica_camera/ProsilicaCameraConfig.h delete mode 100644 src/cams/prosilica_camera/launch/generic.launch delete mode 100644 src/cams/prosilica_camera/launch/prosilica.launch create mode 100644 src/cams/prosilica_camera/launch/prosilica.launch.xml delete mode 100644 src/cams/prosilica_camera/plugins/nodelet_plugins.xml delete mode 100644 src/cams/prosilica_camera/profiling.txt delete mode 100644 src/cams/prosilica_camera/src/nodes/prosilica_nodelet.cpp delete mode 100644 src/cams/prosilica_camera/src/nodes/view_server_nodelet.cpp delete mode 100644 src/cams/prosilica_camera/streaming.launch diff --git a/src/cams/prosilica_camera/CMakeLists.txt b/src/cams/prosilica_camera/CMakeLists.txt index a1e68695..fe463291 100644 --- a/src/cams/prosilica_camera/CMakeLists.txt +++ b/src/cams/prosilica_camera/CMakeLists.txt @@ -1,133 +1,69 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(prosilica_camera) set(CMAKE_CXX_STANDARD 17) - -# Load catkin and all dependencies required for this package -# TODO: remove all from COMPONENTS that are not catkin packages. - -find_package(catkin REQUIRED COMPONENTS - prosilica_gige_sdk - roscpp - std_msgs - std_srvs - message_generation - sensor_msgs - custom_msgs - diagnostic_updater - image_transport - self_test - rosconsole - phase_one - dynamic_reconfigure - camera_calibration_parsers - polled_camera - nodelet - nodelet_topic_tools - cv_bridge - roskv - ) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(custom_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(camera_calibration_parsers REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(cam_utils REQUIRED) +find_package(roskv REQUIRED) +find_package(prosilica_gige_sdk REQUIRED) find_package(nlohmann_json REQUIRED) -find_package(fmt REQUIRED) - find_package(Boost REQUIRED COMPONENTS thread) -# <------------ add hiredis dependency ---------------> find_path(HIREDIS_HEADER hiredis) find_library(HIREDIS_LIB hiredis) find_path(REDIS_PLUS_PLUS_HEADER sw) ## NOTE: this should be *sw* NOT *redis++* -include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS}) - -# Generate dynamic parameters -generate_dynamic_reconfigure_options(cfg/ProsilicaCamera.cfg) - -# Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs # Or other packages containing msgs - sensor_msgs - ) - -catkin_package() - -string (REPLACE ";" " " catkin_INCLUDE_STR "${catkin_INCLUDE_DIRS}") - -message(STATUS "Catkin include: ${catkin_INCLUDE_DIRS}") -message(STATUS "Catkin include: ${catkin_INCLUDE_STR}") +include_directories(include ${Boost_INCLUDE_DIR}) add_library(prosilica src/libprosilica/prosilica.cpp) -target_link_libraries(prosilica ${catkin_LIBRARIES}) - -add_library(prosilica_nodelet - src/nodes/prosilica_nodelet.cpp - src/nodes/view_server_nodelet.cpp - #TODO: import - /root/kamera/src/cams/phase_one/src/phase_one_utils.cpp - ) -target_link_libraries(prosilica_nodelet - prosilica - ${nlohman_json_LIBRARIES} - ${catkin_LIBRARIES} - fmt::fmt - ) -target_include_directories(prosilica_nodelet PUBLIC /opt/ros/noetic/include) -class_loader_hide_library_symbols(prosilica_nodelet) - -add_library(view_server_nodelet - src/nodes/view_server_nodelet.cpp - ) +ament_target_dependencies(prosilica rclcpp sensor_msgs prosilica_gige_sdk) +target_link_libraries(prosilica nlohmann_json::nlohmann_json) add_executable(prosilica_node src/nodes/prosilica_node.cpp ) +ament_target_dependencies(prosilica_node + rclcpp std_msgs std_srvs sensor_msgs custom_msgs image_transport + camera_calibration_parsers cv_bridge cam_utils roskv prosilica_gige_sdk) target_link_libraries(prosilica_node prosilica - prosilica_nodelet - view_server_nodelet - ${Boost_LIBRARIES} - ${catkin_LIBRARIES}) -target_include_directories(prosilica_node PUBLIC /opt/ros/noetic/include) -add_dependencies( - prosilica_camera_gencpp - prosilica_node - ${catkin_EXPORTED_TARGETS} - ${prosilica_camera_EXPORTED_TARGETS}) - -# disabled until it's mature?? -#rosbuild_add_executable(find_camera find_camera.cpp) + ${Boost_LIBRARIES}) add_executable(write_memory src/utilities/write_memory.cpp) -target_link_libraries(write_memory prosilica ${catkin_LIBRARIES}) +target_link_libraries(write_memory prosilica) add_executable(read_memory src/utilities/read_memory.cpp) target_link_libraries(read_memory prosilica - ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(set_ip src/utilities/set_ip.cpp) -target_link_libraries(set_ip prosilica ${catkin_LIBRARIES}) +target_link_libraries(set_ip prosilica) add_executable(set_inhibition src/utilities/set_inhibition.cpp) -target_link_libraries(set_inhibition prosilica ${catkin_LIBRARIES}) +target_link_libraries(set_inhibition prosilica) ## === === === === === === === Installation install(TARGETS prosilica_node write_memory read_memory set_ip set_inhibition - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + DESTINATION lib/${PROJECT_NAME}) -install(TARGETS prosilica prosilica_nodelet - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS prosilica + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(FILES prosilica.launch streaming.launch plugins/nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.launch.xml") -# Clion hackery -message(STATUS "Cmake include: ${CMAKE_INCLUDE_PATH}") +ament_package() diff --git a/src/cams/prosilica_camera/cfg/ProsilicaCamera.cfg b/src/cams/prosilica_camera/cfg/ProsilicaCamera.cfg deleted file mode 100755 index 74a0e89a..00000000 --- a/src/cams/prosilica_camera/cfg/ProsilicaCamera.cfg +++ /dev/null @@ -1,45 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='prosilica_camera' - -from dynamic_reconfigure.msg import SensorLevels -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -mode_enum = gen.enum( [ gen.const("StreamingMode", str_t, "streaming", "Run at maximum frame rate"), - gen.const("SoftwareMode", str_t, "software", "Trigger from software"), - gen.const("PolledMode", str_t, "polled", "Capture frame in response to service call"), - gen.const("TriggerMode", str_t, "triggered", "Capture frame in response to trigger topic"), - gen.const("FixedRateMode", str_t, "fixedrate", "Fixed Rate mode"), - gen.const("External1Mode", str_t, "syncin1", "External trigger on SyncIn1 line"), - gen.const("External2Mode", str_t, "syncin2", "External trigger on SyncIn2 line") ], - "Enum to set the trigger mode") - -# Name Type Reconfiguration level Description Default Min Max -gen.add("trigger_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Camera trigger mode", "fixedrate", edit_method = mode_enum) -gen.add("trig_rate", double_t, SensorLevels.RECONFIGURE_STOP, "Sets the expected triggering rate in externally triggered mode.", 1, 1, 100) -gen.add("auto_exposure", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the camera exposure duration to automatic. Causes the `~exposure` setting to be ignored.", True) -gen.add("exposure", double_t, SensorLevels.RECONFIGURE_RUNNING, "Camera exposure time in seconds.", 0.025, 0.000025, 60.0) -gen.add("auto_gain", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the analog gain to automatic. Causes the `~gain` setting to be ignored.", True) -gen.add("gain", int_t, SensorLevels.RECONFIGURE_RUNNING, "The gain level in dB.", 0, 0, 24) -gen.add("auto_whitebalance", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Whether whitebalance will continuously adjust to the current scene. Causes the `~whitebalance_red` and `~whitebalance_blue` settings to be ignored.", True) -gen.add("whitebalance_red", int_t, SensorLevels.RECONFIGURE_RUNNING, "Red gain as a percentage of the camera default setting.", 100, 80, 300) -gen.add("whitebalance_blue", int_t, SensorLevels.RECONFIGURE_RUNNING, "Blue gain as a percentage of the camera default setting.", 100, 80, 300) -gen.add("binning_x", int_t, SensorLevels.RECONFIGURE_RUNNING, "Number of pixels to bin together horizontally.", 1, 1, 8) -gen.add("binning_y", int_t, SensorLevels.RECONFIGURE_RUNNING, "Number of pixels to bin together vertically.", 1, 1, 14) -gen.add("x_offset", int_t, SensorLevels.RECONFIGURE_RUNNING, "X offset of the region of interest.", 0, 0, 2447) -gen.add("y_offset", int_t, SensorLevels.RECONFIGURE_RUNNING, "Y offset of the region of interest.", 0, 0, 2049) -gen.add("width", int_t, SensorLevels.RECONFIGURE_RUNNING, "Width of the region of interest (0 for automatic).", 0, 0, 2448) -gen.add("height", int_t, SensorLevels.RECONFIGURE_RUNNING, "Height of the region of interest (0 for automatic).", 0, 0, 2050) -gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "The optical camera TF frame set in message headers.", "") -gen.add("trig_timestamp_topic", str_t, SensorLevels.RECONFIGURE_STOP, "Sets the topic from which an externally trigged camera receives its trigger timestamps.", "trigger") -gen.add("auto_adjust_stream_bytes_per_second", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Whether the node should automatically adjust the data rate. Causes `~stream_bytes_per_second` to be ignored.", True) -gen.add("stream_bytes_per_second", int_t, SensorLevels.RECONFIGURE_RUNNING, "Limits the data rate of the camera.", 45000000, 1, 115000000) -gen.add("exposure_auto_max", double_t, SensorLevels.RECONFIGURE_RUNNING, "The max exposure time in auto exposure mode, in seconds.", 0.5, 0.000025, 60.0) -gen.add("exposure_auto_target", int_t, SensorLevels.RECONFIGURE_RUNNING, "The auto exposure target mean value as a percentage, from 0=black to 100=white.", 50, 0, 100) -gen.add("gain_auto_max", int_t, SensorLevels.RECONFIGURE_RUNNING, "The max gain level in auto gain mode, in dB.", 24, 0, 24) -gen.add("gain_auto_target", int_t, SensorLevels.RECONFIGURE_RUNNING, "The auto gain target mean value as a percentage, from 0=black to 100=white.", 50, 0, 100) - -exit(gen.generate(PACKAGE, "prosilica_driver", "ProsilicaCamera")) - diff --git a/src/cams/prosilica_camera/debugging.log b/src/cams/prosilica_camera/debugging.log deleted file mode 100644 index d8ef516a..00000000 --- a/src/cams/prosilica_camera/debugging.log +++ /dev/null @@ -1,59 +0,0 @@ -<> Reconf callback -1610061302.508795021: Stopping camera -1610061302.509645643: [2]Enter frameDone, elapsed: 0.000, status: 16 -1610061302.509667913: user callback thingy 0x7f456823f4d8 #0 -1610061302.510065225: FrameDone 0x7f456823f4d8 #0 -1610061302.510218939: publishImage1 in 0.0005 seconds -1610061302.510248543: [2]Enter frameDone, elapsed: 0.000, status: 14 -1610061302.510262458: user callback thingy 0x7f456823f618 #1 -1610061302.510560521: FrameDone 0x7f456823f618 #1 -1610061302.510590345: publishImage1 in 0.0003 seconds -1610061302.510607893: [2]Enter frameDone, elapsed: 0.000, status: 14 -1610061302.510622972: user callback thingy 0x7f456823f758 #2 -1610061302.510957478: FrameDone 0x7f456823f758 #2 -1610061302.510981507: publishImage1 in 0.0003 seconds -1610061302.511002855: [2]Enter frameDone, elapsed: 0.000, status: 14 -1610061302.511017271: user callback thingy 0x7f456823f898 #3 -1610061302.511330463: FrameDone 0x7f456823f898 #3 -1610061302.511351359: publishImage1 in 0.0003 seconds -1610061302.618518978: End of Camera::stop() - -<> SET EXPOSURE = 0 -1610061302.647939146: starting camera 222298 in sync2 trigger mode -1610061302.665559704: End of Camera::start() -1610061302.666346570: exposure = 100000 -END Setup dynamic reconfigure server <1> -1610061302.676530088: Cam init complete -1610061304.296329498: [2]Enter frameDone, elapsed: 0.000, status: 0 -1610061304.296361753: user callback thingy 0x7f456823f4d8 #0 -1610061304.296760414: FrameDone 0x7f456823f4d8 #0 -img: 1610061304.296372879 sys: 1610061304.001754999 cdt: 0.005617880 ad: 0.294617880 * -1610061304.296830966: [629] Missed 628 frames, based on event seq -1610061304.303174978: frameToImage in 0.0062 seconds -1610061304.306930655: processFrame in 0.0100 seconds -1610061304.313541211: thunk started on 0x7f456823f4d8 0 -1610061304.313565752: postProcImage #? entry -1610061304.313586481: postProcImage 0x7f456823f4d8 #0 entry -1610061304.313606860: postProcImage #0 in 0.0000 seconds -1610061304.313658397: publishImage1 in 0.0172 seconds -1610061306.296413022: [2]Enter frameDone, elapsed: 0.000, status: 0 -1610061306.296447700: user callback thingy 0x7f456823f618 #1 -1610061306.296902624: FrameDone 0x7f456823f618 #1 -img: 1610061306.296457452 sys: 1610061306.002162933 cdt: 0.005294519 ad: 0.294294519 * -1610061306.303212856: frameToImage in 0.0062 seconds -1610061306.306956867: processFrame in 0.0100 seconds -1610061306.313549913: thunk started on 0x7f456823f618 1 -1610061306.313581689: postProcImage #? entry -1610061306.313606731: postProcImage 0x7f456823f618 #1 entry -1610061306.313628519: postProcImage #1 in 0.0000 seconds -1610061306.313667022: publishImage1 in 0.0171 seconds -1610061308.296203507: [2]Enter frameDone, elapsed: 0.000, status: 0 -1610061308.296233992: user callback thingy 0x7f456823f758 #2 -1610061308.296693978: FrameDone 0x7f456823f758 #2 -img: 1610061308.296243862 sys: 1610061308.002162933 cdt: 0.005080929 ad: 0.294080929 * -1610061308.298656271: frameToImage in 0.0019 seconds -1610061308.302368940: processFrame in 0.0056 seconds -1610061308.309206149: publishImage1 in 0.0130 seconds -1610061308.309256753: thunk started on 0x7f456823f758 2 -1610061308.309280909: postProcImage #? entry -1610061308.309297725: postProcImage 0x7f458f914faf #-1936 entry \ No newline at end of file diff --git a/src/cams/prosilica_camera/include/prosilica/prosilica.h b/src/cams/prosilica_camera/include/prosilica/prosilica.h index 8ada7986..35fb1c35 100644 --- a/src/cams/prosilica_camera/include/prosilica/prosilica.h +++ b/src/cams/prosilica_camera/include/prosilica/prosilica.h @@ -38,16 +38,15 @@ #include #include #include +#include #include #include #include -#include -#include /// only for defining MetaFrame struct -#include -#include +#include +#include // PvApi.h isn't aware of the usual detection macros // these include support for i386, x86_64, and arm, on Linux and OSX @@ -57,9 +56,18 @@ #undef _LINUX #undef _x86 -void cb_fail_shutdown(ros::TimerEvent e) { +// rclcpp logging shims to keep ROS1-style call sites +#define ROS_INFO(...) RCLCPP_INFO(rclcpp::get_logger("prosilica"), __VA_ARGS__) +#define ROS_WARN(...) RCLCPP_WARN(rclcpp::get_logger("prosilica"), __VA_ARGS__) +#define ROS_ERROR(...) RCLCPP_ERROR(rclcpp::get_logger("prosilica"), __VA_ARGS__) +#define ROS_DEBUG(...) RCLCPP_DEBUG(rclcpp::get_logger("prosilica"), __VA_ARGS__) +#define ROS_INFO_STREAM(args) RCLCPP_INFO_STREAM(rclcpp::get_logger("prosilica"), args) +#define ROS_WARN_STREAM(args) RCLCPP_WARN_STREAM(rclcpp::get_logger("prosilica"), args) +#define ROS_ERROR_STREAM(args) RCLCPP_ERROR_STREAM(rclcpp::get_logger("prosilica"), args) + +inline void cb_fail_shutdown() { ROS_WARN("Shutting down due to unhealthy"); - ros::requestShutdown(); + rclcpp::shutdown(); } template @@ -90,12 +98,12 @@ class Watchdog { Watchdog(); Watchdog(double lookback_period, double health_threshold); - void DelayedStart(ros::NodeHandlePtr nhp, double t); + void DelayedStart(rclcpp::Node::SharedPtr nhp, double t); void Start(); void Stop(); bool Ok(); - void push_back(ros::Time const &t, double val); + void push_back(rclcpp::Time const &t, double val); int size(); @@ -112,28 +120,31 @@ class Watchdog { double computeHealth(); - void setFailCallback(ros::TimerCallback callback); + void setFailCallback(std::function callback); void callFail(); + rclcpp::Time now(); + private: bool enabled_{false}; /// lookback period in seconds to consider events - ros::Duration lookback_period_{30}; + rclcpp::Duration lookback_period_{30, 0}; /// 0 = balanced 50/50 double health_threshold{0.0}; + rclcpp::Clock clock_{RCL_ROS_TIME}; std::mutex mutex_; - std::vector> array; - ros::TimerCallback failCallback_; - ros::Timer delayStartTimer_; + std::vector> array; + std::function failCallback_; + rclcpp::TimerBase::SharedPtr delayStartTimer_; }; namespace prosilica { -ros::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo); -ros::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo, uint32_t freq); +rclcpp::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo); +rclcpp::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo, uint32_t freq); struct ProsilicaException : public std::runtime_error @@ -223,20 +234,6 @@ class PvFrameWrapper { typedef std::shared_ptr PvFrameWrapperPtr; -class OneShotManager { -public: - OneShotManager() = default; - - void erase(boost::uuids::uuid i); - - boost::uuids::uuid addOneShot(ros::NodeHandlePtr nhp, - const ros::Duration &period, - const ros::TimerCallback& callback); - -private: - std::map timer_map; -}; - /// According to FrameStartTriggerMode Enum - AVT GigE Camera and Driver Attributes /// Firmware 1.38 April 7,2010 @@ -318,9 +315,8 @@ class MetaFrame { /// This violates some of the isolation of the non-ros and ros code, but this driver is already /// heavily modified. :shrug:. The trailing underscore matches the prosilica_nodelet convention. boost::mutex frameMutex_; - sensor_msgs::Image img_; - sensor_msgs::Image broken; - ros::Timer postProcTimer; // for doing things like dumping image + sensor_msgs::msg::Image img_; + sensor_msgs::msg::Image broken; }; diff --git a/src/cams/prosilica_camera/include/prosilica_camera/ProsilicaCameraConfig.h b/src/cams/prosilica_camera/include/prosilica_camera/ProsilicaCameraConfig.h deleted file mode 100644 index 364062fe..00000000 --- a/src/cams/prosilica_camera/include/prosilica_camera/ProsilicaCameraConfig.h +++ /dev/null @@ -1,805 +0,0 @@ -//#line 2 "/opt/ros/kinetic/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" -// ********************************************************* -// -// File autogenerated for the prosilica_camera package -// by the dynamic_reconfigure package. -// Please do not edit. -// -// ********************************************************/ - -#ifndef __prosilica_camera__PROSILICACAMERACONFIG_H__ -#define __prosilica_camera__PROSILICACAMERACONFIG_H__ - -#if __cplusplus >= 201103L -#define DYNAMIC_RECONFIGURE_FINAL final -#else -#define DYNAMIC_RECONFIGURE_FINAL -#endif - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace prosilica_camera -{ - class ProsilicaCameraConfigStatics; - - class ProsilicaCameraConfig - { - public: - class AbstractParamDescription : public dynamic_reconfigure::ParamDescription - { - public: - AbstractParamDescription(std::string n, std::string t, uint32_t l, - std::string d, std::string e) - { - name = n; - type = t; - level = l; - description = d; - edit_method = e; - } - - virtual void clamp(ProsilicaCameraConfig &config, const ProsilicaCameraConfig &max, const ProsilicaCameraConfig &min) const = 0; - virtual void calcLevel(uint32_t &level, const ProsilicaCameraConfig &config1, const ProsilicaCameraConfig &config2) const = 0; - virtual void fromServer(const ros::NodeHandle &nh, ProsilicaCameraConfig &config) const = 0; - virtual void toServer(const ros::NodeHandle &nh, const ProsilicaCameraConfig &config) const = 0; - virtual bool fromMessage(const dynamic_reconfigure::Config &msg, ProsilicaCameraConfig &config) const = 0; - virtual void toMessage(dynamic_reconfigure::Config &msg, const ProsilicaCameraConfig &config) const = 0; - virtual void getValue(const ProsilicaCameraConfig &config, boost::any &val) const = 0; - }; - - typedef boost::shared_ptr AbstractParamDescriptionPtr; - typedef boost::shared_ptr AbstractParamDescriptionConstPtr; - - // Final keyword added to class because it has virtual methods and inherits - // from a class with a non-virtual destructor. - template - class ParamDescription DYNAMIC_RECONFIGURE_FINAL : public AbstractParamDescription - { - public: - ParamDescription(std::string a_name, std::string a_type, uint32_t a_level, - std::string a_description, std::string a_edit_method, T ProsilicaCameraConfig::* a_f) : - AbstractParamDescription(a_name, a_type, a_level, a_description, a_edit_method), - field(a_f) - {} - - T (ProsilicaCameraConfig::* field); - - virtual void clamp(ProsilicaCameraConfig &config, const ProsilicaCameraConfig &max, const ProsilicaCameraConfig &min) const - { - if (config.*field > max.*field) - config.*field = max.*field; - - if (config.*field < min.*field) - config.*field = min.*field; - } - - virtual void calcLevel(uint32_t &comb_level, const ProsilicaCameraConfig &config1, const ProsilicaCameraConfig &config2) const - { - if (config1.*field != config2.*field) - comb_level |= level; - } - - virtual void fromServer(const ros::NodeHandle &nh, ProsilicaCameraConfig &config) const - { - nh.getParam(name, config.*field); - } - - virtual void toServer(const ros::NodeHandle &nh, const ProsilicaCameraConfig &config) const - { - nh.setParam(name, config.*field); - } - - virtual bool fromMessage(const dynamic_reconfigure::Config &msg, ProsilicaCameraConfig &config) const - { - return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field); - } - - virtual void toMessage(dynamic_reconfigure::Config &msg, const ProsilicaCameraConfig &config) const - { - dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field); - } - - virtual void getValue(const ProsilicaCameraConfig &config, boost::any &val) const - { - val = config.*field; - } - }; - - class AbstractGroupDescription : public dynamic_reconfigure::Group - { - public: - AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s) - { - name = n; - type = t; - parent = p; - state = s; - id = i; - } - - std::vector abstract_parameters; - bool state; - - virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0; - virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0; - virtual void updateParams(boost::any &cfg, ProsilicaCameraConfig &top) const= 0; - virtual void setInitialState(boost::any &cfg) const = 0; - - - void convertParams() - { - for(std::vector::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); ++i) - { - parameters.push_back(dynamic_reconfigure::ParamDescription(**i)); - } - } - }; - - typedef boost::shared_ptr AbstractGroupDescriptionPtr; - typedef boost::shared_ptr AbstractGroupDescriptionConstPtr; - - // Final keyword added to class because it has virtual methods and inherits - // from a class with a non-virtual destructor. - template - class GroupDescription DYNAMIC_RECONFIGURE_FINAL : public AbstractGroupDescription - { - public: - GroupDescription(std::string a_name, std::string a_type, int a_parent, int a_id, bool a_s, T PT::* a_f) : AbstractGroupDescription(a_name, a_type, a_parent, a_id, a_s), field(a_f) - { - } - - GroupDescription(const GroupDescription& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups) - { - parameters = g.parameters; - abstract_parameters = g.abstract_parameters; - } - - virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const - { - PT* config = boost::any_cast(cfg); - if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field)) - return false; - - for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) - { - boost::any n = &((*config).*field); - if(!(*i)->fromMessage(msg, n)) - return false; - } - - return true; - } - - virtual void setInitialState(boost::any &cfg) const - { - PT* config = boost::any_cast(cfg); - T* group = &((*config).*field); - group->state = state; - - for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) - { - boost::any n = boost::any(&((*config).*field)); - (*i)->setInitialState(n); - } - - } - - virtual void updateParams(boost::any &cfg, ProsilicaCameraConfig &top) const - { - PT* config = boost::any_cast(cfg); - - T* f = &((*config).*field); - f->setParams(top, abstract_parameters); - - for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) - { - boost::any n = &((*config).*field); - (*i)->updateParams(n, top); - } - } - - virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const - { - const PT config = boost::any_cast(cfg); - dynamic_reconfigure::ConfigTools::appendGroup(msg, name, id, parent, config.*field); - - for(std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) - { - (*i)->toMessage(msg, config.*field); - } - } - - T (PT::* field); - std::vector groups; - }; - -class DEFAULT -{ - public: - DEFAULT() - { - state = true; - name = "Default"; - } - - void setParams(ProsilicaCameraConfig &config, const std::vector params) - { - printf("setParams!\n"); - for (std::vector::const_iterator _i = params.begin(); _i != params.end(); ++_i) - { - boost::any val; - (*_i)->getValue(config, val); - - if("trigger_mode"==(*_i)->name){trigger_mode = boost::any_cast(val);} - if("auto_exposure"==(*_i)->name){auto_exposure = boost::any_cast(val);} - if("exposure"==(*_i)->name){exposure = boost::any_cast(val);} - if("auto_gain"==(*_i)->name){auto_gain = boost::any_cast(val);} - if("gain"==(*_i)->name){gain = boost::any_cast(val);} - if("auto_whitebalance"==(*_i)->name){auto_whitebalance = boost::any_cast(val);} - if("whitebalance_red"==(*_i)->name){whitebalance_red = boost::any_cast(val);} - if("whitebalance_blue"==(*_i)->name){whitebalance_blue = boost::any_cast(val);} - if("binning_x"==(*_i)->name){binning_x = boost::any_cast(val);} - if("binning_y"==(*_i)->name){binning_y = boost::any_cast(val);} - if("x_offset"==(*_i)->name){x_offset = boost::any_cast(val);} - if("y_offset"==(*_i)->name){y_offset = boost::any_cast(val);} - if("width"==(*_i)->name){width = boost::any_cast(val);} - if("height"==(*_i)->name){height = boost::any_cast(val);} - if("frame_id"==(*_i)->name){frame_id = boost::any_cast(val);} - if("trig_timestamp_topic"==(*_i)->name){trig_timestamp_topic = boost::any_cast(val);} - if("trig_rate"==(*_i)->name){trig_rate = boost::any_cast(val);} - if("auto_adjust_stream_bytes_per_second"==(*_i)->name){auto_adjust_stream_bytes_per_second = boost::any_cast(val);} - if("stream_bytes_per_second"==(*_i)->name){stream_bytes_per_second = boost::any_cast(val);} - if("exposure_auto_max"==(*_i)->name){exposure_auto_max = boost::any_cast(val);} - if("exposure_auto_target"==(*_i)->name){exposure_auto_target = boost::any_cast(val);} - if("gain_auto_max"==(*_i)->name){gain_auto_max = boost::any_cast(val);} - if("gain_auto_target"==(*_i)->name){gain_auto_target = boost::any_cast(val);} - } - } - - std::string trigger_mode; -bool auto_exposure; -double exposure; -bool auto_gain; -int gain; -bool auto_whitebalance; -int whitebalance_red; -int whitebalance_blue; -int binning_x; -int binning_y; -int x_offset; -int y_offset; -int width; -int height; -std::string frame_id; -std::string trig_timestamp_topic; -double trig_rate; -bool auto_adjust_stream_bytes_per_second; -int stream_bytes_per_second; -double exposure_auto_max; -int exposure_auto_target; -int gain_auto_max; -int gain_auto_target; - - bool state; - std::string name; - - -}groups; - - - -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - std::string trigger_mode; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - bool auto_exposure; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - double exposure; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - bool auto_gain; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int gain; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - bool auto_whitebalance; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int whitebalance_red; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int whitebalance_blue; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int binning_x; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int binning_y; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int x_offset; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int y_offset; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int width; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int height; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - std::string frame_id; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - std::string trig_timestamp_topic; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - double trig_rate; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - bool auto_adjust_stream_bytes_per_second; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int stream_bytes_per_second; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - double exposure_auto_max; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int exposure_auto_target; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int gain_auto_max; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - int gain_auto_target; -//#line 228 "/opt/ros/kinetic/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" - - bool __fromMessage__(dynamic_reconfigure::Config &msg) - { - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - const std::vector &__group_descriptions__ = __getGroupDescriptions__(); - - int count = 0; - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - if ((*i)->fromMessage(msg, *this)) - count++; - - for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++) - { - if ((*i)->id == 0) - { - boost::any n = boost::any(this); - (*i)->updateParams(n, *this); - (*i)->fromMessage(msg, n); - } - } - - if (count != dynamic_reconfigure::ConfigTools::size(msg)) - { - ROS_ERROR("ProsilicaCameraConfig::__fromMessage__ called with an unexpected parameter."); - ROS_ERROR("Booleans:"); - for (unsigned int i = 0; i < msg.bools.size(); i++) - ROS_ERROR(" %s", msg.bools[i].name.c_str()); - ROS_ERROR("Integers:"); - for (unsigned int i = 0; i < msg.ints.size(); i++) - ROS_ERROR(" %s", msg.ints[i].name.c_str()); - ROS_ERROR("Doubles:"); - for (unsigned int i = 0; i < msg.doubles.size(); i++) - ROS_ERROR(" %s", msg.doubles[i].name.c_str()); - ROS_ERROR("Strings:"); - for (unsigned int i = 0; i < msg.strs.size(); i++) - ROS_ERROR(" %s", msg.strs[i].name.c_str()); - // @todo Check that there are no duplicates. Make this error more - // explicit. - return false; - } - return true; - } - - // This version of __toMessage__ is used during initialization of - // statics when __getParamDescriptions__ can't be called yet. - void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector &__param_descriptions__, const std::vector &__group_descriptions__) const - { - dynamic_reconfigure::ConfigTools::clear(msg); - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - (*i)->toMessage(msg, *this); - - for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) - { - if((*i)->id == 0) - { - (*i)->toMessage(msg, *this); - } - } - } - - void __toMessage__(dynamic_reconfigure::Config &msg) const - { - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - const std::vector &__group_descriptions__ = __getGroupDescriptions__(); - __toMessage__(msg, __param_descriptions__, __group_descriptions__); - } - - void __toServer__(const ros::NodeHandle &nh) const - { - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - (*i)->toServer(nh, *this); - } - - void __fromServer__(const ros::NodeHandle &nh) - { - static bool setup=false; - - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - (*i)->fromServer(nh, *this); - - const std::vector &__group_descriptions__ = __getGroupDescriptions__(); - for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){ - if (!setup && (*i)->id == 0) { - setup = true; - boost::any n = boost::any(this); - (*i)->setInitialState(n); - } - } - } - - void __clamp__() - { - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - const ProsilicaCameraConfig &__max__ = __getMax__(); - const ProsilicaCameraConfig &__min__ = __getMin__(); - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - (*i)->clamp(*this, __max__, __min__); - } - - uint32_t __level__(const ProsilicaCameraConfig &config) const - { - const std::vector &__param_descriptions__ = __getParamDescriptions__(); - uint32_t level = 0; - for (std::vector::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); ++i) - (*i)->calcLevel(level, config, *this); - return level; - } - - static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__(); - static const ProsilicaCameraConfig &__getDefault__(); - static const ProsilicaCameraConfig &__getMax__(); - static const ProsilicaCameraConfig &__getMin__(); - static const std::vector &__getParamDescriptions__(); - static const std::vector &__getGroupDescriptions__(); - - private: - static const ProsilicaCameraConfigStatics *__get_statics__(); - }; - - template <> // Max and min are ignored for strings. - inline void ProsilicaCameraConfig::ParamDescription::clamp(ProsilicaCameraConfig &config, const ProsilicaCameraConfig &max, const ProsilicaCameraConfig &min) const - { - (void) config; - (void) min; - (void) max; - return; - } - - class ProsilicaCameraConfigStatics - { - friend class ProsilicaCameraConfig; - - ProsilicaCameraConfigStatics() - { -ProsilicaCameraConfig::GroupDescription Default("Default", "", 0, 0, true, &ProsilicaCameraConfig::groups); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.trigger_mode = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.trigger_mode = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.trigger_mode = "streaming"; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trigger_mode", "str", 1, "Camera trigger mode", "{'enum_description': 'Enum to set the trigger mode', 'enum': [{'srcline': 10, 'description': 'Run at maximum frame rate', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'streaming', 'ctype': 'std::string', 'type': 'str', 'name': 'StreamingMode'}, {'srcline': 11, 'description': 'Capture frame in response to service call', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'polled', 'ctype': 'std::string', 'type': 'str', 'name': 'PolledMode'}, {'srcline': 12, 'description': 'Fixed Rate mode', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'fixedrate', 'ctype': 'std::string', 'type': 'str', 'name': 'FixedRateMode'}, {'srcline': 13, 'description': 'External trigger on SyncIn1 line', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'syncin1', 'ctype': 'std::string', 'type': 'str', 'name': 'External1Mode'}, {'srcline': 14, 'description': 'External trigger on SyncIn2 line', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'syncin2', 'ctype': 'std::string', 'type': 'str', 'name': 'External2Mode'}]}", &ProsilicaCameraConfig::trigger_mode))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trigger_mode", "str", 1, "Camera trigger mode", "{'enum_description': 'Enum to set the trigger mode', 'enum': [{'srcline': 10, 'description': 'Run at maximum frame rate', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'streaming', 'ctype': 'std::string', 'type': 'str', 'name': 'StreamingMode'}, {'srcline': 11, 'description': 'Capture frame in response to service call', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'polled', 'ctype': 'std::string', 'type': 'str', 'name': 'PolledMode'}, {'srcline': 12, 'description': 'Fixed Rate mode', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'fixedrate', 'ctype': 'std::string', 'type': 'str', 'name': 'FixedRateMode'}, {'srcline': 13, 'description': 'External trigger on SyncIn1 line', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'syncin1', 'ctype': 'std::string', 'type': 'str', 'name': 'External1Mode'}, {'srcline': 14, 'description': 'External trigger on SyncIn2 line', 'srcfile': '/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg', 'cconsttype': 'const char * const', 'value': 'syncin2', 'ctype': 'std::string', 'type': 'str', 'name': 'External2Mode'}]}", &ProsilicaCameraConfig::trigger_mode))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.auto_exposure = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.auto_exposure = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.auto_exposure = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_exposure", "bool", 0, "Sets the camera exposure duration to automatic. Causes the `~exposure` setting to be ignored.", "", &ProsilicaCameraConfig::auto_exposure))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_exposure", "bool", 0, "Sets the camera exposure duration to automatic. Causes the `~exposure` setting to be ignored.", "", &ProsilicaCameraConfig::auto_exposure))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.exposure = 2.5e-05; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.exposure = 60.0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.exposure = 0.025; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure", "double", 0, "Camera exposure time in seconds.", "", &ProsilicaCameraConfig::exposure))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure", "double", 0, "Camera exposure time in seconds.", "", &ProsilicaCameraConfig::exposure))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.auto_gain = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.auto_gain = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.auto_gain = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_gain", "bool", 0, "Sets the analog gain to automatic. Causes the `~gain` setting to be ignored.", "", &ProsilicaCameraConfig::auto_gain))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_gain", "bool", 0, "Sets the analog gain to automatic. Causes the `~gain` setting to be ignored.", "", &ProsilicaCameraConfig::auto_gain))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.gain = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.gain = 24; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.gain = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain", "int", 0, "The gain level in dB.", "", &ProsilicaCameraConfig::gain))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain", "int", 0, "The gain level in dB.", "", &ProsilicaCameraConfig::gain))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.auto_whitebalance = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.auto_whitebalance = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.auto_whitebalance = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_whitebalance", "bool", 0, "Whether whitebalance will continuously adjust to the current scene. Causes the `~whitebalance_red` and `~whitebalance_blue` settings to be ignored.", "", &ProsilicaCameraConfig::auto_whitebalance))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_whitebalance", "bool", 0, "Whether whitebalance will continuously adjust to the current scene. Causes the `~whitebalance_red` and `~whitebalance_blue` settings to be ignored.", "", &ProsilicaCameraConfig::auto_whitebalance))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.whitebalance_red = 80; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.whitebalance_red = 300; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.whitebalance_red = 100; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("whitebalance_red", "int", 0, "Red gain as a percentage of the camera default setting.", "", &ProsilicaCameraConfig::whitebalance_red))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("whitebalance_red", "int", 0, "Red gain as a percentage of the camera default setting.", "", &ProsilicaCameraConfig::whitebalance_red))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.whitebalance_blue = 80; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.whitebalance_blue = 300; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.whitebalance_blue = 100; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("whitebalance_blue", "int", 0, "Blue gain as a percentage of the camera default setting.", "", &ProsilicaCameraConfig::whitebalance_blue))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("whitebalance_blue", "int", 0, "Blue gain as a percentage of the camera default setting.", "", &ProsilicaCameraConfig::whitebalance_blue))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.binning_x = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.binning_x = 8; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.binning_x = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("binning_x", "int", 0, "Number of pixels to bin together horizontally.", "", &ProsilicaCameraConfig::binning_x))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("binning_x", "int", 0, "Number of pixels to bin together horizontally.", "", &ProsilicaCameraConfig::binning_x))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.binning_y = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.binning_y = 14; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.binning_y = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("binning_y", "int", 0, "Number of pixels to bin together vertically.", "", &ProsilicaCameraConfig::binning_y))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("binning_y", "int", 0, "Number of pixels to bin together vertically.", "", &ProsilicaCameraConfig::binning_y))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.x_offset = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.x_offset = 2447; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.x_offset = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("x_offset", "int", 0, "X offset of the region of interest.", "", &ProsilicaCameraConfig::x_offset))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("x_offset", "int", 0, "X offset of the region of interest.", "", &ProsilicaCameraConfig::x_offset))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.y_offset = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.y_offset = 2049; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.y_offset = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("y_offset", "int", 0, "Y offset of the region of interest.", "", &ProsilicaCameraConfig::y_offset))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("y_offset", "int", 0, "Y offset of the region of interest.", "", &ProsilicaCameraConfig::y_offset))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.width = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.width = 2448; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.width = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("width", "int", 0, "Width of the region of interest (0 for automatic).", "", &ProsilicaCameraConfig::width))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("width", "int", 0, "Width of the region of interest (0 for automatic).", "", &ProsilicaCameraConfig::width))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.height = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.height = 2050; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.height = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("height", "int", 0, "Height of the region of interest (0 for automatic).", "", &ProsilicaCameraConfig::height))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("height", "int", 0, "Height of the region of interest (0 for automatic).", "", &ProsilicaCameraConfig::height))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.frame_id = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.frame_id = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.frame_id = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("frame_id", "str", 0, "The optical camera TF frame set in message headers.", "", &ProsilicaCameraConfig::frame_id))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("frame_id", "str", 0, "The optical camera TF frame set in message headers.", "", &ProsilicaCameraConfig::frame_id))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.trig_timestamp_topic = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.trig_timestamp_topic = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.trig_timestamp_topic = ""; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trig_timestamp_topic", "str", 1, "Sets the topic from which an externally trigged camera receives its trigger timestamps.", "", &ProsilicaCameraConfig::trig_timestamp_topic))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trig_timestamp_topic", "str", 1, "Sets the topic from which an externally trigged camera receives its trigger timestamps.", "", &ProsilicaCameraConfig::trig_timestamp_topic))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.trig_rate = 1.0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.trig_rate = 100.0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.trig_rate = 15.0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trig_rate", "double", 0, "Sets the expected triggering rate in externally triggered mode.", "", &ProsilicaCameraConfig::trig_rate))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("trig_rate", "double", 0, "Sets the expected triggering rate in externally triggered mode.", "", &ProsilicaCameraConfig::trig_rate))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.auto_adjust_stream_bytes_per_second = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.auto_adjust_stream_bytes_per_second = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.auto_adjust_stream_bytes_per_second = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_adjust_stream_bytes_per_second", "bool", 0, "Whether the node should automatically adjust the data rate. Causes `~stream_bytes_per_second` to be ignored.", "", &ProsilicaCameraConfig::auto_adjust_stream_bytes_per_second))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("auto_adjust_stream_bytes_per_second", "bool", 0, "Whether the node should automatically adjust the data rate. Causes `~stream_bytes_per_second` to be ignored.", "", &ProsilicaCameraConfig::auto_adjust_stream_bytes_per_second))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.stream_bytes_per_second = 1; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.stream_bytes_per_second = 115000000; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.stream_bytes_per_second = 45000000; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("stream_bytes_per_second", "int", 0, "Limits the data rate of the camera.", "", &ProsilicaCameraConfig::stream_bytes_per_second))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("stream_bytes_per_second", "int", 0, "Limits the data rate of the camera.", "", &ProsilicaCameraConfig::stream_bytes_per_second))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.exposure_auto_max = 2.5e-05; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.exposure_auto_max = 60.0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.exposure_auto_max = 0.5; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure_auto_max", "double", 0, "The max exposure time in auto exposure mode, in seconds.", "", &ProsilicaCameraConfig::exposure_auto_max))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure_auto_max", "double", 0, "The max exposure time in auto exposure mode, in seconds.", "", &ProsilicaCameraConfig::exposure_auto_max))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.exposure_auto_target = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.exposure_auto_target = 100; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.exposure_auto_target = 50; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure_auto_target", "int", 0, "The auto exposure target mean value as a percentage, from 0=black to 100=white.", "", &ProsilicaCameraConfig::exposure_auto_target))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("exposure_auto_target", "int", 0, "The auto exposure target mean value as a percentage, from 0=black to 100=white.", "", &ProsilicaCameraConfig::exposure_auto_target))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.gain_auto_max = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.gain_auto_max = 24; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.gain_auto_max = 24; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain_auto_max", "int", 0, "The max gain level in auto gain mode, in dB.", "", &ProsilicaCameraConfig::gain_auto_max))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain_auto_max", "int", 0, "The max gain level in auto gain mode, in dB.", "", &ProsilicaCameraConfig::gain_auto_max))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __min__.gain_auto_target = 0; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __max__.gain_auto_target = 100; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __default__.gain_auto_target = 50; -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.abstract_parameters.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain_auto_target", "int", 0, "The auto gain target mean value as a percentage, from 0=black to 100=white.", "", &ProsilicaCameraConfig::gain_auto_target))); -//#line 290 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __param_descriptions__.push_back(ProsilicaCameraConfig::AbstractParamDescriptionConstPtr(new ProsilicaCameraConfig::ParamDescription("gain_auto_target", "int", 0, "The auto gain target mean value as a percentage, from 0=black to 100=white.", "", &ProsilicaCameraConfig::gain_auto_target))); -//#line 245 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - Default.convertParams(); -//#line 245 "/opt/ros/kinetic/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" - __group_descriptions__.push_back(ProsilicaCameraConfig::AbstractGroupDescriptionConstPtr(new ProsilicaCameraConfig::GroupDescription(Default))); -//#line 366 "/opt/ros/kinetic/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" - - for (std::vector::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); ++i) - { - __description_message__.groups.push_back(**i); - } - __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__); - __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__); - __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__); - } - std::vector __param_descriptions__; - std::vector __group_descriptions__; - ProsilicaCameraConfig __max__; - ProsilicaCameraConfig __min__; - ProsilicaCameraConfig __default__; - dynamic_reconfigure::ConfigDescription __description_message__; - - static const ProsilicaCameraConfigStatics *get_instance() - { - // Split this off in a separate function because I know that - // instance will get initialized the first time get_instance is - // called, and I am guaranteeing that get_instance gets called at - // most once. - static ProsilicaCameraConfigStatics instance; - return &instance; - } - }; - - inline const dynamic_reconfigure::ConfigDescription &ProsilicaCameraConfig::__getDescriptionMessage__() - { - return __get_statics__()->__description_message__; - } - - inline const ProsilicaCameraConfig &ProsilicaCameraConfig::__getDefault__() - { - return __get_statics__()->__default__; - } - - inline const ProsilicaCameraConfig &ProsilicaCameraConfig::__getMax__() - { - return __get_statics__()->__max__; - } - - inline const ProsilicaCameraConfig &ProsilicaCameraConfig::__getMin__() - { - return __get_statics__()->__min__; - } - - inline const std::vector &ProsilicaCameraConfig::__getParamDescriptions__() - { - return __get_statics__()->__param_descriptions__; - } - - inline const std::vector &ProsilicaCameraConfig::__getGroupDescriptions__() - { - return __get_statics__()->__group_descriptions__; - } - - inline const ProsilicaCameraConfigStatics *ProsilicaCameraConfig::__get_statics__() - { - const static ProsilicaCameraConfigStatics *statics; - - if (statics) // Common case - return statics; - - boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__); - - if (statics) // In case we lost a race. - return statics; - - statics = ProsilicaCameraConfigStatics::get_instance(); - - return statics; - } - -//#line 10 "/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg" - const char * const ProsilicaCamera_StreamingMode = "streaming"; -//#line 11 "/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg" - const char * const ProsilicaCamera_PolledMode = "polled"; -//#line 12 "/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg" - const char * const ProsilicaCamera_FixedRateMode = "fixedrate"; -//#line 13 "/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg" - const char * const ProsilicaCamera_External1Mode = "syncin1"; -//#line 14 "/tmp/binarydeb/ros-kinetic-prosilica-camera-1.9.4/cfg/ProsilicaCamera.cfg" - const char * const ProsilicaCamera_External2Mode = "syncin2"; -} - -#undef DYNAMIC_RECONFIGURE_FINAL - -#endif // __PROSILICACAMERARECONFIGURATOR_H__ diff --git a/src/cams/prosilica_camera/launch/generic.launch b/src/cams/prosilica_camera/launch/generic.launch deleted file mode 100644 index d05b0ad5..00000000 --- a/src/cams/prosilica_camera/launch/generic.launch +++ /dev/null @@ -1,78 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/prosilica_camera/launch/prosilica.launch b/src/cams/prosilica_camera/launch/prosilica.launch deleted file mode 100644 index 5aa9a08a..00000000 --- a/src/cams/prosilica_camera/launch/prosilica.launch +++ /dev/null @@ -1,92 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/prosilica_camera/launch/prosilica.launch.xml b/src/cams/prosilica_camera/launch/prosilica.launch.xml new file mode 100644 index 00000000..739675e5 --- /dev/null +++ b/src/cams/prosilica_camera/launch/prosilica.launch.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cams/prosilica_camera/package.xml b/src/cams/prosilica_camera/package.xml index 110a6cdc..7c47a758 100644 --- a/src/cams/prosilica_camera/package.xml +++ b/src/cams/prosilica_camera/package.xml @@ -1,56 +1,32 @@ - + + + prosilica_camera - 1.9.4 - A ROS driver node for AVT/Prosilica Gigabit Ethernet (GigE) cameras. - Austin Hendrix + 2.0.0 + A ROS2 driver node for AVT/Prosilica Gigabit Ethernet (GigE) cameras. + Adam Romlein BSD http://www.ros.org/wiki/prosilica_camera - - Maintained by William Woodall - wwoodall@willowgarage.com - Contributions by Allison Thackston - allison.thackston@nasa.gov - - catkin - - prosilica_gige_sdk - roscpp - phase_one - std_msgs - custom_msgs - sensor_msgs - diagnostic_updater - diagnostic_msgs - image_transport - self_test - dynamic_reconfigure - camera_calibration_parsers - polled_camera - rosconsole - nodelet - nodelet_topic_tools - roskv - - prosilica_gige_sdk - roscpp - phase_one - std_msgs - custom_msgs - sensor_msgs - diagnostic_updater - diagnostic_msgs - image_transport - self_test - dynamic_reconfigure - camera_calibration_parsers - polled_camera - nodelet - nodelet_topic_tools - roskv + ament_cmake + + rclcpp + std_msgs + std_srvs + sensor_msgs + custom_msgs + image_transport + camera_calibration_parsers + cv_bridge + cam_utils + roskv + prosilica_gige_sdk + libopencv-dev + nlohmann-json-dev - + ament_cmake - diff --git a/src/cams/prosilica_camera/plugins/nodelet_plugins.xml b/src/cams/prosilica_camera/plugins/nodelet_plugins.xml deleted file mode 100644 index b60b4a14..00000000 --- a/src/cams/prosilica_camera/plugins/nodelet_plugins.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - A nodelet version of the prosilica camera driver - - - - - Nodelet for displaying parts of images. - - - - diff --git a/src/cams/prosilica_camera/profiling.txt b/src/cams/prosilica_camera/profiling.txt deleted file mode 100644 index 4f0221de..00000000 --- a/src/cams/prosilica_camera/profiling.txt +++ /dev/null @@ -1,74 +0,0 @@ -Profiling Notes on Nuvo0 at 0.5Hz: - operation size(B) time(sec) speed(Byte/sec) - resizedImage 28829184 0.00967016 2981252016.513 Byte/s - memcpy 28829184 0.00283331 10175089912.505 Byte/s - resizedImage 28829184 0.00979254 2943994510.107 Byte/s - memcpy 28829184 0.00261958 11005269547.027 Byte/s - resizedImage 28829184 0.01008175 2859541647.035 Byte/s - memcpy 28829184 0.00287129 10040498869.846 Byte/s - resizedImage 28829184 0.01014159 2842669048.936 Byte/s - memcpy 28829184 0.00301845 9550989415.097 Byte/s - resizedImage 28829184 0.01124186 2564449655.128 Byte/s - memcpy 28829184 0.00265764 10847663340.407 Byte/s - resizedImage 28829184 0.01093399 2636657249.549 Byte/s - memcpy 28829184 0.00290637 9919309654.311 Byte/s - resizedImage 28829184 0.00940134 3066497329.104 Byte/s - memcpy 28829184 0.00280530 10276684846.540 Byte/s - resizedImage 28829184 0.01180290 2442550898.508 Byte/s - memcpy 28829184 0.00283329 10175161737.768 Byte/s - resizedImage 28829184 0.01005468 2867240329.876 Byte/s - memcpy 28829184 0.00313507 9195706634.940 Byte/s - resizedImage 28829184 0.01102009 2616057037.647 Byte/s - memcpy 28829184 0.00318394 9054562585.978 Byte/s - resizedImage 28829184 0.01127832 2556159428.000 Byte/s - memcpy 28829184 0.00295354 9760891675.752 Byte/s - resizedImage 28829184 0.00991902 2906454871.550 Byte/s - memcpy 28829184 0.00266386 10822334507.069 Byte/s - resizedImage 28829184 0.01034837 2785867146.227 Byte/s - memcpy 28829184 0.00282459 10206502182.618 Byte/s - resizedImage 28829184 0.01030451 2797724879.689 Byte/s - memcpy 28829184 0.00270990 10638467840.142 Byte/s - -Averages (1e9byte/sec) -resizedImage 2.776223 -memcpy 10.119224 - -Ramdisk read/write: -jpg BGR8: -dumped 86487552 in 0.156 -read 86487552 in 0.141 - -bmp mono8: -dumped 28829184 in 0.026 -read 86487552 in 0.051 - -bmp mono8 IMREAD_UNCHANGED -dumped 28829184 in 0.026 -read 28829184 in 0.026 - -SSD: -jpg mono8: -dumped 28829184 in 0.095 -read 28829184 in 0.056 - -jpg bgr8 -dumped 86487552 in 0.160 -read 86487552 in 0.163 - -bayer8 transit to python node: -0.030 - -Event mapping -EventsEnable1 - EventID -4 - 40002 - EventFrameTrigger -5 - 40002 -... -8 - 40003 - EventExposureEnd -9 - 40003 -... -12 - 40002 + 40003 -15 - 40002 + 40003 -16 - ? -31 - 40002 + 40003 -63 - 40002 + 40003 - diff --git a/src/cams/prosilica_camera/src/libprosilica/prosilica.cpp b/src/cams/prosilica_camera/src/libprosilica/prosilica.cpp index ed6d9fb7..d2d2221c 100644 --- a/src/cams/prosilica_camera/src/libprosilica/prosilica.cpp +++ b/src/cams/prosilica_camera/src/libprosilica/prosilica.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include using namespace sw; @@ -59,10 +59,14 @@ do { \ Watchdog::Watchdog() : failCallback_{cb_fail_shutdown} {} Watchdog::Watchdog(double lookback_period, double health_threshold) : - lookback_period_{lookback_period}, + lookback_period_{rclcpp::Duration::from_seconds(lookback_period)}, health_threshold{health_threshold}, failCallback_{cb_fail_shutdown} {} +rclcpp::Time Watchdog::now() { + return clock_.now(); +} + /**Start the watchdog. Events are ignored until started. * */ @@ -75,10 +79,12 @@ void Watchdog::Start() { * * @param t - delay (seconds) */ -void Watchdog::DelayedStart(ros::NodeHandlePtr nhp, double t) { - delayStartTimer_ = nhp->createTimer(ros::Duration{t}, [this](ros::TimerEvent const &e ) { - this->Start(); - }, true, true); +void Watchdog::DelayedStart(rclcpp::Node::SharedPtr nhp, double t) { + delayStartTimer_ = nhp->create_wall_timer( + std::chrono::duration(t), [this]() { + delayStartTimer_->cancel(); + this->Start(); + }); } void Watchdog::Stop() { @@ -89,13 +95,13 @@ bool Watchdog::Ok() { return this->computeHealth() > health_threshold; } -void Watchdog::push_back(ros::Time const &t, double val) { +void Watchdog::push_back(rclcpp::Time const &t, double val) { std::lock_guard guard(mutex_); - if (!t.isValid()) { + if (t.nanoseconds() == 0) { ROS_ERROR("zero/invalid time encountered in Watchdog::push_back()"); return; } - array.emplace_back(std::pair(t, val)); + array.emplace_back(std::pair(t, val)); } int Watchdog::size() { @@ -106,14 +112,14 @@ int Watchdog::size() { void Watchdog::show() { std::lock_guard guard(mutex_); for (const auto pair : array) { - std::cout << pair.first << ": " << pair.second << std::endl; + std::cout << pair.first.seconds() << ": " << pair.second << std::endl; } std::cout << "\n---" << std::endl; } void Watchdog::purge() { std::lock_guard guard(mutex_); - auto now = ros::Time::now(); + auto now = clock_.now(); std::size_t index = 0; for (const auto pair : array) { auto age = now - pair.first ; @@ -127,7 +133,7 @@ void Watchdog::purge() { void Watchdog::pet() { ROS_INFO("Pet the watchdog"); - this->push_back(ros::Time::now(), 1.0); + this->push_back(now(), 1.0); } void Watchdog::kick() { @@ -135,7 +141,7 @@ void Watchdog::kick() { return; } ROS_WARN("kicked the watchdog"); - this->push_back(ros::Time::now(), -1.0); + this->push_back(now(), -1.0); } void Watchdog::check() { @@ -159,14 +165,13 @@ double Watchdog::computeHealth() { return accu / total; } -void Watchdog::setFailCallback(ros::TimerCallback callback) { +void Watchdog::setFailCallback(std::function callback) { failCallback_ = callback; } void Watchdog::callFail() { ROS_ERROR("failed health check"); - ros::TimerEvent e; - failCallback_(e); + failCallback_(); } namespace prosilica { @@ -199,18 +204,18 @@ namespace prosilica { } } - ros::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo) { + rclcpp::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo) { return CvtPvTimestamp(timehi, timelo, 1000000000); } - ros::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo, uint32_t freq) { + rclcpp::Time CvtPvTimestamp(uint32_t timehi, uint32_t timelo, uint32_t freq) { uint64_t utime = ((uint64_t )timehi) << 32; utime = utime + (uint64_t )timelo; double dtime = double (utime) / (double) freq; if (dtime <= 0) { ROS_ERROR("Calculated time is non-positive: %lf", dtime); - return ros::Time{}; + return rclcpp::Time{0, 0, RCL_ROS_TIME}; } - return ros::Time{dtime}; + return rclcpp::Time{(int64_t)(dtime * 1e9), RCL_ROS_TIME}; } @@ -371,25 +376,6 @@ static void openCamera(boost::function info_fn, } - void OneShotManager::erase(boost::uuids::uuid i) { - timer_map.erase(i); -// std::cout << "erasing: " << i << " sz: " << timer_map.size() <createTimer(period, cb2, true, false); - timer_map.emplace(i, tmp); - tmp.start(); // safety here, need to ensure it's in the map before it pops - return i; - } diff --git a/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp b/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp index d2d0f299..74042a63 100644 --- a/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp +++ b/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp @@ -1,8 +1,6 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,21 +28,1469 @@ * 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. +* +* ROS2 port of the KAMERA prosilica driver (formerly a ROS1 nodelet). +* dynamic_reconfigure is replaced by node parameters applied at startup; +* the unused polled_camera / diagnostics plumbing is gone. *********************************************************************/ -#include -#include +#include +#include +#include +#include -int main(int argc, char** argv) +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "prosilica/prosilica.h" +#include "prosilica/rolling_sum.h" + +#include +#include + +#include + +// custom messages from KAMERA +#include +#include +#include +#include +#include + + +bool prosilica_inited = false; +int num_cameras = 0; + +namespace prosilica_camera { + class ProsilicaDriver; +} + +// Container so we can reference the driver from the signal handler +std::map active_drivers; + +void driver_shutdown(); + +void signalHandler( int signum ) { + ROS_WARN(" Interrupt signal (%d)\n", signum); + driver_shutdown(); + rclcpp::shutdown(); +} + +std::string dumpImageMessage(const sensor_msgs::msg::Image::ConstSharedPtr &received_image, const std::string filename, bool debayer) { - ros::init(argc, argv, "prosilica_node"); - nodelet::Loader manager(true); - nodelet::M_string remappings; - nodelet::V_string my_argv; - manager.load(ros::this_node::getName(), "prosilica_camera/driver", remappings, my_argv); - // Use async spinner that uses all available threads to handle the numerous callbacks - ros::AsyncSpinner spinner(0); - spinner.start(); - ros::waitForShutdown(); + std::vector compression_params; + compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); + compression_params.push_back(100); + cv_bridge::CvImagePtr cvPtr; + if (debayer) { + cvPtr = cv_bridge::toCvCopy(received_image, sensor_msgs::image_encodings::BGR8); + } else { + cvPtr = cv_bridge::toCvCopy(received_image, received_image->encoding); + } + + std::filesystem::path path_filename{filename}; + try { + std::filesystem::create_directories(path_filename.parent_path()); + cv::imwrite(filename, cvPtr->image, compression_params); + } catch (std::filesystem::filesystem_error &e) { + ROS_ERROR("Archive Failed [%d]: %s", e.code().value(), e.what()); + return ""; + } + if (!std::filesystem::exists(path_filename)) { + ROS_ERROR("Failed to create file"); + } + return filename; } +/** === === === === === === === === === === === === */ + +static const char* camera_channels[] = {"rgb", "ir", "uv"}; +static std::map camera_delays{{"rgb", 0.423}, {"ir", 0.003}, {"uv", 0.289}}; + +namespace prosilica_camera +{ + std::map pv_error_codes = { + {0, "ePvErrSuccess, No error"}, + {1, "ePvErrCameraFault, Unexpected camera fault"}, + {2, "ePvErrInternalFault, Unexpected fault in PvApi or driver"}, + {3, "ePvErrBadHandle, Camera handle is invalid"}, + {4, "ePvErrBadParameter, Bad parameter to API call"}, + {5, "ePvErrBadSequence, Sequence of API calls is incorrect"}, + {6, "ePvErrNotFound, Camera or attribute not found"}, + {7, "ePvErrAccessDenied, Camera cannot be opened in the specified mode"}, + {8, "ePvErrUnplugged, Camera was unplugged"}, + {9, "ePvErrInvalidSetup, Setup is invalid (an attribute is invalid)"}, + {10, "ePvErrResources, System/network resources or memory not available"}, + {11, "ePvErrBandwidth, 1394 bandwidth not available"}, + {12, "ePvErrQueueFull, Too many frames on queue"}, + {13, "ePvErrBufferTooSmall, Frame buffer is too small"}, + {14, "ePvErrCancelled, Frame cancelled by user"}, + {15, "ePvErrDataLost, The data for the frame was lost"}, + {16, "ePvErrDataMissing, Some data in the frame is missing"}, + {17, "ePvErrTimeout, Timeout during wait"}, + {18, "ePvErrOutOfRange, Attribute value is out of the expected range"}, + {19, "ePvErrWrongType, Attribute is not this type (wrong access function)"}, + {20, "ePvErrForbidden, Attribute write forbidden at this time"}, + {21, "ePvErrUnavailable, Attribute is not available at this time"}, + {22, "ePvErrFirewall, A firewall is blocking the traffic (Windows only)"}, + }; + + /// Static camera configuration, formerly the dynamic_reconfigure config. + struct DriverConfig { + std::string trigger_mode = "fixedrate"; + double trig_rate = 1.0; + bool auto_exposure = true; + double exposure = 0.025; + bool auto_gain = true; + int gain = 0; + bool auto_whitebalance = true; + int whitebalance_red = 100; + int whitebalance_blue = 100; + int binning_x = 1; + int binning_y = 1; + int x_offset = 0; + int y_offset = 0; + int width = 0; + int height = 0; + std::string frame_id = ""; + bool auto_adjust_stream_bytes_per_second = true; + int stream_bytes_per_second = 45000000; + double exposure_auto_max = 0.5; + int exposure_auto_target = 50; + int gain_auto_max = 24; + int gain_auto_target = 50; + }; + + + class ProsilicaDriver +{ + +public: + + ProsilicaDriver(rclcpp::Node::SharedPtr node) + : node_{node}, + auto_adjust_stream_bytes_per_second_(false), + auto_adjust_binning_(false), + count_(0), + frames_dropped_total_(0), frames_completed_total_(0), + frames_dropped_acc_(WINDOW_SIZE), + frames_completed_acc_(WINDOW_SIZE), + packets_missed_total_(0), packets_received_total_(0), + packets_missed_acc_(WINDOW_SIZE), + packets_received_acc_(WINDOW_SIZE) + { + cam_index = num_cameras; + active_drivers[cam_index] = this; + + ++num_cameras; + printf("<> Hi, I am the Prosilica driver\n"); + signal(SIGINT, signalHandler); + signal(SIGTERM, signalHandler); + onInitImpl(); + } + + ~ProsilicaDriver() + { + //! Make sure we interrupt initialization (if it happened to still execute). + init_thread_.interrupt(); + init_thread_.join(); + + if(camera_) + { + camera_->stop(); + camera_.reset(); // must destroy Camera before calling prosilica::fini + } + + active_drivers.erase(cam_index); + --num_cameras; + if(num_cameras<=0) + { + prosilica::fini(); + prosilica_inited = false; + num_cameras = 0; + } + + ROS_WARN("Unloaded prosilica camera with guid %s", hw_id_.c_str()); + } + + void public_stop() { + stop(); + } + +private: + rclcpp::Node::SharedPtr node_; + std::string cam_fov; + std::string cam_channel; + boost::shared_ptr camera_; + boost::thread init_thread_; + rclcpp::TimerBase::SharedPtr update_timer_; + int cam_index; + + image_transport::CameraPublisher image_publisher_; + rclcpp::Publisher::SharedPtr missed_frames_pub_; + rclcpp::Publisher::SharedPtr stat_pub_; + rclcpp::Publisher::SharedPtr errstat_pub_; + rclcpp::Service::SharedPtr set_camera_info_srv_; + rclcpp::Service::SharedPtr get_camera_attr_srv_; + rclcpp::Service::SharedPtr set_camera_attr_srv_; + rclcpp::Service::SharedPtr get_attr_list_srv_; + rclcpp::Service::SharedPtr health_srv_; + rclcpp::Subscription::SharedPtr trigger_sub_; + rclcpp::Subscription::SharedPtr exposure_sub; + rclcpp::Subscription::SharedPtr event_sub; + rclcpp::Subscription::SharedPtr dumper_sub_; + rclcpp::Subscription::SharedPtr shutdown_sub_; + + EventCache event_cache; + Watchdog watchdog; + rclcpp::Duration clock_offset{0, 0}; // offset between system time and camera internal time + std::shared_ptr envoy_; + ArchiverOpts arch_opts_ = ArchiverOpts::from_env(); + + sensor_msgs::msg::Image img_; + sensor_msgs::msg::Image broken_img_; + sensor_msgs::msg::CameraInfo cam_info_; + + custom_msgs::msg::GsofEvt event_; // store the last received event + uint64_t last_published_event_num_ = 0; + + std::string frame_id_; + unsigned long guid_; + std::string hw_id_; + std::string ip_address_; + double open_camera_retry_period_; + std::string trig_timestamp_topic_; + // time last frame was received. Putting this here because frameDone is static + rclcpp::Time frame_recv_time_; + int gvspRetries_; + float gvspResendPercent_; + + double update_rate_; + int trigger_mode_; + bool auto_adjust_stream_bytes_per_second_; + bool auto_adjust_binning_; // allow binning to be requested, otherwise set to 1 + + tPvUint32 sensor_width_, sensor_height_; + tPvUint32 max_binning_x, max_binning_y, dummy; + int count_; + + DriverConfig last_config_; + boost::recursive_mutex config_mutex_; + + // State updater + enum CameraState + { + OPENING, + CAMERA_NOT_FOUND, + FORMAT_ERROR, + ERROR, + OK + }camera_state_; + std::string state_info_; + std::string intrinsics_; + static const int WINDOW_SIZE = 100; // remember previous 5s + unsigned long frames_dropped_total_, frames_completed_total_; + RollingSum frames_dropped_acc_, frames_completed_acc_; + unsigned long packets_missed_total_, packets_received_total_; + RollingSum packets_missed_acc_, packets_received_acc_; + + std::string getName() { + return std::string(node_->get_fully_qualified_name()); + } + + void onInitImpl() + { + //! initialize prosilica if necessary + if(!prosilica_inited) + { + ROS_INFO("Initializing prosilica GIGE API"); + prosilica::init(); + prosilica_inited = true; + } + + //! Retrieve parameters + count_ = 0; + update_rate_=30; + frame_id_ = node_->declare_parameter("frame_id", std::string("/camera_optical_frame")); + ROS_INFO("Loaded param frame_id: %s", frame_id_.c_str()); + + hw_id_ = node_->declare_parameter("guid", std::string("")); + if(hw_id_ == "") + { + guid_ = 0; + } + else + { + guid_ = boost::lexical_cast(hw_id_); + ROS_INFO("Loaded param guid: %lu lu", guid_); + } + + ip_address_ = node_->declare_parameter("ip_address", std::string("")); + ROS_INFO("Loaded ip address: %s", ip_address_.c_str()); + + open_camera_retry_period_ = node_->declare_parameter("open_camera_retry_period", 1.); + ROS_INFO("Retry period: %f", open_camera_retry_period_); + + // load static config (formerly dynamic_reconfigure) + loadConfig(); + + // Setup periodic callback to get new data from the camera (software mode only) + // created on demand in start() + + // Open camera + openCamera(); + + cam_channel = node_->declare_parameter("cam_chan", std::string("")); + cam_fov = node_->declare_parameter("cam_fov", std::string("")); + ROS_INFO("Cameratype: %s/%s", cam_fov.c_str(), cam_channel.c_str()); + + RedisEnvoyOpts envoy_opts = RedisEnvoyOpts::from_env("driver_" + cam_fov + "_" + cam_channel ); + /// Connect with redis param server + std::cout << envoy_opts << " | " << RedisHelper::get_redis_uri() << std::endl; + envoy_ = std::make_shared(envoy_opts); + ROS_WARN("echo: %s", envoy_->echo("Redis connected").c_str()); + std::string ns = node_->get_namespace(); + std::string image_read = ns + "/image_raw"; + ROS_WARN("read topic: %s", image_read.c_str()); + + + // Advertise topics + auto expected_delay = camera_delays[cam_channel]; + event_cache.set_delay(expected_delay); + event_cache.set_tolerance(rclcpp::Duration::from_seconds(0.49)); + image_publisher_ = image_transport::create_camera_publisher(node_.get(), image_read); + missed_frames_pub_ = node_->create_publisher("/missed_frames", 3); + stat_pub_ = node_->create_publisher("/stat", 3); + errstat_pub_ = node_->create_publisher("/errstat", 3); + set_camera_info_srv_ = node_->create_service( + "set_camera_info", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { setCameraInfo(req, rsp); }); + get_camera_attr_srv_ = node_->create_service( + "get_camera_attr", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { getCameraAttrSrv(req, rsp); }); + set_camera_attr_srv_ = node_->create_service( + "set_camera_attr", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { setCameraAttr(req, rsp); }); + get_attr_list_srv_ = node_->create_service( + "get_attr_list", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { getAttrList(req, rsp); }); + health_srv_ = node_->create_service( + "health", + [this](const std::shared_ptr req, + std::shared_ptr rsp) { health(req, rsp); }); + trigger_sub_ = node_->create_subscription( + "trigger", 1, + [this](const std_msgs::msg::Header::ConstSharedPtr msg) { syncInCallback(msg); }); + exposure_sub = node_->create_subscription( + "exposure", 1, + [this](const std_msgs::msg::Int32::ConstSharedPtr msg) { exposureCallback(msg); }); + event_sub = node_->create_subscription( + "/event", 1, + [this](const custom_msgs::msg::GsofEvt::ConstSharedPtr msg) { eventCallback(msg); }); + dumper_sub_ = node_->create_subscription( + image_read, 1, + [this](const sensor_msgs::msg::Image::ConstSharedPtr msg) { dumperCallback(msg); }); + shutdown_sub_ = node_->create_subscription( + "/shutdown", 1, + [](const std_msgs::msg::Int8::ConstSharedPtr msg) { + ROS_INFO("Requesting clean shutdown: %d", msg->data); + rclcpp::shutdown(); + }); + + applyConfig(last_config_, true); + + /** + * These parameters are a double-edged sword. Increasing GvspRetries can decrease risk of dropped frames if + * the system is not under heavy load. Under heavy load and unpredictable conditions, this can cause a + * resend storm that causes no frames to make it. Tune with caution. + */ + gvspRetries_ = node_->declare_parameter("GvspRetries", 3); + gvspResendPercent_ = (float) node_->declare_parameter("GvspResendPercent", 2.0); + + camera_->setAttribute("GvspRetries", (tPvUint32) gvspRetries_); // default: 3.0, too high, and you risk a UDP storm + camera_->setAttribute("GvspResendPercent", (tPvFloat32) gvspResendPercent_); // default: 1.0 + + sensor_msgs::clearImage(broken_img_); + + ROS_GREEN("<> <> <> Cam init complete 1"); + + /// Ignore the first few seconds of frames health-wise since there is a purge + watchdog.DelayedStart(node_, 6.0); + + } + + /** Fill the static config from node parameters (formerly dynamic_reconfigure) */ + void loadConfig() { + DriverConfig c; + c.trigger_mode = node_->declare_parameter("trigger_mode", c.trigger_mode); + c.trig_rate = node_->declare_parameter("trig_rate", c.trig_rate); + c.auto_exposure = node_->declare_parameter("auto_exposure", c.auto_exposure); + c.exposure = node_->declare_parameter("exposure", c.exposure); + // GainMode/GainValue come from the KAMERA config; map onto gain settings + std::string gain_mode = node_->declare_parameter("GainMode", std::string("Auto")); + int gain_value = node_->declare_parameter("GainValue", 0); + c.auto_gain = (gain_mode != "Manual"); + c.gain = gain_value; + c.auto_whitebalance = node_->declare_parameter("auto_whitebalance", c.auto_whitebalance); + c.whitebalance_red = node_->declare_parameter("whitebalance_red", c.whitebalance_red); + c.whitebalance_blue = node_->declare_parameter("whitebalance_blue", c.whitebalance_blue); + c.binning_x = node_->declare_parameter("binning_x", c.binning_x); + c.binning_y = node_->declare_parameter("binning_y", c.binning_y); + c.x_offset = node_->declare_parameter("x_offset", c.x_offset); + c.y_offset = node_->declare_parameter("y_offset", c.y_offset); + c.width = node_->declare_parameter("width", c.width); + c.height = node_->declare_parameter("height", c.height); + c.frame_id = frame_id_; + c.auto_adjust_stream_bytes_per_second = node_->declare_parameter( + "auto_adjust_stream_bytes_per_second", c.auto_adjust_stream_bytes_per_second); + c.stream_bytes_per_second = node_->declare_parameter("stream_bytes_per_second", c.stream_bytes_per_second); + c.exposure_auto_max = node_->declare_parameter("exposure_auto_max", c.exposure_auto_max); + c.exposure_auto_target = node_->declare_parameter("exposure_auto_target", c.exposure_auto_target); + c.gain_auto_max = node_->declare_parameter("gain_auto_max", c.gain_auto_max); + c.gain_auto_target = node_->declare_parameter("gain_auto_target", c.gain_auto_target); + last_config_ = c; + } + + void openCamera() + { + int loop_count = 0; + while (!camera_ && rclcpp::ok()) + { + // For some reason, this doesn't see any on the first try. + // The startup sequence in general is a hot mess but it eventually works + ROS_INFO("== %d Available cameras: %ld ==\n%s", loop_count++, + prosilica::numCameras(), getAvailableCameras().c_str()); + ROS_INFO("== __________________ =="); + + boost::lock_guard scoped_lock(config_mutex_); + camera_state_ = OPENING; + try + { + if(guid_ != 0) + { + state_info_ = "Trying to load camera with guid " + hw_id_; + ROS_INFO("%s", state_info_.c_str()); + camera_ = boost::make_shared((unsigned long)guid_); + ROS_INFO("Started Prosilica camera with guid \"%lu\"", guid_); + + } + else if(!ip_address_.empty()) + { + state_info_ = "Trying to load camera with ipaddress: " + ip_address_; + ROS_INFO("%s", state_info_.c_str()); + camera_ = boost::make_shared(ip_address_.c_str()); + guid_ = camera_->guid(); + hw_id_ = boost::lexical_cast(guid_); + + ROS_INFO("Started Prosilica camera with guid \"%d\"", (int)camera_->guid()); + } + else + { + if(prosilica::numCameras()>0) + { + state_info_ = "Trying to load first camera found"; + ROS_INFO("%s", state_info_.c_str()); + guid_ = prosilica::getGuid(0); + camera_ = boost::make_shared((unsigned long)guid_); + hw_id_ = boost::lexical_cast(guid_); + ROS_INFO("Started Prosilica camera with guid \"%d\"", (int)guid_); + } + else + { + throw std::runtime_error("ERR: Found no cameras on local subnet"); + } + } + + } + catch (std::exception& e) + { + camera_state_ = CAMERA_NOT_FOUND; + std::stringstream err; + if (prosilica::numCameras() == 0) + { + err << "Hm. Found no cameras on local subnet"; + } + else if (guid_ != 0) + { + err << "Unable to open prosilica camera with guid " << guid_ <<": "< cameras = prosilica::listCameras(); + std::stringstream list; + for (unsigned int i = 0; i < cameras.size(); ++i) + { + list << cameras[i].serial << " - " <setAttribute("StreamBytesPerSecond", (tPvUint32)(camera_->max_data_rate / num_cameras)); + camera_->getAttribute("StreamBytesPerSecond", actualStreamBps); + ROS_INFO("Max data rate: %lu current set: %s", camera_->max_data_rate, actualStreamBps.c_str()); + } else { + ROS_WARN("Cannot set StreamBytesPerSecond"); + } + } + + void loadIntrinsics() + { + try + { + camera_->setKillCallback(boost::bind(&ProsilicaDriver::kill, this, boost::placeholders::_1)); + + if(auto_adjust_stream_bytes_per_second_ && camera_->hasAttribute("StreamBytesPerSecond")) { + setSpeed(); + } + + + // Retrieve contents of user memory + std::string buffer(prosilica::Camera::USER_MEMORY_SIZE, '\0'); + camera_->readUserMemory(&buffer[0], prosilica::Camera::USER_MEMORY_SIZE); + + PvAttrRangeUint32(camera_->handle(), "BinningX", &dummy, &max_binning_x); + PvAttrRangeUint32(camera_->handle(), "BinningY", &dummy, &max_binning_y); + PvAttrRangeUint32(camera_->handle(), "Width", &dummy, &sensor_width_); + PvAttrRangeUint32(camera_->handle(), "Height", &dummy, &sensor_height_); + + + // Parse calibration file + std::string camera_name; + if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, cam_info_)) + { + intrinsics_ = "Loaded calibration"; + ROS_INFO("Loaded calibration for camera '%s'", camera_name.c_str()); + } + else + { + intrinsics_ = "Failed to load intrinsics from camera"; + ROS_WARN("Failed to load intrinsics from camera"); + } + } + catch(std::exception &e) + { + camera_state_ = CAMERA_NOT_FOUND; + state_info_ = e.what(); + } + } + + void start() + { + try + { + switch(trigger_mode_) + { + case prosilica::Software: + ROS_INFO("starting camera %s in software trigger mode", hw_id_.c_str()); + camera_->start(prosilica::Software, 1., prosilica::Continuous); + if(update_rate_ > 0) + { + update_timer_ = node_->create_wall_timer( + std::chrono::duration(1.0 / update_rate_), + [this]() { updateCallback(node_->now()); }); + } + break; + case prosilica::Freerun: + ROS_INFO("starting camera %s in freerun trigger mode", hw_id_.c_str()); + camera_->setFrameCallback(boost::bind(&ProsilicaDriver::publishImage, this, boost::placeholders::_1)); + camera_->start(prosilica::Freerun, 1., prosilica::Continuous); + break; + case prosilica::FixedRate: + ROS_INFO("starting camera %s in fixedrate trigger mode", hw_id_.c_str()); + camera_->setFrameCallback(boost::bind(&ProsilicaDriver::publishImage, this, boost::placeholders::_1)); + camera_->start(prosilica::FixedRate, update_rate_, prosilica::Continuous); + break; + case prosilica::SyncIn1: + ROS_INFO("starting camera %s in sync1 trigger mode", hw_id_.c_str()); + camera_->setFrameCallback(boost::bind(&ProsilicaDriver::publishImage, this, boost::placeholders::_1)); + camera_->start(prosilica::SyncIn1, update_rate_, prosilica::Continuous); + break; + case prosilica::SyncIn2: + ROS_INFO("starting camera %s in sync2 trigger mode", hw_id_.c_str()); + camera_->setFrameCallback(boost::bind(&ProsilicaDriver::publishImage, this, boost::placeholders::_1)); + camera_->start(prosilica::SyncIn2, update_rate_, prosilica::Continuous); + break; + default: + break; + } + } + catch(std::exception &e) + { + camera_state_ = CAMERA_NOT_FOUND; + state_info_ = e.what(); + } + + try { + ROS_INFO("exposure = %s", getCameraAttr("ExposureValue").value.c_str()); + } + catch(std::exception &e) { + camera_state_ = CAMERA_NOT_FOUND; + state_info_ = e.what(); + } + ROS_GREEN("start() complete"); + } + + void stop() + { + if (update_timer_) { + update_timer_->cancel(); + } + if(!camera_) + return; + camera_->removeEvents(); + camera_->stop(); + + } + + void kill(unsigned long guid) + { + if(guid == guid_) + { + ROS_WARN("[%s] got Camera::kill() request for prosilica camera %lu",getName().c_str(), guid); + //! Make sure we interrupt initialization (if it happened to still execute). + init_thread_.interrupt(); + init_thread_.join(); + + camera_state_ = CAMERA_NOT_FOUND; + state_info_ = "Prosilica camera " + hw_id_ + " disconnected"; + ROS_ERROR("%s", state_info_.c_str()); + boost::lock_guard scoped_lock(config_mutex_); + stop(); + camera_.reset(); + init_thread_ = boost::thread(boost::bind(&ProsilicaDriver::openCamera, this)); + return; + } + } + + + int syncCamToSysClock() { + ROS_INFO("call syncCamToSysClock() "); + auto err = PvCommandRun(camera_->handle(), "TimeStampValueLatch"); + if (err != ePvErrSuccess) { + ROS_ERROR("Could not sync clock"); + return (int) err; + } + rclcpp::Time after = node_->now(); + tPvUint32 timelo, timehi, freq; + camera_->getAttribute("TimeStampValueHi", timehi); + camera_->getAttribute("TimeStampValueLo", timelo); + camera_->getAttribute("TimeStampFrequency", freq); + rclcpp::Time tsframe = prosilica::CvtPvTimestamp(timehi, timelo, freq); + clock_offset = after - tsframe; + ROS_INFO("Clock synced, offset = %lf", clock_offset.seconds()); + return 0; + } + + /// todo: variably disable archiving and/or publishing. totally remove it and profile ePvWhatevr + void publishImage(tPvFrame* frame) + { bool ok = false; + try { + auto recv_time = node_->now(); + this->publishImageOld(frame, recv_time); + ok = true; + + } catch (std::exception &e) { + ROS_ERROR("publishImage failed: %s", e.what()); + ok = false; + } + if (ok) { + watchdog.pet(); + } else { + watchdog.kick(); + } + } + + void publishImageOld(tPvFrame* frame, rclcpp::Time time) + { + frame_recv_time_ = node_->now(); + + camera_state_ = OK; + state_info_ = "Camera operating normally"; + + /** allow most recent event to be received. + * Events arrive asynchronously via the executor; check a few times + * for a newer event than the last published one. */ + int64_t seq_dt = 0; + int loop_count = 0; + do { + seq_dt = (int64_t) event_.event_num - (int64_t) last_published_event_num_; + } while (seq_dt < 1 && loop_count++ < 3); + + std_msgs::msg::Header gps_header; + uint64_t gps_event_num = 0; + /// todo: null check here or use context manager + prosilica::MetaFrame* meta_frame = (prosilica::MetaFrame*) frame->Context[0]; + if (!meta_frame) { + ROS_ERROR("tPvFrame context is null"); + return; + } + ROS_INFO("FrameDone %p #%ld @ %ld %ld", (void*) meta_frame, (long int) meta_frame->idx, (long int) frame->TimestampHi, (long int) frame->TimestampLo); + + bool success = event_cache.search(frame_recv_time_, gps_header, gps_event_num); + + rclcpp::Time tsframe = prosilica::CvtPvTimestamp(frame->TimestampHi, frame->TimestampLo); + rclcpp::Time corrFrameTime = tsframe + clock_offset; + ROS_INFO("frameTime: %16.4f GPS: %16.4f DT: %7.4f", corrFrameTime.seconds(), + rclcpp::Time(gps_header.stamp).seconds(), + (corrFrameTime - rclcpp::Time(gps_header.stamp)).seconds()); + std::stringstream this_frame_id; + this_frame_id << frame_id_; + + // convey the status of the event binding process + if (success) { + this_frame_id << "?lock=1&eventNum=" << gps_event_num << "&eventTime" << rclcpp::Time(gps_header.stamp).seconds() ; + } else { + this_frame_id << "?lock=0"; + } + + if (image_publisher_.getNumSubscribers() > 0) + { + auto nodeName = getName(); + std::stringstream link; + custom_msgs::msg::Stat stat_msg; + stat_msg.header.stamp = node_->now(); + stat_msg.trace_topic = nodeName + "/publishImage"; + stat_msg.node = nodeName; + stat_msg.trace_header = std_msgs::msg::Header(img_.header); + link << nodeName << "/event/" << event_.event_num; // link this trace to the event trace + stat_msg.link = link.str(); + meta_frame->img_.header.stamp = event_.gps_time; + if (seq_dt > 1) { + ROS_ERROR("[%lu] Missed %ld frames, based on event seq ", (unsigned long) event_.event_num, (long) (seq_dt - 1)); + for (auto i = 0; i < 4 && i < seq_dt - 1; i++) { + watchdog.kick(); + } + } + sensor_msgs::msg::Image::SharedPtr p_img = std::make_shared(meta_frame->img_); + + if (processFrame(frame, *p_img, cam_info_)) // this will memcpy frame's buffer into img_ + { + // Set the image timestamp to match the event that actually triggered it + if (success) { + stat_msg.note = "success"; + img_.header = gps_header; + } + p_img->header.frame_id = this_frame_id.str(); + cam_info_.header = p_img->header; + stat_pub_->publish(stat_msg); + image_publisher_.publish(*p_img, cam_info_); + frames_dropped_acc_.add(0); + + } + else + { + ROS_ERROR("[?][3] Frame parse failed, checking status"); + auto status = frame->Status; + ROS_ERROR("[%lu][3] Frame parse failed, frame status: %d %s", (unsigned long) event_.event_num, status, pv_error_codes[status]); + camera_state_ = FORMAT_ERROR; + state_info_ = "Unable to process frame"; + this_frame_id << "&status=" << status << "&error=" << pv_error_codes[status]; + std_msgs::msg::Header msg = std_msgs::msg::Header(img_.header); + broken_img_.header.stamp = img_.header.stamp; + broken_img_.header.frame_id = this_frame_id.str(); + ++frames_dropped_total_; + missed_frames_pub_->publish(msg); + stat_msg.note = pv_error_codes[status]; + stat_pub_->publish(stat_msg); + errstat_pub_->publish(stat_msg); + image_publisher_.publish(broken_img_, cam_info_); + frames_dropped_acc_.add(1); + } + last_published_event_num_ = event_.event_num; + + + ++frames_completed_total_; + frames_completed_acc_.add(1); + } + auto end = node_->now(); + ROS_INFO("publishImage1 in %.4f seconds", (end - frame_recv_time_).seconds()); + } + + void updateCallback(rclcpp::Time current_real) + { + // Download the most recent data from the device + camera_state_ = OK; + state_info_ = "Camera operating normally"; + if(image_publisher_.getNumSubscribers() > 0) + { + boost::lock_guard lock(config_mutex_); + try + { + tPvFrame* frame = NULL; + frame = camera_->grab(1000); + publishImageOld(frame, current_real); + } + catch(std::exception &e) + { + camera_state_ = ERROR; + state_info_ = e.what(); + ROS_ERROR("Unable to read from camera: %s", e.what()); + ++frames_dropped_total_; + frames_dropped_acc_.add(1); + return; + } + } + } + + void syncInCallback (const std_msgs::msg::Header::ConstSharedPtr& msg) + { + printf("\n <> syncInCallback <> \n"); + if (trigger_mode_ != prosilica::Software) + { + camera_state_ = ERROR; + state_info_ = "Can not sync from topic trigger unless in Software Trigger mode"; + ROS_ERROR("%s", state_info_.c_str()); + return; + } + updateCallback(rclcpp::Time(msg->stamp)); + } + + void dumperCallback (const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + int is_archiving = ArchiverHelper::get_is_archiving(envoy_, "/sys/arch/is_archiving"); + ROS_INFO("dumper: is archiving: %d", is_archiving); + if(is_archiving) { + long int sec = msg->header.stamp.sec; + long int nsec = msg->header.stamp.nanosec; + std::string filename = ArchiverHelper::generateFilename(envoy_, arch_opts_, sec, nsec); + try { + bool debayer{false}; + + if ("rgb" == cam_channel) { + debayer = true; + } + auto filename_written = dumpImageMessage(msg, filename, debayer ); + ROS_INFO("[%s] dumped %s", cam_channel.c_str(), filename_written.c_str()); + } catch (cv_bridge::Exception &e) { + ROS_ERROR("%s", e.what()); + } + + } + } + + void eventCallback (const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) + { + ROS_INFO("[%lu]<1> eventCallback <> %2.2f", (unsigned long) msg->event_num, rclcpp::Time(msg->gps_time).seconds()); + event_ = *msg; + event_cache.push_back(rclcpp::Time(msg->sys_time), msg); + auto nodeName = getName(); + custom_msgs::msg::Stat stat_msg; + std::stringstream link; + stat_msg.header.stamp = node_->now(); + stat_msg.trace_header = (*msg).header; + stat_msg.trace_topic = nodeName + "/eventCallback"; + stat_msg.node = nodeName; + link << nodeName << "/event/" << event_.event_num; // link this trace to the event trace + stat_msg.link = link.str(); + stat_pub_->publish(stat_msg); + event_cache.purge(); + watchdog.check(); + } + + /** Exposure is in microseconds (microsecs). Minimum varies by camera I think. */ + void exposureCallback (const std_msgs::msg::Int32::ConstSharedPtr &msg) + { + printf("\n <> exposureCallback <> \n"); + tPvUint32 microsecs_min = 30; + int32_t microsecs = msg->data; + if (microsecs < 0) { + printf("WARNING: Exposure set to less that zero. Setting auto exposure. This is not a recommended feature"); + camera_->setExposure(microsecs, prosilica::Auto); + return; + } + tPvUint32 umicrosecs = msg->data; + + if (umicrosecs < microsecs_min) { + printf("WARNING: Exposure set to less than max allowed. Clipping to %lu", microsecs_min); + umicrosecs = microsecs_min; + } + printf("INFO: Exposure set to: %lu microseconds", umicrosecs); + camera_->setExposure(umicrosecs, prosilica::Manual); + + } + + void health(const std::shared_ptr req, + std::shared_ptr rsp) { + (void) req; + auto healthy = watchdog.Ok(); + rsp->success = healthy; + if (!healthy) { + rsp->message = "Watchdog timed out"; + } + } + + + /** this is a pretty gross api. It's stringly-typed, so be careful + * also currently does not work with certain types. + * Note: use "1"/"0" for pushing bools. They are pretty rare though*/ + void setCameraAttr(const std::shared_ptr req, + std::shared_ptr rsp) { + ROS_INFO(" setCameraAttr(%s, %s)", req->name.c_str(), req->value.c_str()); + tPvHandle handle = camera_->handle(); + rsp->pv_err = ePvErrUnknown; + rsp->value = "error"; + const char *c_name = req->name.c_str(); + + if (req->name == "SyncClock") { + rsp->pv_err = syncCamToSysClock(); + rsp->dtype = "time"; + rsp->value = std::to_string(clock_offset.seconds()); + return; + } + + /** On failure, pass error to message. This is more descriptive than + * return false*/ + rsp->pv_err = PvAttrIsAvailable(handle, c_name); + if (rsp->pv_err != 0) { return; } + + tPvAttributeInfo info; + + rsp->pv_err = PvAttrInfo(handle, c_name, &info); + if (rsp->pv_err != 0) { return; } + + + switch (info.Datatype) { + case ePvDatatypeEnum: { + rsp->pv_err = PvAttrEnumSet( + handle, c_name, req->value.c_str()); + rsp->dtype = "enum"; + break; + } + case ePvDatatypeString: { + rsp->pv_err = PvAttrStringSet( + handle, c_name, req->value.c_str()); + rsp->dtype = "string"; + break; + } + case ePvDatatypeUint32: { + rsp->pv_err = PvAttrUint32Set( + handle, c_name, std::stoul(req->value)); + rsp->dtype = "uint32"; + break; + } + case ePvDatatypeInt64: { + rsp->pv_err = PvAttrInt64Set( + handle, c_name, std::stol(req->value)); + rsp->dtype = "int64"; + break; + } + case ePvDatatypeFloat32: { + rsp->pv_err = PvAttrFloat32Set( + handle, c_name, std::stof(req->value)); + rsp->dtype = "float32"; + break; + } + case ePvDatatypeBoolean: { + rsp->pv_err = PvAttrBooleanSet( + handle, c_name, std::stoi(req->value)); + rsp->dtype = "bool"; + break; + } + case ePvDatatypeCommand: { + rsp->pv_err = PvCommandRun( + handle, c_name); + rsp->dtype = "PvCommandRun"; + rsp->value = "ok"; + return; + } + default: { + rsp->pv_err = ePvErrBadParameter; + break; + } + } + if (rsp->pv_err != 0) { return; } + custom_msgs::srv::CamGetAttr::Response new_rsp = getCameraAttr(req->name); + rsp->value = new_rsp.value; + } + + void getCameraAttrSrv(const std::shared_ptr req, + std::shared_ptr rsp) { + ROS_INFO(" getCameraAttr(%s)", req->name.c_str()); + + try { + *rsp = getCameraAttr(req->name); + } + catch (prosilica::ProsilicaException &) { + rsp->value = "error"; + } + } + + custom_msgs::srv::CamGetAttr::Response getCameraAttr(std::string name) { + tPvHandle handle = camera_->handle(); + custom_msgs::srv::CamGetAttr::Response rsp; + rsp.pv_err = ePvErrUnknown; + rsp.value = "error"; + const char *c_name = name.c_str(); + + rsp.pv_err = PvAttrIsAvailable(handle, c_name); + if (rsp.pv_err != 0) { return rsp; } + + tPvAttributeInfo info; + + rsp.pv_err = PvAttrInfo(handle, c_name, &info); + if (rsp.pv_err != 0) { return rsp; } + + + switch (info.Datatype) { + case ePvDatatypeEnum: + camera_->getAttributeEnum(name, rsp.value); + rsp.pv_err = ePvErrSuccess; + rsp.dtype = "enum"; + break; + case ePvDatatypeString: + rsp.pv_err = prosilica::getAttribute(handle, c_name, rsp.value); + rsp.dtype = "string"; + break; + case ePvDatatypeUint32: + tPvUint32 value; + rsp.pv_err = PvAttrUint32Get(handle, c_name, &value); + rsp.dtype = "uint32"; + rsp.value = std::to_string((unsigned long) value); + break; + case ePvDatatypeInt64: + tPvInt64 l_value; + rsp.pv_err = PvAttrInt64Get(handle, c_name, &l_value); + rsp.dtype = "int64"; + rsp.value = std::to_string((int64_t) l_value); + break; + case ePvDatatypeFloat32: + tPvFloat32 f_value; + rsp.pv_err = PvAttrFloat32Get(handle, c_name, &f_value); + rsp.dtype = "float32"; + rsp.value = std::to_string((float) f_value); + break; + case ePvDatatypeBoolean: + tPvBoolean b_value; + rsp.pv_err = PvAttrBooleanGet(handle, c_name, &b_value); + rsp.dtype = "bool"; + rsp.value = std::to_string((bool) b_value); + break; + default: + rsp.pv_err = ePvErrBadParameter; + break; + } + return rsp; + + } + + void getAttrList(const std::shared_ptr req, + std::shared_ptr rsp) { + (void) req; + tPvAttrListPtr pListPtr; + unsigned long sz; + // The attribute list is contained in memory allocated by the PvApi module. + tPvErr err = PvAttrList(camera_->handle(), &pListPtr, &sz); + ROS_WARN("sz: %ld ", sz); + if (err != 0) { + rsp->pv_err = err; + rsp->values.push_back("error"); + return; + } + for (unsigned long i = 0; i < sz; i++) { + rsp->values.push_back(pListPtr[i]); + } + } + + // this calls frameToImage which calls fillImage which calls memcpy + bool processFrame(tPvFrame* frame, sensor_msgs::msg::Image &img, sensor_msgs::msg::CameraInfo &cam_info) + { + /// @todo Match time stamp from frame to ROS time? + if (frame==NULL ) { + return false; + } + // we want to deliberately allow some missing-date frames through + // for debugging + if (frame->Status == ePvErrSuccess) { + // pass + } else if (frame->Status == ePvErrDataMissing) { + // pass + ROS_WARN("Data Missing from Frame. This may fail"); + } else { + return false; // you shall not pass + } + try + { + /// @todo Binning values retrieved here may differ from the ones used to actually + /// capture the frame! Maybe need to clear queue when changing binning and/or + /// stuff binning values into context? + tPvUint32 binning_x = 1, binning_y = 1; + if (auto_adjust_binning_) { + if (camera_->hasAttribute("BinningX")) { + camera_->getAttribute("BinningX", binning_x); + camera_->getAttribute("BinningY", binning_y); + } + } + + // Binning averages bayer samples, so just call it mono8 in that case + if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1)) + frame->Format = ePvFmtMono8; + + if (!frameToImage(frame, img)) { + return false; + } + // Set the operational parameters in CameraInfo (binning, ROI) + cam_info.binning_x = binning_x; + cam_info.binning_y = binning_y; + // ROI in CameraInfo is in unbinned coordinates, need to scale up + cam_info.roi.x_offset = frame->RegionX * binning_x; + cam_info.roi.y_offset = frame->RegionY * binning_y; + cam_info.roi.height = frame->Height * binning_y; + cam_info.roi.width = frame->Width * binning_x; + cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) || + (frame->Width != sensor_width_ / binning_x); + } + catch(std::exception &e) + { + return false; + } + + count_++; + return true; + } + + bool frameToImage(tPvFrame* frame, sensor_msgs::msg::Image &image) + { + // NOTE: 16-bit and Yuv formats not supported + static const char* BAYER_ENCODINGS[] = { "bayer_rggb8", "bayer_gbrg8", "bayer_grbg8", "bayer_bggr8" }; + + std::string encoding; + if (frame->Format == ePvFmtMono8) encoding = sensor_msgs::image_encodings::MONO8; + else if (frame->Format == ePvFmtBayer8) encoding = BAYER_ENCODINGS[frame->BayerPattern]; + else if (frame->Format == ePvFmtRgb24) encoding = sensor_msgs::image_encodings::RGB8; + else if (frame->Format == ePvFmtBgr24) encoding = sensor_msgs::image_encodings::BGR8; + else if (frame->Format == ePvFmtRgba32) encoding = sensor_msgs::image_encodings::RGBA8; + else if (frame->Format == ePvFmtBgra32) encoding = sensor_msgs::image_encodings::BGRA8; + else { + ROS_WARN("Received frame with unsupported pixel format %d", frame->Format); + return false; + } + + + if(frame->ImageSize == 0) { + ROS_WARN("Image size is zero"); + return false; + } + if(frame->Height == 0) { + ROS_WARN("Image height is zero"); + return false; + } + + uint32_t step = frame->ImageSize / frame->Height; + // fillImage calls memcpy + return sensor_msgs::fillImage(image, encoding, frame->Height, frame->Width, step, frame->ImageBuffer); + } + + void setCameraInfo(const std::shared_ptr req, + std::shared_ptr rsp) + { + ROS_INFO(" New camera info received"); + sensor_msgs::msg::CameraInfo &info = req->camera_info; + + // Sanity check: the image dimensions should match the max resolution of the sensor. + if (info.width != sensor_width_ || info.height != sensor_height_) + { + rsp->success = false; + std::stringstream err; + err << "Camera_info resolution " << info.width << "x" << info.height + << " does not match current video setting, camera running at resolution " + << sensor_width_ << "x" << sensor_height_ << "."; + rsp->status_message = err.str(); + ROS_ERROR("%s", rsp->status_message.c_str()); + return; + } + + stop(); + + std::string cam_name = "prosilica"; + cam_name += hw_id_; + std::stringstream ini_stream; + if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info)) + { + rsp->status_message = "Error formatting camera_info for storage."; + rsp->success = false; + } + else + { + std::string ini = ini_stream.str(); + if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE) + { + rsp->success = false; + rsp->status_message = "Unable to write camera_info to camera memory, exceeded storage capacity."; + } + else + { + try + { + camera_->writeUserMemory(ini.c_str(), ini.size()); + cam_info_ = info; + rsp->success = true; + } + catch (prosilica::ProsilicaException &e) + { + rsp->success = false; + rsp->status_message = e.what(); + } + } + } + if (!rsp->success) + ROS_ERROR("%s", rsp->status_message.c_str()); + + start(); + } + + /** Apply the static configuration to the camera. + * ROS2 port of the old dynamic_reconfigure callback; runs once at startup. */ + void applyConfig(DriverConfig &config, bool restart) + { + printf("\n<> Apply config \n"); + + if (restart) + stop(); + + //! Trigger mode + if (config.trigger_mode == "streaming") + { + trigger_mode_ = prosilica::Freerun; + update_rate_ = 1.; // make sure we get _something_ + } + else if (config.trigger_mode == "syncin1") + { + trigger_mode_ = prosilica::SyncIn1; + update_rate_ = config.trig_rate; + } + else if (config.trigger_mode == "syncin2") + { + trigger_mode_ = prosilica::SyncIn2; + update_rate_ = config.trig_rate; + } + else if (config.trigger_mode == "fixedrate") + { + trigger_mode_ = prosilica::FixedRate; + update_rate_ = config.trig_rate; + } + else if (config.trigger_mode == "software") + { + trigger_mode_ = prosilica::Software; + update_rate_ = config.trig_rate; + } + + else if (config.trigger_mode == "polled") + { + trigger_mode_ = prosilica::Software; + update_rate_ = 0; + } + else if (config.trigger_mode == "triggered") + { + trigger_mode_ = prosilica::Software; + update_rate_ = 0; + } + else + { + ROS_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str()); + } + + // Exposure + if (config.auto_exposure) + { + camera_->setExposure(0, prosilica::Auto); + if (camera_->hasAttribute("ExposureAutoMax")) + { + tPvUint32 us = config.exposure_auto_max*1000000. + 0.5; + camera_->setAttribute("ExposureAutoMax", us); + } + if (camera_->hasAttribute("ExposureAutoTarget")) + camera_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target); + } + else + { + unsigned us = config.exposure*1000000. + 0.5; + camera_->setExposure(us, prosilica::Manual); + camera_->setAttribute("ExposureValue", (tPvUint32)us); + } + + // Gain + if (config.auto_gain) + { + if (camera_->hasAttribute("GainAutoMax")) + { + camera_->setGain(0, prosilica::Auto); + camera_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max); + camera_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target); + } + else + { + tPvUint32 major, minor; + camera_->getAttribute("FirmwareVerMajor", major); + camera_->getAttribute("FirmwareVerMinor", minor); + ROS_WARN("Auto gain not available for this camera. Auto gain is available " + "on firmware versions 1.36 and above. You are running version %u.%u.", + (unsigned)major, (unsigned)minor); + config.auto_gain = false; + } + } + else + { + camera_->setGain(config.gain, prosilica::Manual); + camera_->setAttribute("GainValue", (tPvUint32)config.gain); + } + + // White balance + if (config.auto_whitebalance) + { + if (camera_->hasAttribute("WhitebalMode")) + camera_->setWhiteBalance(0, 0, prosilica::Auto); + else + { + ROS_WARN("Auto white balance not available for this camera."); + config.auto_whitebalance = false; + } + } + else + { + camera_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual); + if (camera_->hasAttribute("WhitebalValueRed")) + camera_->setAttribute("WhitebalValueRed", (tPvUint32)config.whitebalance_red); + if (camera_->hasAttribute("WhitebalValueBlue")) + camera_->setAttribute("WhitebalValueBlue", (tPvUint32)config.whitebalance_blue); + } + + // Binning configuration + if (camera_->hasAttribute("BinningX")) + { + config.binning_x = std::min(config.binning_x, (int)max_binning_x); + config.binning_y = std::min(config.binning_y, (int)max_binning_y); + + camera_->setBinning(config.binning_x, config.binning_y); + } + else if (config.binning_x > 1 || config.binning_y > 1) + { + ROS_WARN("Binning not available for this camera."); + config.binning_x = config.binning_y = 1; + } + + // Region of interest configuration + // Make sure ROI fits in image + config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1); + config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1); + config.width = std::min(config.width, (int)sensor_width_ - config.x_offset); + config.height = std::min(config.height, (int)sensor_height_ - config.y_offset); + // If width or height is 0, set it as large as possible + int width = config.width ? config.width : sensor_width_ - config.x_offset; + int height = config.height ? config.height : sensor_height_ - config.y_offset; + + // Adjust full-res ROI to binning ROI + int x_offset = config.x_offset / config.binning_x; + int y_offset = config.y_offset / config.binning_y; + unsigned int right_x = (config.x_offset + width + config.binning_x - 1) / config.binning_x; + unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y; + // Rounding up is bad when at max resolution which is not divisible by the amount of binning + right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x)); + bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y)); + width = right_x - x_offset; + height = bottom_y - y_offset; + + camera_->setRoi(x_offset, y_offset, width, height); + + // TF frame + img_.header.frame_id = cam_info_.header.frame_id = config.frame_id; + + // Normally the node adjusts the bandwidth used by the camera during diagnostics, to use as + // much as possible without dropping packets. But this can create interference if two + // cameras are on the same switch, e.g. for stereo. So we allow the user to set the bandwidth + // directly. + auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second; + if (!auto_adjust_stream_bytes_per_second_) + camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second); + else + camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(camera_->max_data_rate/num_cameras)); + + //! If exception thrown due to bad settings, it will fail to start camera + if (restart) + { + try + { + start(); + } + catch(std::exception &e) + { + ROS_ERROR("Invalid settings: %s", e.what()); + } + } + + last_config_ = config; + } +}; + + + +} // end namespace + +/** === === === === === === === === === === === === */ +void driver_shutdown() { + for ( auto const& it: active_drivers) + { + ROS_WARN("Stopping driver %d", it.first); + it.second->public_stop(); + } +} + + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("prosilica_driver"); + prosilica_camera::ProsilicaDriver driver(node); + // Use a multithreaded executor to handle the numerous callbacks + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/cams/prosilica_camera/src/nodes/prosilica_nodelet.cpp b/src/cams/prosilica_camera/src/nodes/prosilica_nodelet.cpp deleted file mode 100644 index fd49f798..00000000 --- a/src/cams/prosilica_camera/src/nodes/prosilica_nodelet.cpp +++ /dev/null @@ -1,1787 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* -* 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 the Willow Garage 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 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 -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include "prosilica/prosilica.h" -#include "prosilica/rolling_sum.h" - -#include -#include - -#include - -// custom messages from KAMERA -#include -#include -#include -#include -#include - - -bool prosilica_inited = false;//for if the nodelet is loaded multiple times in the same manager -int num_cameras = 0; - -namespace prosilica_camera { - class ProsilicaNodelet ; -} - -// Container so we can actually reference the ProsilicaNodelet without ROS magic -std::map active_nodelets; - -void cb_request_shutdown(std_msgs::Int8 const &msg) { - ROS_INFO("Requesting clean shutdown: %d", msg.data); - ros::requestShutdown(); -} - -void driver_shutdown(); - -bool build_broken_img(sensor_msgs::Image& img); - -void signalHandler( int signum ) { - ROS_WARN(" Interrupt signal (%d)\n", signum); - driver_shutdown(); - ros::shutdown(); -} - -// sensor_msgs::ImageConstPtr -void resizeImageMessage(const sensor_msgs::ImageConstPtr& received_image) -{ - cv_bridge::CvImagePtr cvPtr; - cvPtr = cv_bridge::toCvCopy(received_image, sensor_msgs::image_encodings::BGR8); - - cv::Mat undist = cvPtr->image; -// NODELET_WARN("Warning: hack here, deliberately shrinking image"); - - cv::resize(undist, cvPtr->image, cv::Size(64, 48), - 0, 0, cv::INTER_LINEAR); - -// return cvPtr->toImageMsg(); -} - -// sensor_msgs::ImageConstPtr -std::string dumpImageMessage(const sensor_msgs::Image & received_image, const std::string filename) { - ROS_WARN("Deprecated! dumpImageMessage(Image&)"); - auto cvPtr = cv_bridge::toCvCopy(received_image, sensor_msgs::image_encodings::BGR8); - -} -std::string dumpImageMessage(const sensor_msgs::ImageConstPtr &received_image, const std::string filename, bool debayer) -{ - std::vector compression_params; - compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); - compression_params.push_back(100); - auto start_db = ros::Time::now(); - cv_bridge::CvImagePtr cvPtr; - if (debayer) { - cvPtr = cv_bridge::toCvCopy(received_image, sensor_msgs::image_encodings::BGR8); - } else { - cvPtr = cv_bridge::toCvCopy(received_image, received_image->encoding); - } - ROS_INFO("debayered? %d %ld in %2.3f ", debayer, (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), (ros::Time::now()-start_db).toSec()); - cv::Mat undist = cvPtr->image; - auto nodeName = ros::this_node::getName(); - - start_db = ros::Time::now(); - - boost::filesystem::path path_filename{filename}; - try { - boost::filesystem::create_directories(path_filename.parent_path()); - cv::imwrite(filename, cvPtr->image, compression_params); - } catch (boost::filesystem::filesystem_error &e) { - ROS_ERROR("Archive Failed [%d]: %s", e.code().value(), e.what()); - return ""; - } -// cv::imwrite(filename, cvPtr->image); - ROS_INFO("dumped %ld in %2.3f ", (long int)(cvPtr->image.total() * cvPtr->image.elemSize()), (ros::Time::now()-start_db).toSec()); - start_db = ros::Time::now(); -// auto filename2 = std::string("/mnt/ram/miketest/driverdump/") + nodeName + "/" + std::to_string(now.toSec()) + ".jpg"; -// cv::imwrite(filename2, cvPtr->image, compression_params); -// auto out = cv::imread(filename, cv::IMREAD_UNCHANGED); - if (!boost::filesystem::exists(path_filename)) { - ROS_ERROR("Failed to create file"); - } -// ROS_INFO("read %ld in %2.3f ", (long int)(out.total() * out.elemSize()), (ros::Time::now()-start_db).toSec()); - return filename; - -// return cvPtr->toImageMsg(); -} - -/** === === === === === === === === === === === === */ - -static const char* camera_channels[] = {"rgb", "ir", "uv"}; -static std::map camera_delays{{"rgb", 0.423}, {"ir", 0.003}, {"uv", 0.289}}; - -/// Look for weird exceptions in time formats. Currently just checks validity, may check more if needed -/// isValid checks (!g_use_sim_time) || !g_sim_time.isZero(); -bool timeIsBad(ros::Time const &t) { - if (!t.isValid()) return false; - return true; -} - -/// Time should be within some reasonable distance of detected time -bool timeIsUnreasonable(ros::Time const &t, double tolerance) { - -} - - -std::string get_cam_chan() { - auto nodeName = ros::this_node::getName(); - for (auto i=0; i<3; i++) { - auto s = std::string(camera_channels[i]); - std::size_t found = nodeName.find("/" + s + "/"); - if (found != std::string::npos) { - return s; - } - } - ROS_ERROR("Unable to auto-detect camera channel"); - return std::string("no_chan"); -} - -namespace prosilica_camera -{ - std::map pv_error_codes = { - {0, "ePvErrSuccess, No error"}, - {1, "ePvErrCameraFault, Unexpected camera fault"}, - {2, "ePvErrInternalFault, Unexpected fault in PvApi or driver"}, - {3, "ePvErrBadHandle, Camera handle is invalid"}, - {4, "ePvErrBadParameter, Bad parameter to API call"}, - {5, "ePvErrBadSequence, Sequence of API calls is incorrect"}, - {6, "ePvErrNotFound, Camera or attribute not found"}, - {7, "ePvErrAccessDenied, Camera cannot be opened in the specified mode"}, - {8, "ePvErrUnplugged, Camera was unplugged"}, - {9, "ePvErrInvalidSetup, Setup is invalid (an attribute is invalid)"}, - {10, "ePvErrResources, System/network resources or memory not available"}, - {11, "ePvErrBandwidth, 1394 bandwidth not available"}, - {12, "ePvErrQueueFull, Too many frames on queue"}, - {13, "ePvErrBufferTooSmall, Frame buffer is too small"}, - {14, "ePvErrCancelled, Frame cancelled by user"}, - {15, "ePvErrDataLost, The data for the frame was lost"}, - {16, "ePvErrDataMissing, Some data in the frame is missing"}, - {17, "ePvErrTimeout, Timeout during wait"}, - {18, "ePvErrOutOfRange, Attribute value is out of the expected range"}, - {19, "ePvErrWrongType, Attribute is not this type (wrong access function)"}, - {20, "ePvErrForbidden, Attribute write forbidden at this time"}, - {21, "ePvErrUnavailable, Attribute is not available at this time"}, - {22, "ePvErrFirewall, A firewall is blocking the traffic (Windows only)"}, - }; - - std::map tPv_dtypes = { - {"unknown", ePvDatatypeUnknown}, - {"string", ePvDatatypeString}, - {"ExposureMode", ePvDatatypeEnum}, - {"ExposureValue", ePvDatatypeUint32}, - }; - - - class ProsilicaNodelet : public nodelet::Nodelet -{ - -public: - - virtual ~ProsilicaNodelet() - { - - //! Make sure we interrupt initialization (if it happened to still execute). - init_thread_.interrupt(); - init_thread_.join(); - - if(camera_) - { - camera_->stop(); - camera_.reset(); // must destroy Camera before calling prosilica::fini - } - - trigger_sub_.shutdown(); - poll_srv_.shutdown(); - image_publisher_.shutdown(); - - active_nodelets.erase(cam_index); - --num_cameras; - if(num_cameras<=0) - { - prosilica::fini(); - prosilica_inited = false; - num_cameras = 0; - } - - NODELET_WARN("Unloaded prosilica camera with guid %s", hw_id_.c_str()); - } - - /// todo: may want to disable auto_adj - ProsilicaNodelet() - : auto_adjust_stream_bytes_per_second_(false), - auto_adjust_binning_(false), - count_(0), - frames_dropped_total_(0), frames_completed_total_(0), - frames_dropped_acc_(WINDOW_SIZE), - frames_completed_acc_(WINDOW_SIZE), - packets_missed_total_(0), packets_received_total_(0), - packets_missed_acc_(WINDOW_SIZE), - packets_received_acc_(WINDOW_SIZE) - { - cam_index = num_cameras; - active_nodelets[cam_index] = this; - - ++num_cameras; - printf("<> Hi, I am Prosilica Nodelet 1\n"); - signal(SIGINT, signalHandler); - signal(SIGTERM, signalHandler); - - } - - void public_stop() { - stop(); - } - -private: - std::string cam_fov; - std::string cam_channel = get_cam_chan(); - boost::shared_ptr camera_; - boost::thread init_thread_; - ros::Timer update_timer_; - int cam_index; - ros::NodeHandle nh; - ros::NodeHandlePtr nhp = boost::make_shared(nh); - - image_transport::CameraPublisher image_publisher_; - polled_camera::PublicationServer poll_srv_; - ros::Publisher missed_frames_pub_; - ros::Publisher stat_pub_; - ros::Publisher errstat_pub_; - ros::ServiceServer set_camera_info_srv_; - ros::ServiceServer get_camera_attr_srv_; - ros::ServiceServer set_camera_attr_srv_; - ros::ServiceServer get_attr_list_srv_; - ros::ServiceServer health_srv_; - ros::Subscriber trigger_sub_; - ros::Subscriber trigger_sub2_; - ros::Subscriber exposure_sub; - ros::Subscriber event_sub; - ros::Subscriber dumper_sub_; - ros::Subscriber shutdown_sub_; - - EventCache event_cache; - Watchdog watchdog; - ros::Duration clock_offset; // offset between system time and camera internal time - std::shared_ptr envoy_; - ArchiverOpts arch_opts_ = ArchiverOpts::from_env(); - prosilica::OneShotManager oneShotManager{}; -// std::unique_ptr archiver_; -// ImageSync fsm; - - sensor_msgs::Image img_; - sensor_msgs::Image broken_img_; - sensor_msgs::CameraInfo cam_info_; - - custom_msgs::GSOF_EVT event_; // store the last received event - std_msgs::Header last_published_; // Last header which was successfully published - - std::string frame_id_; - unsigned long guid_; - std::string hw_id_; - std::string ip_address_; - double open_camera_retry_period_; - std::string trig_timestamp_topic_; - ros::Time trig_time_; - // time last frame was received. Putting this here because frameDone is static - ros::Time frame_recv_time_; - std::string gainMode; - int gainValue; - int gvspRetries_; - float gvspResendPercent_; - - // Dynamic reconfigure parameters - double update_rate_; - int trigger_mode_; - bool auto_adjust_stream_bytes_per_second_; - bool auto_adjust_binning_; // allow binning to be requested, otherwise set to 1 - - tPvUint32 sensor_width_, sensor_height_; - tPvUint32 max_binning_x, max_binning_y, dummy; - int count_; - - // Dynamic Reconfigure - prosilica_camera::ProsilicaCameraConfig last_config_; - boost::recursive_mutex config_mutex_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; - - // State updater - enum CameraState - { - OPENING, - CAMERA_NOT_FOUND, - FORMAT_ERROR, - ERROR, - OK - }camera_state_; - std::string state_info_; - std::string intrinsics_; - static const int WINDOW_SIZE = 100; // remember previous 5s - unsigned long frames_dropped_total_, frames_completed_total_; - RollingSum frames_dropped_acc_, frames_completed_acc_; - unsigned long packets_missed_total_, packets_received_total_; - RollingSum packets_missed_acc_, packets_received_acc_; - - diagnostic_updater::Updater updater; - - - virtual void onInit() - { - //! We will be retrying to open camera until it is open, which may block the - //! thread. Nodelet::onInit() should not block, hence spawning a new thread - //! to do initialization. - init_thread_ = boost::thread(boost::bind(&ProsilicaNodelet::onInitImpl, this)); - - } - - void onInitImpl() - { - nh = getNodeHandle(); -// ros::NodeHandle& nh = getNodeHandle(); -// nh2 = getNodeHandle(); - ros::NodeHandle& pn = getPrivateNodeHandle(); - - //! initialize prosilica if necessary - if(!prosilica_inited) - { - NODELET_INFO("Initializing prosilica GIGE API"); - prosilica::init(); - prosilica_inited = true; - } - - //! Retrieve parameters from server - count_ = 0; - update_rate_=30; - NODELET_INFO("namespace: %s", pn.getNamespace().c_str()); - pn.param("frame_id", frame_id_, "/camera_optical_frame"); - NODELET_INFO("Loaded param frame_id: %s", frame_id_.c_str()); - - pn.param("guid", hw_id_, ""); - if(hw_id_ == "") - { - guid_ = 0; - } - else - { - guid_ = boost::lexical_cast(hw_id_); - NODELET_INFO("Loaded param guid: %lu lu", guid_); - } - - pn.param("ip_address", ip_address_, ""); - NODELET_INFO("Loaded ip address: %s", ip_address_.c_str()); - - pn.param("open_camera_retry_period", open_camera_retry_period_, 1.); - NODELET_INFO("Retry period: %f", open_camera_retry_period_); - - // Setup updater - updater.add(getName().c_str(), this, &ProsilicaNodelet::getCurrentState); - NODELET_INFO("updated state"); - // Setup periodic callback to get new data from the camera - update_timer_ = nh.createTimer(ros::Rate(update_rate_).expectedCycleTime(), &ProsilicaNodelet::updateCallback, this, false ,false); - update_timer_.stop(); - NODELET_INFO("created update timer"); - // Open camera - openCamera(); - - pn.param("cam_chan", cam_channel, ""); - pn.param("cam_fov", cam_fov, ""); - NODELET_INFO("Cameratype: %s/%s", cam_fov.c_str(), cam_channel.c_str()); - - RedisEnvoyOpts envoy_opts = RedisEnvoyOpts::from_env("driver_" + cam_fov + "_" + cam_channel ); - /// Connect with redis param server - NODELET_WARN("gonna initialize"); - std::cout << envoy_opts << " | " << RedisHelper::get_redis_uri() << std::endl; - envoy_ = std::make_shared(envoy_opts); - NODELET_WARN("echo: %s", envoy_->echo("Redis connected").c_str()); - std::string ns = ros::this_node::getNamespace(); - std::string image_read = ns + "/image_raw"; - ROS_WARN("read topic: %s", image_read.c_str()); - - - // Advertise topics - auto expected_delay = camera_delays[cam_channel]; - event_cache.set_delay(expected_delay); - event_cache.set_tolerance(ros::Duration(0.49)); - ros::NodeHandle image_nh(nh); - image_transport::ImageTransport image_it(image_nh); - image_publisher_ = image_it.advertiseCamera("image_raw", 1); - poll_srv_ = polled_camera::advertise(nh, "request_image", &ProsilicaNodelet::pollCallback, this); - missed_frames_pub_ = nh.advertise("/missed_frames", 3); - stat_pub_ = nh.advertise("/stat", 3); - errstat_pub_ = nh.advertise("/errstat", 3); - set_camera_info_srv_ = pn.advertiseService("set_camera_info", &ProsilicaNodelet::setCameraInfo, this); - get_camera_attr_srv_ = pn.advertiseService("get_camera_attr", &ProsilicaNodelet::getCameraAttr, this); - set_camera_attr_srv_ = pn.advertiseService("set_camera_attr", &ProsilicaNodelet::setCameraAttr, this); - get_attr_list_srv_ = pn.advertiseService("get_attr_list", &ProsilicaNodelet::getAttrList, this); - health_srv_ = pn.advertiseService("health", &ProsilicaNodelet::health, this); - trigger_sub_ = pn.subscribe(trig_timestamp_topic_, 1, &ProsilicaNodelet::syncInCallback, this); - trigger_sub2_ = pn.subscribe("trigger", 1, &ProsilicaNodelet::syncInCallback, this); - exposure_sub = pn.subscribe("exposure", 1, &ProsilicaNodelet::exposureCallback, this); - event_sub = pn.subscribe("/event", 1, &ProsilicaNodelet::eventCallback, this); - dumper_sub_ = nh.subscribe( image_read, 1, &ProsilicaNodelet::dumperCallback, this); - shutdown_sub_ = nh.subscribe("/shutdown", 1, cb_request_shutdown); - - /**Setup dynamic reconfigure server - This for some goofy reason needs to run in order to apply launch settings - */ - - printf("\n <> Setup dynamic reconfigure server <> \n"); - reconfigure_server_.reset(new ReconfigureServer(config_mutex_, pn)); - ReconfigureServer::CallbackType f = boost::bind(&ProsilicaNodelet::reconfigureCallback, this, _1, _2); - reconfigure_server_->setCallback(f); - printf("END Setup dynamic reconfigure server <1> \n"); - - - /** - * These parameters are a double-edged sword. Increasing GvspRetries can decrease risk of dropped frames if - * the system is not under heavy load. Under heavy load and unpredictable conditions, this can cause a - * resend storm that causes no frames to make it. Tune with caution. - */ - nhp->param("/cfg/prosilica/GvspRetries", gvspRetries_, 3); - nhp->param("/cfg/prosilica/GvspResendPercent", gvspResendPercent_, 2.0); - - camera_->setAttribute("GvspRetries", (tPvUint32) gvspRetries_); // default: 3.0, too high, and you risk a UDP storm - camera_->setAttribute("GvspResendPercent", (tPvFloat32) gvspResendPercent_); // default: 1.0 - - build_broken_img(broken_img_); - - -// archiver_ = std::make_unique(ArchiverHelper::from_env()); - ROS_GREEN("<> <> <> Cam init complete 1"); - - /// Ignore the first few seconds of frames health-wise since there is a purge - watchdog.DelayedStart(nhp, 6.0); - - } - - void openCamera() - { - int loop_count = 0; - while (!camera_) - { - // For some reason, this doesn't see any on the first try. - // The startup sequence in general is a hot mess but it eventually works - NODELET_INFO("== %d Available cameras: %ld ==\n%s", loop_count++, - prosilica::numCameras(), getAvailableCameras().c_str()); - NODELET_INFO("== __________________ =="); - - boost::lock_guard scoped_lock(config_mutex_); - camera_state_ = OPENING; - try - { - if(guid_ != 0) - { - state_info_ = "Trying to load camera with guid " + hw_id_; - NODELET_INFO("%s", state_info_.c_str()); - camera_ = boost::make_shared((unsigned long)guid_); - updater.setHardwareIDf("%d", guid_); - ROS_INFO("Started Prosilica camera with guid \"%lu\"", guid_); - - } - else if(!ip_address_.empty()) - { - state_info_ = "Trying to load camera with ipaddress: " + ip_address_; - NODELET_INFO("%s", state_info_.c_str()); - camera_ = boost::make_shared(ip_address_.c_str()); - guid_ = camera_->guid(); - hw_id_ = boost::lexical_cast(guid_); - updater.setHardwareIDf("%d", guid_); - - ROS_INFO("Started Prosilica camera with guid \"%d\"", (int)camera_->guid()); - } - else - { - updater.setHardwareID("unknown"); - if(prosilica::numCameras()>0) - { - state_info_ = "Trying to load first camera found"; - NODELET_INFO("%s", state_info_.c_str()); - guid_ = prosilica::getGuid(0); - camera_ = boost::make_shared((unsigned long)guid_); - hw_id_ = boost::lexical_cast(guid_); - updater.setHardwareIDf("%d", guid_); - ROS_INFO("Started Prosilica camera with guid \"%d\"", (int)guid_); - } - else - { - throw std::runtime_error("ERR: Found no cameras on local subnet"); - } - } - - } - catch (std::exception& e) - { - camera_state_ = CAMERA_NOT_FOUND; - std::stringstream err; - if (prosilica::numCameras() == 0) - { - err << "Hm. Found no cameras on local subnet"; - } - else if (guid_ != 0) - { - err << "Unable to open prosilica camera with guid " << guid_ <<": "<handle()); - syncCamToSysClock(); - ROS_BLUE("Camera starting"); - start(); - } - - std::string getAvailableCameras() - { - std::vector cameras = prosilica::listCameras(); - std::stringstream list; - for (unsigned int i = 0; i < cameras.size(); ++i) - { - list << cameras[i].serial << " - " <setAttribute("StreamBytesPerSecond", (tPvUint32)(camera_->max_data_rate / num_cameras)); - camera_->getAttribute("StreamBytesPerSecond", actualStreamBps); - ROS_INFO("Max data rate: %lu current set: %s", camera_->max_data_rate, actualStreamBps.c_str()); - } else { - ROS_WARN("Cannot set StreamBytesPerSecond"); - } - } - - void loadIntrinsics() - { - try - { - camera_->setKillCallback(boost::bind(&ProsilicaNodelet::kill, this, _1)); - - if(auto_adjust_stream_bytes_per_second_ && camera_->hasAttribute("StreamBytesPerSecond")) { - setSpeed(); - } - - - // Retrieve contents of user memory - std::string buffer(prosilica::Camera::USER_MEMORY_SIZE, '\0'); - camera_->readUserMemory(&buffer[0], prosilica::Camera::USER_MEMORY_SIZE); - - PvAttrRangeUint32(camera_->handle(), "BinningX", &dummy, &max_binning_x); - PvAttrRangeUint32(camera_->handle(), "BinningY", &dummy, &max_binning_y); - PvAttrRangeUint32(camera_->handle(), "Width", &dummy, &sensor_width_); - PvAttrRangeUint32(camera_->handle(), "Height", &dummy, &sensor_height_); - - - // Parse calibration file - std::string camera_name; - if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, cam_info_)) - { - intrinsics_ = "Loaded calibration"; - NODELET_INFO("Loaded calibration for camera '%s'", camera_name.c_str()); - } - else - { - intrinsics_ = "Failed to load intrinsics from camera"; - NODELET_WARN("Failed to load intrinsics from camera"); - } - } - catch(std::exception &e) - { - camera_state_ = CAMERA_NOT_FOUND; - state_info_ = e.what(); - } - } - - void start() - { - try - { - switch(trigger_mode_) - { - case prosilica::Software: - NODELET_INFO("starting camera %s in software trigger mode", hw_id_.c_str()); - camera_->start(prosilica::Software, 1., prosilica::Continuous); - if(update_rate_ > 0) - { - update_timer_.setPeriod(ros::Rate(update_rate_).expectedCycleTime()); - update_timer_.start(); - } - break; - case prosilica::Freerun: - NODELET_INFO("starting camera %s in freerun trigger mode", hw_id_.c_str()); - camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1)); - camera_->start(prosilica::Freerun, 1., prosilica::Continuous); - break; - case prosilica::FixedRate: - NODELET_INFO("starting camera %s in fixedrate trigger mode", hw_id_.c_str()); - camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1)); - camera_->start(prosilica::FixedRate, update_rate_, prosilica::Continuous); - break; - case prosilica::SyncIn1: - NODELET_INFO("starting camera %s in sync1 trigger mode", hw_id_.c_str()); - camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1)); - camera_->start(prosilica::SyncIn1, update_rate_, prosilica::Continuous); - break; - case prosilica::SyncIn2: - NODELET_INFO("starting camera %s in sync2 trigger mode", hw_id_.c_str()); - camera_->setFrameCallback(boost::bind(&ProsilicaNodelet::publishImage, this, _1)); - camera_->start(prosilica::SyncIn2, update_rate_, prosilica::Continuous); - break; - default: - break; - } - } - catch(std::exception &e) - { - camera_state_ = CAMERA_NOT_FOUND; - state_info_ = e.what(); - } - - try { - NODELET_INFO("exposure = %s", getCameraAttr("ExposureValue").value.c_str()); - } - catch(std::exception &e) { - camera_state_ = CAMERA_NOT_FOUND; - state_info_ = e.what(); - } - ROS_GREEN("start() complete"); - } - - void stop() - { - update_timer_.stop(); - if(!camera_) - return; - camera_->removeEvents(); - camera_->stop(); - - } - - void kill(unsigned long guid) - { - if(guid == guid_) - { - NODELET_WARN("[%s] got Camera::kill() request for prosilica camera %lu",getName().c_str(), guid); - //! Make sure we interrupt initialization (if it happened to still execute). - init_thread_.interrupt(); - init_thread_.join(); - - camera_state_ = CAMERA_NOT_FOUND; - state_info_ = "Prosilica camera " + hw_id_ + " disconnected"; - NODELET_ERROR("%s", state_info_.c_str()); - updater.update(); - boost::lock_guard scoped_lock(config_mutex_); - stop(); - camera_.reset(); - init_thread_ = boost::thread(boost::bind(&ProsilicaNodelet::openCamera, this)); - return; - } - } - - - int syncCamToSysClock() { - ros::Time before = ros::Time::now(); - NODELET_INFO("call syncCamToSysClock() "); - auto err = PvCommandRun(camera_->handle(), "TimeStampValueLatch"); - if (err != ePvErrSuccess) { - ROS_ERROR("Could not sync clock"); - return (int) err; - } - ros::Time after = ros::Time::now(); - tPvUint32 timelo, timehi, freq; - camera_->getAttribute("TimeStampValueHi", timehi); - camera_->getAttribute("TimeStampValueLo", timelo); - camera_->getAttribute("TimeStampFrequency", freq); - ros::Time tsframe = prosilica::CvtPvTimestamp(timehi, timelo, freq); - clock_offset = after - tsframe; - NODELET_INFO("Clock synced, offset = %lf", clock_offset.toSec()); - return 0; - } - - void publishImageProfile(tPvFrame* frame) - { - auto start_time = ros::Time::now(); -// publishImage(frame, ros::Time::now()); - if (frame->ImageBufferSize > 0) { - auto start_resize = ros::Time::now(); - ROS_WARN("Deprecated! publishImageProfile)"); - -// meta_frame->broken.data.resize(frame->ImageBufferSize); -// ROS_INFO("resizedImage in %ld/%.8f seconds", frame->ImageBufferSize, (ros::Time::now() - start_resize).toSec()); -// auto start_memcpy = ros::Time::now(); -// memcpy(&(meta_frame->broken.data)[0], frame->ImageBuffer, frame->ImageBufferSize); -// ROS_INFO("memcpy in %ld/%.8f seconds", frame->ImageBufferSize, (ros::Time::now() - start_memcpy).toSec()); - - } - auto end = ros::Time::now(); - ROS_INFO("publishImage0 in %.4f seconds", (end - start_time).toSec()); - } - - /// this will definitely take some finess to figure out. seems like there is a race condition which occasionally starts - /// on the 2-th (3rd) callback thunk. with the mutex, it freezes, without the mutex, it segs. - /// buffer initializes in order. error occurs roughly 1 in 5. - /// seems like when it segs, the meta_frame pointer is bad - /// I wonder if I should simplify the meta_frame struct in some way - void postProcessImage(prosilica::MetaFrame *meta_frame, int is_archiving) { - ROS_WARN("Deprecated! postPRocessImage(MetaFrame*)"); - -// ROS_INFO("postProcImage #? entry"); -// ROS_INFO("postProcImage %p #%d entry", meta_frame, meta_frame->idx); - auto start_time = ros::Time::now(); -// boost::lock_guard guard(meta_frame->frameMutex_); // this thing isn't releasing correctly, but without it, there's asegfault -// meta_frame->frameMutex_.lock(); // deliberate lock for testing -// ROS_INFO("is_archiving: %d", is_archiving); - if(is_archiving) { -// long int sec = meta_frame->img_.header.stamp.sec; -// long int nsec = meta_frame->img_.header.stamp.nsec; -// std::string filename = ArchiverHelper::generateFilename(envoy_, arch_opts_, sec, nsec); -// auto filename_written = dumpImageMessage(meta_frame->img_, filename); -// ROS_INFO("dumped #%d %s",meta_frame->idx, filename_written.c_str()); - } - auto end = ros::Time::now(); - ROS_WARN("postProcImage #%d in %.4f seconds",meta_frame->idx, (end - start_time).toSec()); - } - - void nop() { - ROS_WARN("nop"); - } - - /// todo: variably disable archiving and/or publishing. totally remove it and profile ePvWhatevr - void publishImage(tPvFrame* frame) - { bool ok = false; - try { - auto recv_time = ros::Time::now(); - ROS_INFO("yeeting frame buffer into new thread callback"); - if (false ) { - prosilica::PvFrameWrapperPtr pframe = prosilica::PvFrameWrapper::make_shared(frame); - /// ProsilicaNodelet *this, shared, ros::Time - ros::TimerCallback cb = [this, pframe, recv_time](ros::TimerEvent const &e) { - this->publishImage(pframe, recv_time); - }; - oneShotManager.addOneShot(nhp, ALMOST_INSTANT, cb); - } else { - this->publishImageOld(frame, recv_time); - } - ok = true; - - } catch (std::exception &e) { - ROS_ERROR("publishImage failed: %s", e.what()); - ok = false; - } - if (ok) { - watchdog.pet(); - } else { - watchdog.kick(); - } - } - - void publishImage(prosilica::PvFrameWrapperPtr pframe, ros::Time time) { - tPvFrame *frameptr = &pframe->frame_; - publishImageOld(frameptr, time); - } - - void publishImageOld(tPvFrame* frame, ros::Time time) - { - frame_recv_time_ = ros::Time::now(); - int is_archiving = 0; -// ROS_WARN("about to get"); - -// std::string msg = envoy_->get("foo"); -// ROS_WARN(msg.c_str()); - -// envoy_->get() -// nh.getParam("/sys/arch/is_archiving", is_archiving); - - camera_state_ = OK; - state_info_ = "Camera operating normally"; - - /** allow most recent event to be received - * There is a lot of async going on here, I'm sure there are better ways. - * Really the best way would be to use the PvCaptureWaitForFrameDone api - * call and do things more sync-like. But I am out of time for this problem. - */ - int seq_dt = 0; - int loop_count = 0; - do { - - seq_dt = event_.header.seq - last_published_.seq; - } while (seq_dt < 1 && loop_count++ < 3); - - std_msgs::Header gps_header; - /// todo: null check here or use context manager - prosilica::MetaFrame* meta_frame = (prosilica::MetaFrame*) frame->Context[0]; - if (!meta_frame) { - ROS_ERROR("tPvFrame context is null"); - return; - } - ROS_INFO("FrameDone %p #%ld @ %ld %ld", meta_frame, (long int) meta_frame->idx, (long int) frame->TimestampHi, (long int) frame->TimestampLo); - auto frame_stamp_time = prosilica::CvtPvTimestamp(frame->TimestampHi, frame->TimestampLo) + clock_offset; - - bool success = event_cache.search(frame_recv_time_, gps_header); - - ros::Time tsframe = prosilica::CvtPvTimestamp(frame->TimestampHi, frame->TimestampLo); - ros::Time corrFrameTime = tsframe + clock_offset; - NODELET_INFO("frameTime: %16.4f GPS: %16.4f DT: %7.4f", corrFrameTime.toSec(), gps_header.stamp.toSec(), (corrFrameTime-gps_header.stamp).toSec()); -// std::cout << "frameTime: " << corrFrameTime << ", GPS: " << gps_header.stamp << ", DT" << std::endl; -// std::cout << "ROS: "<< ros::Time::now() << ", frameTime: " << tsframe + clock_offset << ", TSF: " << tsframe << ", GPS: " << gps_header.stamp<< std::endl; -// std::cout << "ROS: "<< ros::Time::now() << ", frameTime: " << tsframe + clock_offset << ", TSF: " << tsframe << ", GPS: " << gps_header.stamp<< std::endl; - std::stringstream this_frame_id; - this_frame_id << frame_id_; - - // convey the status of the event binding process - if (success) { - this_frame_id << "?lock=1&eventNum=" << gps_header.seq << "&eventTime" << gps_header.stamp.toSec() ; - } else { - this_frame_id << "?lock=0"; - } - - - ros::Duration delta = frame_recv_time_ - event_.gps_time; -// auto seq = event_.header.seq; -// ROS_DEBUG("[%d] evt seq ", seq); - if (image_publisher_.getNumSubscribers() > 0) - { - auto nodeName = ros::this_node::getName(); - std::stringstream link; - custom_msgs::Stat stat_msg; - stat_msg.header.stamp = ros::Time::now(); - stat_msg.trace_topic = nodeName + "/publishImage"; - stat_msg.node = nodeName; - stat_msg.trace_header = std_msgs::Header(img_.header); - stat_msg.trace_header.seq = count_; - link << nodeName << "/event/" << event_.header.seq; // link this trace to the event trace - stat_msg.link = link.str(); - meta_frame->img_.header.stamp = event_.gps_time; - if (seq_dt > 1) { - ROS_ERROR("[%d] Missed %d frames, based on event seq ", event_.header.seq, seq_dt - 1); - for (auto i = 0; i < 4 && i < seq_dt - 1; i++) { - watchdog.kick(); - } - } - sensor_msgs::ImagePtr p_img = boost::make_shared(meta_frame->img_); - - if (processFrame(frame, *p_img, cam_info_)) // this will memcpy frame's buffer into img_ - { - // time taken to buffer image from cam -// ROS_DEBUG("[%d][3] Pub'ing! === Elapsed: %2.3f", seq, delta.toSec()); -// ROS_INFO("Publishing! === === : %2.3f", img_.header.stamp.toSec()); - -// Set the image timestamp to match the event that actually triggered it -// ROS image transport does its own thing with header sequence here, so we can't actually rely on that -// to group the events together. Fortunately, the time stamp itself is hashable. - if (success) { - stat_msg.note = "success"; - img_.header = gps_header; - } - sensor_msgs::CameraInfoConstPtr pc_cam_info = boost::make_shared(cam_info_); - meta_frame->img_.header.frame_id = this_frame_id.str(); - stat_pub_.publish(stat_msg); - image_publisher_.publish(p_img, pc_cam_info); -// ROS_WARN(" %d %d", event_.header.seq, img_.header.seq); - frames_dropped_acc_.add(0); - - } - else - { - ROS_ERROR("[?][3] Frame parse failed, checking status"); - auto status = frame->Status; - ROS_ERROR("[%d][3] Frame parse failed, frame status: %d %s", event_.header.seq, status, pv_error_codes[status]); - camera_state_ = FORMAT_ERROR; - state_info_ = "Unable to process frame"; - this_frame_id << "&status=" << status << "&error=" << pv_error_codes[status]; - std_msgs::Header msg = std_msgs::Header(img_.header); - broken_img_.header.stamp = img_.header.stamp; - broken_img_.header.frame_id = this_frame_id.str(); - msg.seq = (unsigned int) ++frames_dropped_total_; - missed_frames_pub_.publish(msg); - stat_msg.note = pv_error_codes[status]; - stat_pub_.publish(stat_msg); - errstat_pub_.publish(stat_msg); - image_publisher_.publish(broken_img_, cam_info_); - frames_dropped_acc_.add(1); - } - last_published_ = event_.header; - - - ++frames_completed_total_; - frames_completed_acc_.add(1); - } - updater.update(); - auto end = ros::Time::now(); - ROS_INFO("publishImage1 in %.4f seconds", (end - frame_recv_time_).toSec()); - } - - void updateCallback(const ros::TimerEvent &event) - { - // Download the most recent data from the device - camera_state_ = OK; - state_info_ = "Camera operating normally"; - if(image_publisher_.getNumSubscribers() > 0) - { - boost::lock_guard lock(config_mutex_); - try - { - tPvFrame* frame = NULL; - frame = camera_->grab(1000); - ROS_WARN("is this enabled?"); - publishImageOld(frame, event.current_real); - } - catch(std::exception &e) - { - camera_state_ = ERROR; - state_info_ = e.what(); - NODELET_ERROR("Unable to read from camera: %s", e.what()); - ++frames_dropped_total_; - frames_dropped_acc_.add(1); - updater.update(); - return; - } - } - } - - void pollCallback(polled_camera::GetPolledImage::Request& req, - polled_camera::GetPolledImage::Response& rsp, - sensor_msgs::Image& image, sensor_msgs::CameraInfo& info) - { - if (trigger_mode_ != prosilica::Software) - { - rsp.success = false; - rsp.status_message = "Camera is not in software triggered mode"; - return; - } - - last_config_.binning_x = req.binning_x; - last_config_.binning_y = req.binning_y; - last_config_.x_offset = req.roi.x_offset; - last_config_.y_offset = req.roi.y_offset; - last_config_.height = req.roi.height; - last_config_.width = req.roi.width; - - reconfigureCallback(last_config_, dynamic_reconfigure::SensorLevels::RECONFIGURE_RUNNING); - - try - { - tPvFrame* frame = NULL; - frame = camera_->grab(req.timeout.toSec()*100); - if (processFrame(frame, image, info)) - { - image.header.stamp = info.header.stamp =rsp.stamp = ros::Time::now(); - rsp.status_message = "Success"; - rsp.success = true; - } - else - { - rsp.success = false; - rsp.status_message = "Failed to process image"; - return; - } - } - catch(std::exception &e) - { - rsp.success = false; - std::stringstream err; - err<< "Failed to grab frame: "< syncInCallback <> \n"); - if (trigger_mode_ != prosilica::Software) - { - camera_state_ = ERROR; - state_info_ = "Can not sync from topic trigger unless in Software Trigger mode"; - NODELET_ERROR_ONCE("%s", state_info_.c_str()); - return; - } - ros::TimerEvent e; - e.current_real = msg->stamp; - updateCallback(e); - } - - void dumperCallback (const sensor_msgs::ImageConstPtr &msg) { - int is_archiving = ArchiverHelper::get_is_archiving(envoy_, "/sys/arch/is_archiving"); - NODELET_INFO("dumper: is archiving: %d", is_archiving); - if(is_archiving) { - long int sec = msg->header.stamp.sec; - long int nsec = msg->header.stamp.nsec; - std::string filename = ArchiverHelper::generateFilename(envoy_, arch_opts_, sec, nsec); - try { - bool debayer{false}; - - if ("rgb" == cam_channel) { - debayer = true; - } - auto filename_written = dumpImageMessage(msg, filename, debayer ); - ROS_INFO("[%s] dumped #%d %s", cam_channel.c_str(), msg->header.seq, filename_written.c_str()); - } catch (cv_bridge::Exception &e) { - ROS_ERROR("%s", e.what()); - } - - } - } - - /// void (ProsilicaNodelet::*)(const custom_msgs::GSOF_WVTConstPtr&) = ProsilicaNodelet::eventCallback - void eventCallback (const boost::shared_ptr& msg) - { - ROS_INFO("[%d]<1> eventCallback <> %2.2f", msg->header.seq, msg->gps_time.toSec()); - // I am pretty sure this does not reference leak but this is not my wheelhouse - event_ = *msg; - event_cache.push_back(msg->sys_time, msg); - auto nodeName = getName(); - custom_msgs::Stat stat_msg; - std::stringstream link; - stat_msg.header.stamp = ros::Time::now(); - stat_msg.trace_header = (*msg).header; - stat_msg.trace_topic = nodeName + "/eventCallback"; - stat_msg.node = nodeName; - link << nodeName << "/event/" << event_.header.seq; // link this trace to the event trace - stat_msg.link = link.str(); - stat_pub_.publish(stat_msg); - event_cache.purge(); - watchdog.check(); -// event_cache.show(); - - -// std::cout << stat_msg << "\n---" << std::endl; - } - - /** Exposure is in microseconds (microsecs). Minimum varies by camera I think. */ - void exposureCallback (const std_msgs::Int32Ptr &msg) - { - printf("\n <> exposureCallback <> \n"); - tPvUint32 microsecs_min = 30; - int32_t microsecs = msg->data; - if (microsecs < 0) { - printf("WARNING: Exposure set to less that zero. Setting auto exposure. This is not a recommended feature"); - camera_->setExposure(microsecs, prosilica::Auto); - return; - } - tPvUint32 umicrosecs = msg->data; - - if (umicrosecs < microsecs_min) { - printf("WARNING: Exposure set to less than max allowed. Clipping to %lu", microsecs_min); - umicrosecs = microsecs_min; - } - printf("INFO: Exposure set to: %lu microseconds", umicrosecs); - camera_->setExposure(umicrosecs, prosilica::Manual); -// camera_->setAttribute("ExposureValue", (tPvUint32)microsecs); // redundant? - - } - - void gainCallback (const std_msgs::Int32Ptr &msg) - { - printf("\n <> gainCallback <> \n"); - const tPvUint32 gain_max = 24; - int32_t gain = msg->data; - if (gain < 0) { - printf("WARNING: Gain set to less that zero. Setting auto_gain once. This is not a recommended feature"); - camera_->setGain(gain, prosilica::AutoOnce); - return; - } - tPvUint32 ugain = (tPvUint32) gain; - if (ugain > gain_max) { - printf("WARNING: Gain set to greater than max allowed. Clipping to %lu/n", gain_max); - ugain = gain_max; - } - printf("INFO: Gain set to: %lu", ugain); - camera_->setGain( ugain, prosilica::Manual); - - } - bool health(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &rsp) { - auto healthy = watchdog.Ok(); - rsp.success = healthy; - if (!healthy) { - rsp.message = "Watchdog timed out"; - } - return healthy; - } - - - // todo: check if I want return false, or return true with error code on srv - /** this is a pretty gross api. It's stringly-typed, so be careful - * also currently does not work with certain types. - * Note: use "1"/"0" for pushing bools. They are pretty rare though*/ - bool setCameraAttr(custom_msgs::CamSetAttrRequest &req, custom_msgs::CamSetAttrResponse &rsp) { - NODELET_INFO(" setCameraAttr(%s, %s)", req.name.c_str(), req.value.c_str()); - tPvHandle handle = camera_->handle(); - rsp.tPvErr = ePvErrUnknown; - rsp.value = "error"; - const char *c_name = req.name.c_str(); - - if (req.name == "SyncClock") { - rsp.tPvErr = syncCamToSysClock(); - rsp.dtype = "time"; - rsp.value = std::to_string(clock_offset.toSec()); - return (rsp.tPvErr != 0); - } - - /** On failure, pass error to message. This is more descriptive than - * return false*/ - rsp.tPvErr = PvAttrIsAvailable(handle, c_name); - if (rsp.tPvErr != 0) { return true; } - - tPvAttributeInfo info; - - rsp.tPvErr = PvAttrInfo(handle, c_name, &info); - if (rsp.tPvErr != 0) { return true; } - - - switch (info.Datatype) { - case ePvDatatypeEnum: { - rsp.tPvErr = PvAttrEnumSet( - handle, c_name, req.value.c_str()); - rsp.dtype = "enum"; - break; - } - case ePvDatatypeString: { - rsp.tPvErr = PvAttrStringSet( - handle, c_name, req.value.c_str()); - rsp.dtype = "string"; - break; - } - case ePvDatatypeUint32: { - rsp.tPvErr = PvAttrUint32Set( - handle, c_name, std::stoul(req.value)); - rsp.dtype = "uint32"; - break; - } - case ePvDatatypeInt64: { - rsp.tPvErr = PvAttrInt64Set( - handle, c_name, std::stol(req.value)); - rsp.dtype = "int64"; - break; - } - case ePvDatatypeFloat32: { - rsp.tPvErr = PvAttrFloat32Set( - handle, c_name, std::stof(req.value)); - rsp.dtype = "float32"; - break; - } - case ePvDatatypeBoolean: { - rsp.tPvErr = PvAttrBooleanSet( - handle, c_name, std::stoi(req.value)); - rsp.dtype = "bool"; - break; - } - case ePvDatatypeCommand: { - rsp.tPvErr = PvCommandRun( - handle, c_name); - rsp.dtype = "PvCommandRun"; - rsp.value = "ok"; - return true; - } - default: { - rsp.tPvErr = ePvErrBadParameter; - break; - } - } - if (rsp.tPvErr != 0) { return true; } - custom_msgs::CamGetAttrResponse new_rsp = getCameraAttr(req.name); - rsp.value = new_rsp.value; - - return true; - } - - bool getCameraAttr(custom_msgs::CamGetAttrRequest &req, custom_msgs::CamGetAttrResponse &rsp) { - NODELET_INFO(" getCameraAttr(%s)", req.name.c_str()); - - tPvUint32 value; - try { - rsp = getCameraAttr(req.name); - } - catch (prosilica::ProsilicaException) { - rsp.value = "error"; - } - return true; - } - - custom_msgs::CamGetAttrResponse getCameraAttr(std::string name) { - tPvHandle handle = camera_->handle(); - custom_msgs::CamGetAttrResponse rsp; - rsp.tPvErr = ePvErrUnknown; - rsp.value = "error"; - const char *c_name = name.c_str(); - - rsp.tPvErr = PvAttrIsAvailable(handle, c_name); - if (rsp.tPvErr != 0) { return rsp; } - - tPvAttributeInfo info; - - rsp.tPvErr = PvAttrInfo(handle, c_name, &info); - if (rsp.tPvErr != 0) { return rsp; } - - - switch (info.Datatype) { - case ePvDatatypeEnum: - camera_->getAttributeEnum(name, rsp.value); - rsp.tPvErr = ePvErrSuccess; - rsp.dtype = "enum"; - break; - case ePvDatatypeString: - rsp.tPvErr = prosilica::getAttribute(handle, c_name, rsp.value); - rsp.dtype = "string"; - break; - case ePvDatatypeUint32: - tPvUint32 value; - rsp.tPvErr = PvAttrUint32Get(handle, c_name, &value); - rsp.dtype = "uint32"; - rsp.value = std::to_string((unsigned long) value); - break; - case ePvDatatypeInt64: - tPvInt64 l_value; - rsp.tPvErr = PvAttrInt64Get(handle, c_name, &l_value); - rsp.dtype = "int64"; - rsp.value = std::to_string((int64_t) l_value); - break; - case ePvDatatypeFloat32: - tPvFloat32 f_value; - rsp.tPvErr = PvAttrFloat32Get(handle, c_name, &f_value); - rsp.dtype = "float32"; - rsp.value = std::to_string((float) f_value); - break; - case ePvDatatypeBoolean: - tPvBoolean b_value; - rsp.tPvErr = PvAttrBooleanGet(handle, c_name, &b_value); - rsp.dtype = "bool"; - rsp.value = std::to_string((bool) b_value); - break; - default: - rsp.tPvErr = ePvErrBadParameter; - break; - } - return rsp; - - } - - bool getAttrList(custom_msgs::StrListRequest &req, custom_msgs::StrListResponse &rsp) { - tPvAttrListPtr pListPtr; - unsigned long sz; - //typedef const char* const* tPvAttrListPtr; - // The attribute list is contained in memory allocated by the PvApi module. - tPvErr err = PvAttrList(camera_->handle(), &pListPtr, &sz); - NODELET_WARN("sz: %ld ", sz); - if (err != 0) { - rsp.tPvErr = err; - rsp.values[0] = "error"; - return false; - } - for (int i = 0; i < sz; i++) { - rsp.values.push_back(pListPtr[i]); - } - return true; - } - // this calls frameToImage which calls fillImage which calls memcpy - bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info) - { - auto start_time = ros::Time::now(); - /// @todo Match time stamp from frame to ROS time? - if (frame==NULL ) { - return false; - } - // we want to deliberately allow some missing-date frames through - // for debugging - if (frame->Status == ePvErrSuccess) { - // pass - } else if (frame->Status == ePvErrDataMissing) { - // pass - NODELET_WARN("Data Missing from Frame. This may fail"); - } else { - return false; // you shall not pass - } - try - { - /// @todo Binning values retrieved here may differ from the ones used to actually - /// capture the frame! Maybe need to clear queue when changing binning and/or - /// stuff binning values into context? - tPvUint32 binning_x = 1, binning_y = 1; - if (auto_adjust_binning_) { - if (camera_->hasAttribute("BinningX")) { - camera_->getAttribute("BinningX", binning_x); - camera_->getAttribute("BinningY", binning_y); - } - } - - // Binning averages bayer samples, so just call it mono8 in that case - if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1)) - frame->Format = ePvFmtMono8; - - if (!frameToImage(frame, img)) { -// NODELET_WARN("Failed to parse frame to image message"); - return false; - } /// this suceeds === === === === === === VVV - // Set the operational parameters in CameraInfo (binning, ROI) - cam_info.binning_x = binning_x; - cam_info.binning_y = binning_y; - // ROI in CameraInfo is in unbinned coordinates, need to scale up - cam_info.roi.x_offset = frame->RegionX * binning_x; - cam_info.roi.y_offset = frame->RegionY * binning_y; - cam_info.roi.height = frame->Height * binning_y; - cam_info.roi.width = frame->Width * binning_x; - cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) || - (frame->Width != sensor_width_ / binning_x); - - /// this doesn't seem like it's doing anything, but chesterton's fence - if (false) { - if (auto_adjust_stream_bytes_per_second_ && camera_->hasAttribute("StreamBytesPerSecond")) - camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(camera_->max_data_rate / num_cameras)); - } - } - catch(std::exception &e) - { - return false; - } - auto end = ros::Time::now(); - /// === === === === === === ^^^ - ROS_INFO("processFrame in %.4f seconds", (end - start_time).toSec()); - - count_++; - return true; - } - - bool frameToImage(tPvFrame* frame, sensor_msgs::Image &image) - { - auto start_time = ros::Time::now(); - - // NOTE: 16-bit and Yuv formats not supported - static const char* BAYER_ENCODINGS[] = { "bayer_rggb8", "bayer_gbrg8", "bayer_grbg8", "bayer_bggr8" }; - - std::string encoding; - if (frame->Format == ePvFmtMono8) encoding = sensor_msgs::image_encodings::MONO8; - else if (frame->Format == ePvFmtBayer8) encoding = BAYER_ENCODINGS[frame->BayerPattern]; - else if (frame->Format == ePvFmtRgb24) encoding = sensor_msgs::image_encodings::RGB8; - else if (frame->Format == ePvFmtBgr24) encoding = sensor_msgs::image_encodings::BGR8; - else if (frame->Format == ePvFmtRgba32) encoding = sensor_msgs::image_encodings::RGBA8; - else if (frame->Format == ePvFmtBgra32) encoding = sensor_msgs::image_encodings::BGRA8; - else { - NODELET_WARN("Received frame with unsupported pixel format %d", frame->Format); - return false; - } - - - if(frame->ImageSize == 0) { - /** image size for GT6600 is 28829184. You can try to spoof the buffer - * size check but you will get stale data - */ - -// NODELET_WARN("Image size is zero but will try to recover"); -// frame->ImageSize = ; // hack - NODELET_WARN("Image size is zero"); - return false; - } - if(frame->Height == 0) { - NODELET_WARN("Image height is zero"); - return false; - } - - uint32_t step = frame->ImageSize / frame->Height; - // fillImage calls memcpy - auto out = sensor_msgs::fillImage(image, encoding, frame->Height, frame->Width, step, frame->ImageBuffer); - auto end = ros::Time::now(); - /// makes it to here before freezing - ROS_INFO("frameToImage in %.4f seconds, format: %ld", (end - start_time).toSec(), (long int) frame->Format); - - return out; - } - - bool setCameraInfo(sensor_msgs::SetCameraInfoRequest &req, sensor_msgs::SetCameraInfoResponse &rsp) - { - NODELET_INFO(" New camera info received"); - sensor_msgs::CameraInfo &info = req.camera_info; - - // Sanity check: the image dimensions should match the max resolution of the sensor. - if (info.width != sensor_width_ || info.height != sensor_height_) - { - rsp.success = false; - rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video " - "setting, camera running at resolution %ix%i.") - % info.width % info.height % sensor_width_ % sensor_height_).str(); - NODELET_ERROR("%s", rsp.status_message.c_str()); - return true; - } - - stop(); - - std::string cam_name = "prosilica"; - cam_name += hw_id_; - std::stringstream ini_stream; - if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, cam_name, info)) - { - rsp.status_message = "Error formatting camera_info for storage."; - rsp.success = false; - } - else - { - std::string ini = ini_stream.str(); - if (ini.size() > prosilica::Camera::USER_MEMORY_SIZE) - { - rsp.success = false; - rsp.status_message = "Unable to write camera_info to camera memory, exceeded storage capacity."; - } - else - { - try - { - camera_->writeUserMemory(ini.c_str(), ini.size()); - cam_info_ = info; - rsp.success = true; - } - catch (prosilica::ProsilicaException &e) - { - rsp.success = false; - rsp.status_message = e.what(); - } - } - } - if (!rsp.success) - NODELET_ERROR("%s", rsp.status_message.c_str()); - - start(); - - return true; - } - - void reconfigureCallback(prosilica_camera::ProsilicaCameraConfig &config, uint32_t level) - { - printf("\n<> Reconf callback \n"); - NODELET_DEBUG("Reconfigure request received"); - - if (level >= (uint32_t)dynamic_reconfigure::SensorLevels::RECONFIGURE_STOP) - stop(); - - //! Trigger mode - if (config.trigger_mode == "streaming") - { - trigger_mode_ = prosilica::Freerun; - update_rate_ = 1.; // make sure we get _something_ - } - else if (config.trigger_mode == "syncin1") - { - trigger_mode_ = prosilica::SyncIn1; - update_rate_ = config.trig_rate; - } - else if (config.trigger_mode == "syncin2") - { - trigger_mode_ = prosilica::SyncIn2; - update_rate_ = config.trig_rate; - } - else if (config.trigger_mode == "fixedrate") - { - trigger_mode_ = prosilica::FixedRate; - update_rate_ = config.trig_rate; - } - else if (config.trigger_mode == "software") - { - trigger_mode_ = prosilica::Software; - update_rate_ = config.trig_rate; - } - - else if (config.trigger_mode == "polled") - { - trigger_mode_ = prosilica::Software; - update_rate_ = 0; - } - else if (config.trigger_mode == "triggered") - { - trigger_mode_ = prosilica::Software; - update_rate_ = 0; - } - else - { - NODELET_ERROR("Invalid trigger mode '%s' in reconfigure request", config.trigger_mode.c_str()); - } - - if(config.trig_timestamp_topic != last_config_.trig_timestamp_topic) - { - trigger_sub_.shutdown(); - trig_timestamp_topic_ = config.trig_timestamp_topic; - } - - if(!trigger_sub_ && config.trigger_mode == "triggered") - { - trigger_sub_ = ros::NodeHandle().subscribe(trig_timestamp_topic_, 1, &ProsilicaNodelet::syncInCallback, this); - } - - - // Exposure - if (config.auto_exposure) - { - camera_->setExposure(0, prosilica::Auto); - if (camera_->hasAttribute("ExposureAutoMax")) - { - tPvUint32 us = config.exposure_auto_max*1000000. + 0.5; - camera_->setAttribute("ExposureAutoMax", us); - } - if (camera_->hasAttribute("ExposureAutoTarget")) - camera_->setAttribute("ExposureAutoTarget", (tPvUint32)config.exposure_auto_target); - } - else - { - unsigned us = config.exposure*1000000. + 0.5; - camera_->setExposure(us, prosilica::Manual); - camera_->setAttribute("ExposureValue", (tPvUint32)us); - } - - // Gain - if (config.auto_gain) - { - if (camera_->hasAttribute("GainAutoMax")) - { - camera_->setGain(0, prosilica::Auto); - camera_->setAttribute("GainAutoMax", (tPvUint32)config.gain_auto_max); - camera_->setAttribute("GainAutoTarget", (tPvUint32)config.gain_auto_target); - } - else - { - tPvUint32 major, minor; - camera_->getAttribute("FirmwareVerMajor", major); - camera_->getAttribute("FirmwareVerMinor", minor); - NODELET_WARN("Auto gain not available for this camera. Auto gain is available " - "on firmware versions 1.36 and above. You are running version %u.%u.", - (unsigned)major, (unsigned)minor); - config.auto_gain = false; - } - } - else - { - camera_->setGain(config.gain, prosilica::Manual); - camera_->setAttribute("GainValue", (tPvUint32)config.gain); - } - - // White balance - if (config.auto_whitebalance) - { - if (camera_->hasAttribute("WhitebalMode")) - camera_->setWhiteBalance(0, 0, prosilica::Auto); - else - { - NODELET_WARN("Auto white balance not available for this camera."); - config.auto_whitebalance = false; - } - } - else - { - camera_->setWhiteBalance(config.whitebalance_blue, config.whitebalance_red, prosilica::Manual); - if (camera_->hasAttribute("WhitebalValueRed")) - camera_->setAttribute("WhitebalValueRed", (tPvUint32)config.whitebalance_red); - if (camera_->hasAttribute("WhitebalValueBlue")) - camera_->setAttribute("WhitebalValueBlue", (tPvUint32)config.whitebalance_blue); - } - - // Binning configuration - if (camera_->hasAttribute("BinningX")) - { - config.binning_x = std::min(config.binning_x, (int)max_binning_x); - config.binning_y = std::min(config.binning_y, (int)max_binning_y); - - camera_->setBinning(config.binning_x, config.binning_y); - } - else if (config.binning_x > 1 || config.binning_y > 1) - { - NODELET_WARN("Binning not available for this camera."); - config.binning_x = config.binning_y = 1; - } - - // Region of interest configuration - // Make sure ROI fits in image - config.x_offset = std::min(config.x_offset, (int)sensor_width_ - 1); - config.y_offset = std::min(config.y_offset, (int)sensor_height_ - 1); - config.width = std::min(config.width, (int)sensor_width_ - config.x_offset); - config.height = std::min(config.height, (int)sensor_height_ - config.y_offset); - // If width or height is 0, set it as large as possible - int width = config.width ? config.width : sensor_width_ - config.x_offset; - int height = config.height ? config.height : sensor_height_ - config.y_offset; - - // Adjust full-res ROI to binning ROI - /// @todo Replicating logic from polledCallback - int x_offset = config.x_offset / config.binning_x; - int y_offset = config.y_offset / config.binning_y; - unsigned int right_x = (config.x_offset + width + config.binning_x - 1) / config.binning_x; - unsigned int bottom_y = (config.y_offset + height + config.binning_y - 1) / config.binning_y; - // Rounding up is bad when at max resolution which is not divisible by the amount of binning - right_x = std::min(right_x, (unsigned)(sensor_width_ / config.binning_x)); - bottom_y = std::min(bottom_y, (unsigned)(sensor_height_ / config.binning_y)); - width = right_x - x_offset; - height = bottom_y - y_offset; - - camera_->setRoi(x_offset, y_offset, width, height); - - // TF frame - img_.header.frame_id = cam_info_.header.frame_id = config.frame_id; - - // Normally the node adjusts the bandwidth used by the camera during diagnostics, to use as - // much as possible without dropping packets. But this can create interference if two - // cameras are on the same switch, e.g. for stereo. So we allow the user to set the bandwidth - // directly. - auto_adjust_stream_bytes_per_second_ = config.auto_adjust_stream_bytes_per_second; - if (!auto_adjust_stream_bytes_per_second_) - camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)config.stream_bytes_per_second); - else - camera_->setAttribute("StreamBytesPerSecond", (tPvUint32)(camera_->max_data_rate/num_cameras)); - - //! If exception thrown due to bad settings, it will fail to start camera - //! Reload last good config - if (level >= (uint32_t)dynamic_reconfigure::SensorLevels::RECONFIGURE_STOP) - { - try - { - start(); - } - catch(std::exception &e) - { - NODELET_ERROR("Invalid settings: %s", e.what()); - config = last_config_; - } - } - - last_config_ = config; - } - - void getCurrentState(diagnostic_updater::DiagnosticStatusWrapper &stat) - { - stat.add("Serial", guid_); - stat.add("Info", state_info_); - stat.add("Intrinsics", intrinsics_); - stat.add("Total frames dropped", frames_dropped_total_); - stat.add("Total frames", frames_completed_total_); - - if(frames_completed_total_>0) - { - stat.add("Total % frames dropped", 100.*(double)frames_dropped_total_/frames_completed_total_); - } - if(frames_completed_acc_.sum()>0) - { - stat.add("Recent % frames dropped", 100.*frames_dropped_acc_.sum()/frames_completed_acc_.sum()); - } - - switch (camera_state_) - { - case OPENING: - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Opening camera"); - break; - case OK: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Camera operating normally"); - break; - case CAMERA_NOT_FOUND: - stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Can not find camera %d", guid_ ); - stat.add("Available Cameras", getAvailableCameras()); - break; - case FORMAT_ERROR: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Problem retrieving frame"); - break; - case ERROR: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Camera has encountered an error"); - break; - default: - break; - } - } -}; - - - -} // end namespace - -/** === === === === === === === === === === === === */ -void driver_shutdown() { - for ( auto const& it: active_nodelets) - { - ROS_WARN("Stopping nodelet %d", it.first); - it.second->public_stop(); - } -} - -/** Makes a placeholder image for broken frames - * @param img - image message to populate - * @return success - */ -bool build_broken_img(sensor_msgs::Image& img) { - sensor_msgs::clearImage(img); -} - - -#include -PLUGINLIB_EXPORT_CLASS(prosilica_camera::ProsilicaNodelet, nodelet::Nodelet); - diff --git a/src/cams/prosilica_camera/src/nodes/view_server_nodelet.cpp b/src/cams/prosilica_camera/src/nodes/view_server_nodelet.cpp deleted file mode 100644 index b1b5664a..00000000 --- a/src/cams/prosilica_camera/src/nodes/view_server_nodelet.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using std::cout; -using std::endl; - -namespace prosilica_camera -{ - class ViewServerNodelet : public nodelet::Nodelet - { - public: - ViewServerNodelet() - { - } - - private: - // Cache image, its header, and its encoding - std::deque> image_q_; - std::map> frame2H; - std::map frame2newimg; - cv_bridge::CvImage img_bridge; - ros::ServiceServer image_view_service_; - image_transport::Subscriber sub; - - virtual void onInit() { - ROS_INFO("Image View Server: Initialization"); - // ROS initialization - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - image_transport::ImageTransport it(nh); - std::string ns = ros::this_node::getNamespace(); - std::string topic = ns + "/image_raw"; - std::cout << topic << "\n\n\n"; - sub = it.subscribe(topic, 1, - &ViewServerNodelet::image_callback, this); - image_view_service_ = pnh.advertiseService("get_image_view", - &ViewServerNodelet::getImageView, this); - ROS_INFO("Image View Server: Finished"); - }; - - void image_callback(const sensor_msgs::ImageConstPtr& msg) { - ROS_INFO("Received Image!"); - // Keep a running most-recent queue of size 1 - // Decompress message into raw image, and store that in q - auto tic = std::chrono::high_resolution_clock::now(); - cv_bridge::CvImagePtr cv_ptr; - std::string encoding = msg->encoding; - if (encoding == "bayer_grbg8") - encoding = sensor_msgs::image_encodings::RGB8; - try - { - cv_ptr = cv_bridge::toCvCopy(msg, encoding); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - cv::Mat cv_image = cv_ptr->image.clone(); - std_msgs::Header header = msg->header; - std::tuple T = std::make_tuple( - cv_image, header, encoding); - if ( !image_q_.empty() ) { - image_q_.pop_front(); - image_q_.push_back(T); - } else { - image_q_.push_back(T); - } - // Reset cache - for (auto const& x : frame2newimg) - { - frame2newimg[x.first] = true; - } - ROS_INFO_STREAM("Size of image queue is: " << image_q_.size()); - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic; - ROS_INFO_STREAM("View Server: Time to process image was: " << dt.count() / 1e9 << "s\n"); - }; - - bool getImageView(custom_msgs::RequestImageView::Request& req, - custom_msgs::RequestImageView::Response& resp) { - auto tic = std::chrono::high_resolution_clock::now(); - ROS_INFO_STREAM("Size of image queue is: " << image_q_.size()); - std::tuple T; - cv::Mat cv_image; - std_msgs::Header header; - std::string encoding; - if ( !image_q_.empty() ) { - T = image_q_[0]; - cv_image = std::get<0>(T); - header = std::get<1>(T); - encoding = std::get<2>(T); - } else { - resp.success = false; - resp.image = sensor_msgs::Image(); - return false; - } - - std::vector H = req.homography; - // If the last requested hasn't changed, don't bother returning - bool stale_H; - try { - stale_H = H == frame2H[req.frame]; - } catch (...) { - stale_H = false; - } - // If homography is the same as last frequest for the same frame, - // and no new image has arrived, return a null frame. - if (stale_H && !frame2newimg[req.frame]) { - resp.success = false; - resp.image = sensor_msgs::Image(); - return true; - } - // Cache new values - frame2H[req.frame] = H; - // Global variable holding state if a new image has been received - frame2newimg[req.frame] = false; - - ROS_INFO_STREAM("View Server: Received image of width: " << cv_image.size().width << - " and height: " << cv_image.size().height << - " channels: " << cv_image.channels() << std::endl); - int h = req.output_height; - int w = req.output_width; - int interp = req.interpolation; - try { - - cv::Mat imgWarp = cv::Mat(cv::Size(w, h), cv_image.type()); - - cv::Mat warp_matrix; - warp_matrix = cv::Mat::eye(3, 3, CV_32F); - int row = 0; - int col = 0; - for (auto &hom : H) - { - // stuff the values into the matrix - warp_matrix.at(row, col) = hom; - //ROS_INFO_STREAM(" " << hom); - col++; - if (col > 2) - { - col = 0; - row++; - } - } - cv::warpPerspective(cv_image, imgWarp, warp_matrix.inv(), cv::Size(w, h), interp); - cv::Mat claheWarp; - if(req.apply_clahe) { - cv::Ptr clahe = cv::createCLAHE(); - clahe->setClipLimit(req.contrast_strength); - clahe->setTilesGridSize(cv::Size(8, 8)); - clahe->apply(imgWarp, claheWarp); - } else { - claheWarp = imgWarp; - } - - // convert UV mono images to color - cv::Mat finalWarp; - if (claheWarp.channels() < 3) { - cv::cvtColor(claheWarp, finalWarp, cv::COLOR_GRAY2RGB); - } else { - finalWarp = claheWarp; - } - if (req.show_saturated_pixels) { - int maxval = 255; - cv::Scalar sat_pix = cv::Scalar(maxval, maxval, maxval); - cv::Mat mask; - cv::inRange(claheWarp, sat_pix, sat_pix, mask); - // Set white pixels to red - finalWarp.setTo(cv::Scalar(255, 0, 0), mask); - } - - // Debayer raw rgb - // Always return "color" image - img_bridge = cv_bridge::CvImage(header, - sensor_msgs::image_encodings::RGB8, finalWarp); - sensor_msgs::Image output_msg; - img_bridge.toImageMsg(output_msg); - - resp.success = true; - resp.image = output_msg; - } catch (...) { - ROS_INFO("CV Warp exception."); - resp.success = false; - return false; - } - - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic; - ROS_INFO_STREAM("View Server: Time to process image request for " << req.frame << " was: " << dt.count() / 1e9 << "s\n"); - return true; - }; - }; - - -PLUGINLIB_EXPORT_CLASS(prosilica_camera::ViewServerNodelet, nodelet::Nodelet); -} // end namespace diff --git a/src/cams/prosilica_camera/streaming.launch b/src/cams/prosilica_camera/streaming.launch deleted file mode 100644 index 1c2ff0cc..00000000 --- a/src/cams/prosilica_camera/streaming.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - From e0056b24ec37b09da48fd44fb75a0adc18577004 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:38:58 -0400 Subject: [PATCH 08/20] Port sprokit_adapters (VIAME detector adapter) to ROS2 - kw_detector_fusion_adapter_node ported roscpp -> rclcpp: node parameters replace private-nodehandle params, background spin thread replaces the AsyncSpinner, detections publish on ~/detections_out remapped as before - Dropped sources that were never built into the node: the ros_dynamic_config / ros_detector_scaling sprokit plugins (ROS1 dynamic_reconfigure based) and netbeans project files - publish_sync_msgs / rebroadcast / save_* debug scripts left as-is; they are not launched by nayak/taiga supervisors - Launch converted to ROS2 XML --- .../sprokit_adapters/CMakeLists.txt | 110 +++------ .../config/debug_rosconsole.conf | 7 - .../launch/publish_sync_msgs.launch | 11 - .../sprokit_detector_fusion_adapter.launch | 104 -------- ...sprokit_detector_fusion_adapter.launch.xml | 67 ++++++ .../nbproject/configurations.xml | 52 ---- .../nbproject/private/Default.properties | 0 .../nbproject/private/configurations.xml | 107 --------- .../nbproject/private/launcher.properties | 40 --- .../nbproject/private/private.xml | 10 - .../sprokit_adapters/nbproject/project.xml | 26 -- .../sprokit_adapters/package.xml | 63 +---- .../src/kw_detector_fusion_adapter.cpp | 227 +++++++----------- .../src/ros_detector_scaling.cxx | 138 ----------- .../src/ros_detector_scaling.h | 50 ---- .../src/ros_dynamic_config.cxx | 142 ----------- .../sprokit_adapters/src/ros_dynamic_config.h | 62 ----- 17 files changed, 199 insertions(+), 1017 deletions(-) delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/config/debug_rosconsole.conf delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/launch/publish_sync_msgs.launch delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch create mode 100644 src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch.xml delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/configurations.xml delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/private/Default.properties delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/private/configurations.xml delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/private/launcher.properties delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/private/private.xml delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/nbproject/project.xml delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.cxx delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.h delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.cxx delete mode 100644 src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.h diff --git a/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt b/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt index dc7b673a..358fd041 100644 --- a/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt +++ b/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt @@ -1,30 +1,16 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.8) project(sprokit_adapters) set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -## 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 - cv_bridge - roscpp - std_msgs - custom_msgs - image_transport - eigen_conversions - roskv - ) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -#find_package(fletch REQUIRED) -#find_package(kwiver REQUIRED) -#find_package(OpenCV REQUIRED) -catkin_package() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(custom_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(roskv REQUIRED) -########### -## Build ## -########### find_package(KWIVER REQUIRED) set(KWIVER_CMAKE_DIR "/opt/noaa/viame/lib/cmake/kwiver/") @@ -37,76 +23,36 @@ list( INSERT CMAKE_MODULE_PATH 0 "${KWIVER_CMAKE_DIR}" ) include(kwiver-cmake-future) include(kwiver-utils) -add_definitions(-std=c++17) - - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) include_directories( - ${catkin_INCLUDE_DIRS} ${KWIVER_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - ${EIGEN_INCLUDE_DIR} - ${cv_bridge_INCLUDE_DIRS} ${CMAKE_BINARY_DIR} ) -## Declare a C++ executable link_directories( ${KWIVER_LIBRARY_DIR} ) add_executable(kw_detector_fusion_adapter_node src/kw_detector_fusion_adapter.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(sprokit_adapters_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -# <------------ add hiredis dependency ---------------> -find_path(HIREDIS_HEADER hiredis) -find_library(HIREDIS_LIB hiredis) -# <------------ add redis-plus-plus dependency --------------> -# NOTE: this should be *sw* NOT *redis++* -find_path(REDIS_PLUS_PLUS_HEADER sw) -find_library(REDIS_PLUS_PLUS_LIB redis++) - -## Specify libraries to link a library or executable target against +ament_target_dependencies(kw_detector_fusion_adapter_node + rclcpp std_msgs sensor_msgs custom_msgs cv_bridge roskv) target_link_libraries( kw_detector_fusion_adapter_node - ${catkin_LIBRARIES} - ${KWIVER_LIBRARIES} - ${OpenCV_LIBRARIES} - ${HIREDIS_LIB} - ${REDIS_PLUS_PLUS_LIB} - sprokit_pipeline sprokit_pipeline_util - ) - -### -# Add detector scaling plugin -kwiver_add_plugin( ros_detector_scaling - SOURCES src/ros_detector_scaling.h - src/ros_detector_scaling.cxx - PRIVATE vital - vital_algo - vital_vpm + kwiver_adapter + vital vital_config - ${catkin_LIBRARIES} + vital_exceptions + vital_logger + vital_util + kwiver_algo_ocv + sprokit_pipeline + sprokit_pipeline_util + ${OpenCV_LIBRARIES} ) -# need to install plugin in kwiver area to simplify loading. -set_target_properties( ros_detector_scaling - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${KWIVER_MODULE_DIR} ) - -### -# Add dynamic config provider -kwiver_add_plugin( ros_dynamic_config - SOURCES src/ros_dynamic_config.h - src/ros_dynamic_config.cxx - PRIVATE vital - vital_algo - vital_vpm - vital_config - ${catkin_LIBRARIES} - ) +install(TARGETS kw_detector_fusion_adapter_node + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.launch.xml") +install(DIRECTORY pipelines + DESTINATION share/${PROJECT_NAME}) -# need to install plugin in kwiver area to simplify loading. -set_target_properties( ros_dynamic_config - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${KWIVER_MODULE_DIR} ) +ament_package() diff --git a/src/kitware-ros-pkg/sprokit_adapters/config/debug_rosconsole.conf b/src/kitware-ros-pkg/sprokit_adapters/config/debug_rosconsole.conf deleted file mode 100644 index d4142944..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/config/debug_rosconsole.conf +++ /dev/null @@ -1,7 +0,0 @@ -# -# You can define your own by e.g. copying this file and setting -# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file -# -log4j.logger.ros=INFO -log4j.logger.ros.sprokit_adapters=DEBUG -log4j.logger.ros.roscpp.superdebug=WARN diff --git a/src/kitware-ros-pkg/sprokit_adapters/launch/publish_sync_msgs.launch b/src/kitware-ros-pkg/sprokit_adapters/launch/publish_sync_msgs.launch deleted file mode 100644 index d7e795d7..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/launch/publish_sync_msgs.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch b/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch deleted file mode 100644 index ef892bba..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch +++ /dev/null @@ -1,104 +0,0 @@ - - - - - - - - - - - \ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch.xml b/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch.xml new file mode 100644 index 00000000..da3f13f1 --- /dev/null +++ b/src/kitware-ros-pkg/sprokit_adapters/launch/sprokit_detector_fusion_adapter.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/configurations.xml b/src/kitware-ros-pkg/sprokit_adapters/nbproject/configurations.xml deleted file mode 100644 index 04f4683e..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/nbproject/configurations.xml +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - kw_detector_adapter.cpp - kw_image_adapter.cpp - ros_detector_scaling.cxx - ros_dynamic_config.cxx - - - - CMakeLists.txt - Makefile - - - ^(nbproject)$ - - . - - Makefile - - - - default - false - false - - - - - - . - ${MAKE} -f Makefile - ${MAKE} -f Makefile clean - - - - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/Default.properties b/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/Default.properties deleted file mode 100644 index e69de29b..00000000 diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/configurations.xml b/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/configurations.xml deleted file mode 100644 index 3a54735c..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/configurations.xml +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - CMakeCCompilerId.c - - - CMakeCXXCompilerId.cpp - - - - - feature_tests.c - feature_tests.cxx - - - - - - - - - - - - - - - - - kw_detector_adapter.cpp - kw_image_adapter.cpp - ros_detector_scaling.cxx - ros_detector_scaling.h - ros_dynamic_config.cxx - ros_dynamic_config.h - - - - - - Makefile - - - - localhost - 2 - - - - . - ${AUTO_FOLDER} - - ${AUTO_FOLDER} - - ${MAKE} ${ITEM_NAME}.o - ${AUTO_COMPILE} - - ${AUTO_COMPILE} - - - - - - - - - - - - - - - gdb - - - - "${OUTPUT_PATH}" - - "${OUTPUT_PATH}" - . - false - 0 - 0 - - - - - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/launcher.properties b/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/launcher.properties deleted file mode 100644 index 6cc2127d..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/launcher.properties +++ /dev/null @@ -1,40 +0,0 @@ -# Launchers File syntax: -# -# [Must-have property line] -# launcher1.runCommand= -# [Optional extra properties] -# launcher1.displayName= -# launcher1.buildCommand= -# launcher1.runDir= -# launcher1.symbolFiles= -# launcher1.env.= -# (If this value is quoted with ` it is handled as a native command which execution result will become the value) -# [Common launcher properties] -# common.runDir= -# (This value is overwritten by a launcher specific runDir value if the latter exists) -# common.env.= -# (Environment variables from common launcher are merged with launcher specific variables) -# common.symbolFiles= -# (This value is overwritten by a launcher specific symbolFiles value if the latter exists) -# -# In runDir, symbolFiles and env fields you can use these macroses: -# ${PROJECT_DIR} - project directory absolute path -# ${OUTPUT_PATH} - linker output path (relative to project directory path) -# ${OUTPUT_BASENAME}- linker output filename -# ${TESTDIR} - test files directory (relative to project directory path) -# ${OBJECTDIR} - object files directory (relative to project directory path) -# ${CND_DISTDIR} - distribution directory (relative to project directory path) -# ${CND_BUILDDIR} - build directory (relative to project directory path) -# ${CND_PLATFORM} - platform name -# ${CND_CONF} - configuration name -# ${CND_DLIB_EXT} - dynamic library extension -# -# All the project launchers must be listed in the file! -# -# launcher1.runCommand=... -# launcher2.runCommand=... -# ... -# common.runDir=... -# common.env.KEY=VALUE - -# launcher1.runCommand= \ No newline at end of file diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/private.xml b/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/private.xml deleted file mode 100644 index 4f3fdc62..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/nbproject/private/private.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - true - - - 0 - 0 - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/nbproject/project.xml b/src/kitware-ros-pkg/sprokit_adapters/nbproject/project.xml deleted file mode 100644 index cbb758db..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/nbproject/project.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - org.netbeans.modules.cnd.makeproject - - - sprokit_adapters - c - cpp,cxx - h - UTF-8 - - - . - - - - Default - 0 - - - - false - - - - diff --git a/src/kitware-ros-pkg/sprokit_adapters/package.xml b/src/kitware-ros-pkg/sprokit_adapters/package.xml index 169e5ace..e052c52a 100644 --- a/src/kitware-ros-pkg/sprokit_adapters/package.xml +++ b/src/kitware-ros-pkg/sprokit_adapters/package.xml @@ -1,61 +1,24 @@ - + + sprokit_adapters - 0.0.0 - The sprokit_adapters package + 1.0.0 + ROS2 adapter feeding synchronized imagery into VIAME/sprokit detector pipelines - - - Adam Romlein - - - - - Apache 2.0 + ament_cmake - - - - - - - - - - + rclcpp + std_msgs + sensor_msgs + custom_msgs + cv_bridge + roskv + libopencv-dev - - - - - - - - - - - - - catkin - roscpp - std_msgs - image_transport - custom_msgs - roskv - eigen_conversions - image_transport - roscpp - roskv - std_msgs - custom_msgs - - - - - + ament_cmake diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp b/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp index 58ad5e41..3ae7886a 100644 --- a/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp +++ b/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp @@ -1,16 +1,15 @@ -#include "ros/ros.h" -#include "sensor_msgs/Image.h" -#include "std_msgs/String.h" -#include +#include +#include +#include #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -22,28 +21,26 @@ #include #include +#include #include #include #include #include #include #include -#include +#include -// GLOBAL pointer to embedded pipeline -kwiver::embedded_pipeline* g_pep; +static rclcpp::Logger LOG = rclcpp::get_logger("sprokit_detector_fusion_adapter"); +#define ROS_INFO(...) RCLCPP_INFO(LOG, __VA_ARGS__) +#define ROS_WARN(...) RCLCPP_WARN(LOG, __VA_ARGS__) +#define ROS_ERROR(...) RCLCPP_ERROR(LOG, __VA_ARGS__) +#define ROS_INFO_STREAM(args) RCLCPP_INFO_STREAM(LOG, args) +#define ROS_ERROR_STREAM(args) RCLCPP_ERROR_STREAM(LOG, args) -// =============================================================== -struct InputMetadata -{ - // input message header - std_msgs::Header m_header; +// GLOBAL pointer to embedded pipeline +kwiver::embedded_pipeline* g_pep; - // Input image size - int m_height; - int m_width; -}; // check if file exists for sync node inline bool file_exists (const std::string& name) { @@ -101,7 +98,7 @@ class random_string_generator random_string( size_t length ) { std::string rand_str( length, 0 ); - for (int i = 0; i < length; ++i) { + for (size_t i = 0; i < length; ++i) { rand_str[i] = randchar(); } return rand_str; @@ -119,10 +116,10 @@ class random_string_generator class AdapterCallback { public: - AdapterCallback( ros::NodeHandle &nh, + AdapterCallback( rclcpp::Node::SharedPtr nh, kwiver::embedded_pipeline* pipeline_ptr, std::string topic, int sync_q_size, int rgb_port_ind, - int ir_port_ind, int uv_port_ind) + int ir_port_ind, int uv_port_ind) : m_pep( pipeline_ptr ), m_topic( topic ), m_rgb_port_ind( rgb_port_ind ), @@ -131,12 +128,15 @@ class AdapterCallback { // Set up callback for input topic depending on the image message type ROS_INFO_STREAM( "Subscribing to SynchronizedImages topic: " << topic ); - m_synchronized_images_sub = nh.subscribe( topic, sync_q_size, - &AdapterCallback::synchronizedImagesCallback, this ); + m_synchronized_images_sub = nh->create_subscription( + topic, sync_q_size, + [this](const custom_msgs::msg::SynchronizedImages::ConstSharedPtr msg) { + synchronizedImagesCallback(msg); + }); } // ROS callback for SynchronizedImages - void synchronizedImagesCallback( const custom_msgs::SynchronizedImagesConstPtr& msg ) + void synchronizedImagesCallback( const custom_msgs::msg::SynchronizedImages::ConstSharedPtr& msg ) { // Create dataset for input auto ds = kwiver::adapter::adapter_data_set::create(); @@ -150,17 +150,17 @@ class AdapterCallback cv_image = cv_bridge::toCvCopy( msg->image_rgb, "rgb8" )->image; } else if ( msg->image_rgb.data.empty() ) { if ( file_exists(file_name) ) { - cv_image = cv::imread(file_name, cv::IMREAD_COLOR); - if ( cv_image.empty() ) { - ROS_ERROR_STREAM("Could not read image from disk: " << file_name.c_str()); + cv_image = cv::imread(file_name, cv::IMREAD_COLOR); + if ( cv_image.empty() ) { + ROS_ERROR_STREAM("Could not read image from disk: " << file_name.c_str()); return; - } else { - ROS_INFO("Successfully read rgb image from disk."); - } - } else { - ROS_ERROR_STREAM("RGB file name does not exist " << file_name.c_str()); + } else { + ROS_INFO("Successfully read rgb image from disk."); + } + } else { + ROS_ERROR_STREAM("RGB file name does not exist " << file_name.c_str()); return ; - } + } } else { ROS_ERROR("RGB image is null-ish "); return ; @@ -201,17 +201,17 @@ class AdapterCallback cv_image2 = cv_bridge::toCvCopy( msg->image_ir, "mono16" )->image; } else if ( msg->image_ir.data.empty() ) { if ( file_exists(file_name) ) { - cv_image2 = cv::imread(file_name, cv::IMREAD_ANYDEPTH); - if ( cv_image2.empty() ) { - ROS_ERROR_STREAM("Could not read image from disk: " << file_name.c_str()); + cv_image2 = cv::imread(file_name, cv::IMREAD_ANYDEPTH); + if ( cv_image2.empty() ) { + ROS_ERROR_STREAM("Could not read image from disk: " << file_name.c_str()); return; - } else { - ROS_INFO("Successfully read IR image from disk."); - } - } else { - ROS_ERROR_STREAM("IR file name does not exist " << file_name.c_str()); + } else { + ROS_INFO("Successfully read IR image from disk."); + } + } else { + ROS_ERROR_STREAM("IR file name does not exist " << file_name.c_str()); return ; - } + } } else { ROS_ERROR("IR image is null-ish "); return ; @@ -255,7 +255,7 @@ class AdapterCallback private: kwiver::embedded_pipeline* m_pep; - ros::Subscriber m_synchronized_images_sub; + rclcpp::Subscription::SharedPtr m_synchronized_images_sub; std::string m_frame_id; std::string m_topic; int m_rgb_port_ind; @@ -268,8 +268,9 @@ class AdapterCallback void sigint_handler( int sig ) { + (void) sig; g_pep->send_end_of_input(); - ros::shutdown(); + rclcpp::shutdown(); } @@ -294,13 +295,11 @@ main( int argc, char** argv ) random_string_generator string_generator; - std::ofstream debug_out("/root/kamera_ws/image_id.txt"); - ros::init( argc, argv, "sprokit_detector_fusion_adapter" ); - ros::NodeHandle nh_pub; - ros::NodeHandle nh_priv("~"); + rclcpp::init( argc, argv ); + auto node = std::make_shared( "sprokit_detector_fusion_adapter" ); - std::string redis_uri; - if ( ! nh_priv.getParam("redis_uri", redis_uri) ) { + std::string redis_uri = node->declare_parameter("redis_uri", std::string("")); + if ( redis_uri.empty() ) { ROS_ERROR( "'redis_uri' not found in parameters." ); return -1; } @@ -309,34 +308,35 @@ main( int argc, char** argv ) std::shared_ptr envoy = std::make_shared(envoy_opts); // Find pipeline file name from parameters (see README) - // get hostname from param namespace - std::string ns = nh_priv.getNamespace(); + // get hostname from node namespace + std::string ns = node->get_namespace(); + if (ns == "/") { + ns = ""; + } std::ostringstream oss; std::ostringstream healthss; healthss << ns << "/health"; oss << "/sys" << ns << "/pipefile"; std::string health_param = healthss.str(); std::string redis_pipefile = oss.str(); - // Try and get pipefile from redis, if fails, get from rosparam + // Try and get pipefile from redis, if fails, get from node param std::string pipe_file; try { ROS_INFO("Trying to get Redis pipefile from: %s.", redis_pipefile.c_str()); pipe_file = envoy->get(redis_pipefile); - } catch( std::invalid_argument e ) { - ROS_WARN("No Redis failed for pipefile, falling back to rosparam."); - if ( ! nh_priv.getParam("pipe_file", pipe_file) ) { + } catch( std::invalid_argument &e ) { + ROS_WARN("No Redis failed for pipefile, falling back to node param."); + pipe_file = node->declare_parameter("pipe_file", std::string("")); + if ( pipe_file.empty() ) { ROS_ERROR( "'pipe_file' not found in parameter path <<." ); return -1; } else { - ROS_INFO( "Setting Redis pipefile based off ros param." ); + ROS_INFO( "Setting Redis pipefile based off node param." ); envoy->put(redis_pipefile, pipe_file); } } - std::string pipeline_dir; - if ( ! nh_priv.param("pipeline_dir", pipeline_dir, "")) { - ROS_WARN( "'pipeline_dir' file not found in parameters" ); - } + std::string pipeline_dir = node->declare_parameter("pipeline_dir", std::string("")); // Open pipeline description std::ifstream pipe_str; pipe_str.open( pipe_file, std::ifstream::in ); @@ -353,64 +353,24 @@ main( int argc, char** argv ) if (pipeline_dir.empty()) { ROS_WARN( "'pipeline_dir' file not found in parameters. Defaulting to '`dirname pipe_file`" ); - boost::filesystem::path pipefile_path(pipe_file); - pipefile_path.remove_filename(); - pipeline_dir = pipefile_path.string(); + std::filesystem::path pipefile_path(pipe_file); + pipeline_dir = pipefile_path.parent_path().string(); } ROS_INFO("pipeline_dir=%s", pipeline_dir.c_str()); - int rgb_port_ind; - if ( ! nh_priv.getParam("rgb_port_ind", rgb_port_ind) ) - { - // Entry not found, use default name - ROS_WARN( "'rgb_port_ind' not found, RGB image will not be sent to pipeline." ); - rgb_port_ind = 0; - } - - int ir_port_ind; - if ( ! nh_priv.getParam("ir_port_ind", ir_port_ind) ) - { - // Entry not found, use default name - ROS_WARN( "'ir_port_ind' not found, RGB image will not be sent to pipeline." ); - ir_port_ind = 0; - } - - int uv_port_ind; - if ( ! nh_priv.getParam("uv_port_ind", uv_port_ind) ) - { - // Entry not found, use default name - ROS_WARN( "'uv_port_ind' not found, RGB image will not be sent to pipeline." ); - uv_port_ind = 0; - } + int rgb_port_ind = node->declare_parameter("rgb_port_ind", 0); + int ir_port_ind = node->declare_parameter("ir_port_ind", 0); + int uv_port_ind = node->declare_parameter("uv_port_ind", 0); // Get detector ID string, which identifies the detector used (see README). - std::string detector_id_string; - if( ! nh_priv.getParam( "detector_id_string", detector_id_string ) ) - { - // Entry not found, use default name - ROS_WARN( "'detector_id_string' not defined, defaulting to 'unspecified'." ); - detector_id_string = "unspecified"; - } - else - { - ROS_INFO( "'detector_id_string' set to '%s'", detector_id_string.c_str() ); - } + std::string detector_id_string = node->declare_parameter("detector_id_string", std::string("unspecified")); + ROS_INFO( "'detector_id_string' set to '%s'", detector_id_string.c_str() ); // OpenCV Threading Value - int ocv_num_threads; - if( ! nh_priv.getParam( "ocv_num_threads", ocv_num_threads ) ) - { - ROS_WARN( "'ocv_num_threads' not defined, defaulting to -1 (serial execution)." ); - ocv_num_threads = -1; - } + int ocv_num_threads = node->declare_parameter("ocv_num_threads", -1); // ROS sync queue value - int sync_q_size; - if( ! nh_priv.getParam( "sync_q_size", sync_q_size ) ) - { - ROS_WARN( "'sync_q_size' not defined, defaulting to 5,000." ); - sync_q_size = 5000; - } + int sync_q_size = node->declare_parameter("sync_q_size", 5000); // 0 means "OpenCV will disable threading optimizations and run all its functions sequentially" // <0 means default allocation. @@ -430,34 +390,27 @@ main( int argc, char** argv ) // There are an, as of yet, unknown number of image topics that are to be // multiplexed through the detector pipeline. So, we incrementally seek - // parameter "synchronized_images_in_topic#" until we find it not populated or populated - // with "unused". + // parameter "synchronized_images_in#" until we find it not populated or + // populated with "unused". std::vector input_cbs; int i = 1; // The annotated image base will be concatenated with the integer image number. std::string topic_name; - // Incrementally seek parameter "synchronized_images_in_topic#" until we find - // it not populated or populated with "unused". while( true ) { std::string topic_param; topic_param = std::string( "synchronized_images_in" ) + std::to_string( i ); - if ( nh_priv.getParam( topic_param, topic_name ) ) + topic_name = node->declare_parameter( topic_param, std::string("unused") ); + if( topic_name != "unused" ) { - if( topic_name == "unused" ) - { - break; - } - // Set callbacks - // Create instance of image callback for current image topic and add to // vector of callback instances. ROS_INFO( "Found SynchronizedImages topic %s", topic_name.c_str() ); - input_cbs.push_back( new AdapterCallback( nh_pub, &pipeline, topic_name, - sync_q_size, rgb_port_ind, - ir_port_ind, uv_port_ind ) ); + input_cbs.push_back( new AdapterCallback( node, &pipeline, topic_name, + sync_q_size, rgb_port_ind, + ir_port_ind, uv_port_ind ) ); } else if( i == 1) { @@ -471,18 +424,17 @@ main( int argc, char** argv ) ++i; } - ros::Publisher detection_pub; - detection_pub = nh_priv.advertise< custom_msgs::ImageSpaceDetectionList > ( "detections_out", 20 ); + auto detection_pub = node->create_publisher( + "~/detections_out", 20 ); - // Start ROS spinner - ros::AsyncSpinner spinner( 1 ); - spinner.start(); + // Start ROS spinner in the background; the main thread services the pipeline + std::thread spin_thread([node]() { rclcpp::spin(node); }); json::json health; int frame = 0; - while ( ros::ok() ) + while ( rclcpp::ok() ) { ROS_INFO( "OpenCV thread number: %d", cv::getNumThreads() ); auto ods = pipeline.receive(); // blocks until data ready @@ -495,7 +447,8 @@ main( int argc, char** argv ) ROS_INFO( "End of data found by node. Waiting for scheduler to complete." ); pipeline.wait(); // wait for pipeline scheduler to complete ROS_INFO( "Waiting for scheduler to ROS shutdown." ); - ros::shutdown(); + rclcpp::shutdown(); + spin_thread.join(); return 0; } @@ -525,7 +478,7 @@ main( int argc, char** argv ) ROS_INFO( "Received 'file_name' %s from pipeline", src_img_fname.c_str() ); - custom_msgs::ImageSpaceDetectionList det_list; + custom_msgs::msg::ImageSpaceDetectionList det_list; // Get values from the metadata "header" item. det_list.header.frame_id = src_img_fname; @@ -539,7 +492,7 @@ main( int argc, char** argv ) // loop over det_v - adding to list for ( auto det : det_v ) { - custom_msgs::ImageSpaceDetection one_det; + custom_msgs::msg::ImageSpaceDetection one_det; one_det.header = det_list.header; one_det.camera_of_origin = src_img_fname; one_det.uid = string_generator.random_string( 20 ); @@ -570,9 +523,9 @@ main( int argc, char** argv ) } } // end loop - detection_pub.publish( det_list ); + detection_pub->publish( det_list ); - double time = ros::Time::now().toSec(); + double time = node->now().seconds(); auto int_dets = det_v.size(); // Convert things to string for json @@ -589,4 +542,6 @@ main( int argc, char** argv ) } // end big while + spin_thread.join(); + return 0; } // main diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.cxx b/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.cxx deleted file mode 100644 index 10398e0f..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.cxx +++ /dev/null @@ -1,138 +0,0 @@ - -#include "ros_detector_scaling.h" - -#include -#include - -// ------------------------------------------------------------------ -ros_detector_scaling:: -ros_detector_scaling() - : m_scaleFactor(1.0) -{ - // Allocate config block - m_config = kwiver::vital::config_block::empty_config(); -} - - -// ------------------------------------------------------------------ -ros_detector_scaling:: -~ros_detector_scaling() -{} - - -// ------------------------------------------------------------------ -kwiver::vital::config_block_sptr -ros_detector_scaling:: -get_configuration() const -{ - // Get base config from base class - kwiver::vital::config_block_sptr config = kwiver::vital::algorithm::get_configuration(); - - // Could configure the topic to listen on - config->set_value( "topic", "scale_factor", - "ROS Topic name to subscribe. This topic will supply the float64 scaling value " - "from (0 - 1)." ); - - return config; -} - - -// ------------------------------------------------------------------ -void -ros_detector_scaling:: -set_configuration( kwiver::vital::config_block_sptr config_in ) -{ - // Starting with our generated config_block to ensure that assumed values are present - // An alternative is to check for key presence before performing a get_value() call. - kwiver::vital::config_block_sptr config = this->get_configuration(); - - config->merge_config( config_in ); - - this->m_topic = config->get_value( "topic", "scale_factor" ); - - // Need to delay initializing the ROS interface until we have a good - // config. Can not be done in CTOR because objects are created for - // introspection without valid ROS environment. - ros::init( ros::M_string(), "ros_detector_scaling" ); - m_node = std::make_shared("~"); - - // Set up callback for input topic - m_sub = m_node->subscribe( this->m_topic, 1, &ros_detector_scaling::callbackEvent, this ); - - // Start ROS spinner - ros::AsyncSpinner spinner( 1 ); - spinner.start(); -} - - -// ------------------------------------------------------------------ -bool -ros_detector_scaling:: -check_configuration( kwiver::vital::config_block_sptr config ) const -{ - return true; -} - - -// ------------------------------------------------------------------ -kwiver::vital::config_block_sptr -ros_detector_scaling:: -get_dynamic_configuration() -{ - m_config->set_value("scale_factor", this->m_scaleFactor ); - - return m_config; -} - - -// ------------------------------------------------------------------ -// accepts a message of expected type. -void -ros_detector_scaling:: -callbackEvent(const std_msgs::Float64& msg ) -{ - double factor = msg.data; - - // validate stane factor - if (factor < 0) - { - factor = 0; - ROS_WARN( "Scaling value less than 0. Set to 0." ); - } - else if (factor > 1) - { - factor = 1.0 ; - ROS_WARN( "Scaling factor greater than 1. Set to 1." ); - } - - // save value in local storage; - this->m_scaleFactor = factor; -} - - -// ================================================================== -// Register this as a plugin -extern "C" -ROS_DETECTOR_SCALING_EXPORT -void -register_factories( kwiver::vital::plugin_loader& vpm ) -{ - static auto const module_name = std::string( "kamera.ros.ros_detector_scaling" ); - if (vpm.is_module_loaded( module_name ) ) - { - return; - } - - // add factory implementation-name type-to-create - auto fact = vpm.ADD_ALGORITHM( "ros_detector_scaling", ros_detector_scaling ); - fact->add_attribute( kwiver::vital::plugin_factory::PLUGIN_DESCRIPTION, - "Proivides dynamic scale factor.\n\n" - "Listens on topic \"scale_factor\" for ros::Float64 message. " - "Supplies value as \"scale_factor\" in the config block." ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_MODULE_NAME, module_name ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_VERSION, "1.0" ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_ORGANIZATION, "Kitware Inc." ) - ; - - vpm.mark_module_as_loaded( module_name ); -} diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.h b/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.h deleted file mode 100644 index 5e72b4ee..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/src/ros_detector_scaling.h +++ /dev/null @@ -1,50 +0,0 @@ - -#include "ros_detector_scaling_export.h" - -#include "ros/ros.h" -#include "std_msgs/Float64.h" - -#include - -// ------------------------------------------------------------------ -/** Algorithm instance to support detector scaling. - * - * This algorithm registers as a ROS subscriber, listening to a topic - * that will supply a double value to be supplied as a scaling factor. - * - * Typical config - * :scaling:type ros_detector_scaling - * :scaling:ros_detector_scaling:topic spinner1 - * - * Note that this class is a special case of ros_dynamic_config class. - */ -class ROS_DETECTOR_SCALING_EXPORT ros_detector_scaling -: public kwiver::vital::algo::dynamic_configuration -{ -public: - ros_detector_scaling(); - virtual ~ros_detector_scaling(); - - virtual kwiver::vital::config_block_sptr get_configuration() const; - virtual void set_configuration( kwiver::vital::config_block_sptr config ); - virtual bool check_configuration( kwiver::vital::config_block_sptr config ) const; - - /// Return dynamic configuration values - /** - * This method returns dynamic configuration values. a valid config - * block is returned even if there are not values being returned. - */ - virtual kwiver::vital::config_block_sptr get_dynamic_configuration(); - -private: - void callbackEvent(const std_msgs::Float64& msg ); - - double m_scaleFactor; // scale factor (0-1) - ros::Subscriber m_sub; // handle to subscriber - - // Persistent config block that is used to hold the scaling value. - kwiver::vital::config_block_sptr m_config; - std::string m_topic; - - std::shared_ptr m_node; -}; diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.cxx b/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.cxx deleted file mode 100644 index 30e26a5b..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.cxx +++ /dev/null @@ -1,142 +0,0 @@ - -#include "ros_dynamic_config.h" - -#include -#include -#include - -// ------------------------------------------------------------------ -ros_dynamic_config:: -ros_dynamic_config() -{ - // Allocate config block - m_config = kwiver::vital::config_block::empty_config(); -} - - -// ------------------------------------------------------------------ -ros_dynamic_config:: -~ros_dynamic_config() -{} - - -// ------------------------------------------------------------------ -kwiver::vital::config_block_sptr -ros_dynamic_config:: -get_configuration() const -{ - // Get base config from base class - kwiver::vital::config_block_sptr config = kwiver::vital::algorithm::get_configuration(); - - // Could configure the topic to listen on - config->set_value( "topic", "dyn_config", - "ROS Topic name to subscribe. This topic will supply " - "a diagnostic_msgs::DiagnosticStatus message that contains the key/value " - "pairs that are copied to the config block." ); - - return config; -} - - -// ------------------------------------------------------------------ -void -ros_dynamic_config:: -set_configuration( kwiver::vital::config_block_sptr config_in ) -{ - // Starting with our generated config_block to ensure that assumed values are present - // An alternative is to check for key presence before performing a get_value() call. - kwiver::vital::config_block_sptr config = this->get_configuration(); - - config->merge_config( config_in ); - - m_topic = config->get_value( "topic", "dyn_config" ); - - // Need to delay initializing the ROS interface until we have a good - // config. Can not be done in CTOR because objects are created for - // introspection without valid ROS environment. - ros::init( ros::M_string(), "ros_dynamic_config" ); - m_node = std::make_shared("~"); - - // Set up callback for input topic - m_sub = m_node->subscribe( this->m_topic, 1, &ros_dynamic_config::callbackEvent, this ); - - // Start ROS spinner - ros::AsyncSpinner spinner( 1 ); - spinner.start(); -} - - -// ------------------------------------------------------------------ -bool -ros_dynamic_config:: -check_configuration( kwiver::vital::config_block_sptr config ) const -{ - return true; -} - - -// ------------------------------------------------------------------ -kwiver::vital::config_block_sptr -ros_dynamic_config:: -get_dynamic_configuration() -{ - std::lock_guard lock(m_config_lock); - - return m_config; -} - - -// ------------------------------------------------------------------ -// accepts a message of expected type. -void -ros_dynamic_config:: -callbackEvent(const diagnostic_msgs::DiagnosticStatus& msg ) -{ - // Start with a new config block so we can add entries with out - // colliding with the get_dynamic_configuration() method since we - // are running in separate threads. - auto config = kwiver::vital::config_block::empty_config(); - - for ( auto kv : msg.values ) - { - LOG_DEBUG( logger(), "Adding config entry to dynamic set - " - << kv.key << " = " << kv.value ); - - config->set_value( kv.key, kv.value ); - } - - // Do an atomic store so we do not get into trouble with the - // asynchronous client - std::lock_guard lock(m_config_lock); - - this->m_config = config; -} - - -// ================================================================== -// Register this as a plugin -extern "C" -ROS_DYNAMIC_CONFIG_EXPORT -void -register_factories( kwiver::vital::plugin_loader& vpm ) -{ - static auto const module_name = std::string( "kamera.ros.ros_dynamic_config" ); - if (vpm.is_module_loaded( module_name ) ) - { - return; - } - - // add factory implementation-name type-to-create - auto fact = vpm.ADD_ALGORITHM( "ros_dynamic_config", ros_dynamic_config ); - fact->add_attribute( kwiver::vital::plugin_factory::PLUGIN_DESCRIPTION, - "Proivides dynamic configuration values.\n\n" - "Listens on topic \"dyn_config\" by default for ROS " - "diagnostic_msgs::DiagnosticStatus message. " - "Puts all key/value pairs in the config block." ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_MODULE_NAME, module_name ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_VERSION, "1.0" ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_ORGANIZATION, "Kitware Inc." ) - ; - - vpm.mark_module_as_loaded( module_name ); -} diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.h b/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.h deleted file mode 100644 index 8063b307..00000000 --- a/src/kitware-ros-pkg/sprokit_adapters/src/ros_dynamic_config.h +++ /dev/null @@ -1,62 +0,0 @@ - -#include "ros_dynamic_config_export.h" - -#include "ros/ros.h" -#include "diagnostic_msgs/DiagnosticStatus.h" - -#include -#include -#include - -// ------------------------------------------------------------------ -/** Algorithm instance to dynamic configuration in a ROS environment - * - * This algorithm registers as a ROS subscriber, listening to a topic - * that will supply a diagnostic_msgs::DiagnosticStatus message. This - * message is not a great fit for this application, but it does have a - * key/value vector. - * - * The key/value vector is transferred to the config block and made - * available in the get_dynamic_configuration() call. - * - * Typical config - * :dynamic_config:type ros_dynamic_config - * :dynamic_config:ros_dynamic_config:topic display_config - */ -class ROS_DYNAMIC_CONFIG_EXPORT ros_dynamic_config -: public kwiver::vital::algo::dynamic_configuration -{ -public: - ros_dynamic_config(); - virtual ~ros_dynamic_config(); - - virtual kwiver::vital::config_block_sptr get_configuration() const; - virtual void set_configuration( kwiver::vital::config_block_sptr config ); - virtual bool check_configuration( kwiver::vital::config_block_sptr config ) const; - - /// Return dynamic configuration values - /** - * This method returns dynamic configuration values. a valid config - * block is returned even if there are not values being returned. - */ - virtual kwiver::vital::config_block_sptr get_dynamic_configuration(); - -private: - /// Callback from subscriber. - /// This may be misusing the DiagnosticStatus message, but it has a key/value array. - void callbackEvent(const diagnostic_msgs::DiagnosticStatus& msg ); - - ros::Subscriber m_sub; // handle to subscriber - - // config block that is used to hold the scaling value. - kwiver::vital::config_block_sptr m_config; - - // Subscribe to this topic - std::string m_topic; - - std::shared_ptr m_node; - - // Lock for pointer to config block. - // Tried std::atomic, but did not work well with smart pointers. - std::mutex m_config_lock; -}; From 8cb69966903c6c07b5e661a255995c502b3991d8 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:39:40 -0400 Subject: [PATCH 09/20] Convert backend metapackage to ament; postproc jobs need no port - backend becomes a plain ament metapackage depending on the ported ROS2 packages (GUI/phase_one entries removed - not part of the nayak/taiga supervisor set) - Dropped the unfinished diagnostics.py stub (copy of ins driver boilerplate that was never completed or launched) - The postproc supervisor group (flight_summary, homography, detections) runs pure bash + kamera.postflight python with no ROS dependency, so those entry points are unchanged --- src/backend/CMakeLists.txt | 18 ++--------- src/backend/package.xml | 28 +++++++---------- src/backend/scripts/__init__.py | 0 src/backend/scripts/diagnostics.py | 48 ------------------------------ src/backend/setup.py | 9 ------ 5 files changed, 13 insertions(+), 90 deletions(-) delete mode 100644 src/backend/scripts/__init__.py delete mode 100755 src/backend/scripts/diagnostics.py delete mode 100644 src/backend/setup.py diff --git a/src/backend/CMakeLists.txt b/src/backend/CMakeLists.txt index 97442e68..152ed913 100644 --- a/src/backend/CMakeLists.txt +++ b/src/backend/CMakeLists.txt @@ -1,18 +1,6 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(backend) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) -catkin_python_setup() -catkin_package() - -install(PROGRAMS - scripts/diagnostics.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -#if (CATKIN_ENABLE_TESTING) -# find_package(roslint) -# roslint_python() -# roslint_add_test() -#endif() +ament_package() diff --git a/src/backend/package.xml b/src/backend/package.xml index 18b5fbda..c1d5cc98 100644 --- a/src/backend/package.xml +++ b/src/backend/package.xml @@ -1,7 +1,8 @@ - + + backend - 0.5.0 + 1.0.0 High level package to install all the backend dependencies @@ -9,30 +10,21 @@ Adam Romlein Apache 2.0 + ament_cmake - - catkin - - rospy - kamcore custom_msgs - kamerahealth roskv - phase_one - prosilica_camera - kw_genicam_driver + kamcore ins_driver mcc_daq + ser_daq nexus - sysinfo view_server - wxpython_gui - + cam_utils + prosilica_camera + kw_genicam_driver - - - - + ament_cmake diff --git a/src/backend/scripts/__init__.py b/src/backend/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/backend/scripts/diagnostics.py b/src/backend/scripts/diagnostics.py deleted file mode 100755 index 6549f1dd..00000000 --- a/src/backend/scripts/diagnostics.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import socket -import rospy -import sys - - - -def netcat(hostname, port, content): - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - s.connect((hostname, port)) - # s.sendall(content) - s.shutdown(socket.SHUT_WR) - while 1: - data = s.recv(1024) - if data == "": - break - print("Received: {}".format(repr(data))) - print("Connection closed.") - s.close() - - - - - -if __name__ == '__main__': - rospy.init_node('ins_socket_driver') - try: - host = rospy.get_param('~ip', '0.0.0.0') - port = rospy.get_param('~port', 10110) - buffer_size = rospy.get_param('~buffer_size', 4096) - timeout = rospy.get_param('~timeout_sec', 2) - spoof = rospy.get_param('~spoof') - replay_path = rospy.get_param('~replay') - except KeyError as e: - rospy.logerr("Parameter %s not found" % e) - sys.exit(1) - - - client = AvxClient() - if spoof > 0 : - client.spoof(spoof) - elif replay_path: - client.replay(replay_path) - else: - client.run(host, port, buffer_size, timeout) - diff --git a/src/backend/setup.py b/src/backend/setup.py deleted file mode 100644 index 07876ad8..00000000 --- a/src/backend/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['diag'], - package_dir={'': 'src'}) - -setup(**d) From 82a611c09e20c57e69008bd0c54ccbb4e9c6c8a1 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 19:45:41 -0400 Subject: [PATCH 10/20] Switch nayak/taiga runtime plumbing from ROS1 to ROS2 - compose: ROS_MASTER_URI env replaced with ROS_DOMAIN_ID everywhere; core image ROS_DISTRO noetic -> humble - tmux env.sh (nayak/taiga): drop ROS master/hostname exports; export a shared ROS_DOMAIN_ID for DDS discovery instead - The roscore supervisor program/compose service becomes core_init: with no master in ROS2 its only remaining job is seeding Redis with the static system config (ros2 run kamcore seed_redis_config) - Entry scripts: roslaunch --wait -> ros2 launch *.launch.xml with norespawn mapped to the launch respawn arg; catkin build debug rebuilds -> colcon build --packages-select; dropped the 'REQUIRED node has died' log-scrape hack (roslaunch-specific, ros2 launch propagates exit properly) - viame.sh: detection csv/image-list dirs passed via environment (pipelines read env; the old launch env-vars are gone) - rosnode_list.sh diagnostic -> ros2 node list; dev aliases updated Not touched (out of nayak/taiga supervisor scope): gui.sh, cam_phaseone.sh, postproc.sh (cas), uas tmux configs, wxpython_gui. --- compose/cam_ir.yml | 4 +- compose/cam_param_monitor.yml | 2 +- compose/cam_rgb.yml | 4 +- compose/cam_uv.yml | 4 +- compose/{roscore.yml => core_init.yml} | 10 ++--- compose/daq.yml | 2 +- compose/detector.yml | 2 +- compose/fps_monitor.yml | 2 +- compose/gui.yml | 2 +- compose/image_manager.yml | 2 +- compose/imageview.yml | 2 +- compose/ins.yml | 2 +- compose/nodelist.yml | 2 +- compose/shapefile_monitor.yml | 2 +- compose/spoof_events.yml | 2 +- compose/sync_msg_publisher.yml | 2 +- src/run_scripts/aliases.sh | 10 ++--- src/run_scripts/entry/cam_ir.sh | 21 +++------- src/run_scripts/entry/cam_param_monitor.sh | 2 +- src/run_scripts/entry/cam_prosilica.sh | 19 +++------ src/run_scripts/entry/core_init.sh | 26 ++++++++++++ src/run_scripts/entry/daq.sh | 7 ++-- src/run_scripts/entry/fps_monitor.sh | 2 +- src/run_scripts/entry/imageview.sh | 4 +- src/run_scripts/entry/ins.sh | 9 ++-- src/run_scripts/entry/master.sh | 49 ---------------------- src/run_scripts/entry/rosnode_list.sh | 7 ++-- src/run_scripts/entry/shapefile_monitor.sh | 2 +- src/run_scripts/entry/spoof_events.sh | 2 +- src/run_scripts/entry/viame.sh | 31 ++++---------- tmux/nayak/env.sh | 7 ++-- tmux/nayak/leader/supervisor.conf | 4 +- tmux/taiga/env.sh | 6 +-- tmux/taiga/leader/supervisor.conf | 4 +- 34 files changed, 98 insertions(+), 160 deletions(-) rename compose/{roscore.yml => core_init.yml} (72%) create mode 100755 src/run_scripts/entry/core_init.sh delete mode 100755 src/run_scripts/entry/master.sh diff --git a/compose/cam_ir.yml b/compose/cam_ir.yml index 6620c3e8..a904f07c 100644 --- a/compose/cam_ir.yml +++ b/compose/cam_ir.yml @@ -7,8 +7,8 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" - ROS_DISTRO: "noetic" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" + ROS_DISTRO: "humble" REDIS_HOST: "${REDIS_HOST}" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "ir" diff --git a/compose/cam_param_monitor.yml b/compose/cam_param_monitor.yml index 207b5d86..a5519c12 100644 --- a/compose/cam_param_monitor.yml +++ b/compose/cam_param_monitor.yml @@ -7,7 +7,7 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" REDIS_HOST: "${REDIS_HOST}" SYSTEM_NAME: "${SYSTEM_NAME}" diff --git a/compose/cam_rgb.yml b/compose/cam_rgb.yml index 746eb406..fbe9c0c6 100644 --- a/compose/cam_rgb.yml +++ b/compose/cam_rgb.yml @@ -8,8 +8,8 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" - ROS_DISTRO: "noetic" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" + ROS_DISTRO: "humble" REDIS_HOST: "${REDIS_HOST}" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "rgb" diff --git a/compose/cam_uv.yml b/compose/cam_uv.yml index e93da621..efbc29f7 100644 --- a/compose/cam_uv.yml +++ b/compose/cam_uv.yml @@ -8,9 +8,9 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" REDIS_HOST: "${REDIS_HOST}" - ROS_DISTRO: "noetic" + ROS_DISTRO: "humble" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "uv" CAM_FOV: "${CAM_FOV}" diff --git a/compose/roscore.yml b/compose/core_init.yml similarity index 72% rename from compose/roscore.yml rename to compose/core_init.yml index d59c5409..30a0fbc9 100644 --- a/compose/roscore.yml +++ b/compose/core_init.yml @@ -1,15 +1,15 @@ --- -## Bring up the core nodes +## Seed Redis with the static system config (formerly also ran roscore) services: ## =========================== headless nodes ============================= - roscore: - container_name: roscore + core_init: + container_name: core_init image: ${KAMERA_CORE_IMAGE} init: true tty: true network_mode: host environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" SPOOF_RATE: "${SPOOF_RATE}" REDIS_HOST: "${REDIS_HOST}" @@ -21,7 +21,7 @@ services: volumes: - "/mnt:/mnt" - "${PWD}/src:${DOCKER_KAMERA_DIR}/src:ro" - command: ["/entry/master.sh"] + command: ["/entry/core_init.sh"] restart: no ... diff --git a/compose/daq.yml b/compose/daq.yml index 65d58edc..0b870e94 100644 --- a/compose/daq.yml +++ b/compose/daq.yml @@ -10,7 +10,7 @@ services: - "${MCC_DAQ}:${MCC_DAQ}" environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" MCC_DAQ: "${MCC_DAQ}" SYSTEM_NAME: "${SYSTEM_NAME}" diff --git a/compose/detector.yml b/compose/detector.yml index 0461496c..20b69651 100644 --- a/compose/detector.yml +++ b/compose/detector.yml @@ -11,7 +11,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME:-}" PRODUCER_HOST: "${PRODUCER_HOST:-}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" diff --git a/compose/fps_monitor.yml b/compose/fps_monitor.yml index 91e228dd..8c858576 100644 --- a/compose/fps_monitor.yml +++ b/compose/fps_monitor.yml @@ -7,7 +7,7 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" REDIS_HOST: "${REDIS_HOST}" SYSTEM_NAME: "${SYSTEM_NAME}" diff --git a/compose/gui.yml b/compose/gui.yml index 005122df..e471f023 100644 --- a/compose/gui.yml +++ b/compose/gui.yml @@ -9,7 +9,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" ROS_IP: "${ROS_IP:-}" ROS_HOSTNAME: "${ROS_HOSTNAME:-}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" diff --git a/compose/image_manager.yml b/compose/image_manager.yml index f80acf00..1fcc7d61 100644 --- a/compose/image_manager.yml +++ b/compose/image_manager.yml @@ -9,7 +9,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" SYSTEM_NAME: "${SYSTEM_NAME}" dns: diff --git a/compose/imageview.yml b/compose/imageview.yml index 073735ed..3ea5f94d 100644 --- a/compose/imageview.yml +++ b/compose/imageview.yml @@ -7,7 +7,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_FOV: "${CAM_FOV}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" diff --git a/compose/ins.yml b/compose/ins.yml index 779a3aa1..a59c7726 100644 --- a/compose/ins.yml +++ b/compose/ins.yml @@ -10,7 +10,7 @@ services: - "${PULSE_TTY}:${PULSE_TTY}" environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" ALLOW_SERIAL_INS_SPOOF: "${ALLOW_SERIAL_INS_SPOOF}" SPOOF_INS: "${SPOOF_INS}" diff --git a/compose/nodelist.yml b/compose/nodelist.yml index 9a0eb69d..aefcc647 100644 --- a/compose/nodelist.yml +++ b/compose/nodelist.yml @@ -9,7 +9,7 @@ services: tty: true network_mode: host environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" volumes: - "${PWD}/src:${DOCKER_KAMERA_DIR}/src:ro" diff --git a/compose/shapefile_monitor.yml b/compose/shapefile_monitor.yml index 841d7b44..967ba644 100644 --- a/compose/shapefile_monitor.yml +++ b/compose/shapefile_monitor.yml @@ -8,7 +8,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" DOCKER_KAMERA_DIR: "${DOCKER_KAMERA_DIR}" SYSTEM_NAME: "${SYSTEM_NAME}" diff --git a/compose/spoof_events.yml b/compose/spoof_events.yml index 0272cc69..cc691d6b 100644 --- a/compose/spoof_events.yml +++ b/compose/spoof_events.yml @@ -9,7 +9,7 @@ services: devices: - "${PULSE_TTY}:${PULSE_TTY}" environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" SYSTEM_NAME: "${SYSTEM_NAME}" diff --git a/compose/sync_msg_publisher.yml b/compose/sync_msg_publisher.yml index 5d6f18ac..5b147f2b 100644 --- a/compose/sync_msg_publisher.yml +++ b/compose/sync_msg_publisher.yml @@ -7,7 +7,7 @@ services: init: true tty: true environment: - ROS_MASTER_URI: "${ROS_MASTER_URI}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" NODE_HOSTNAME: "${NODE_HOSTNAME:-}" ROS_IP: "${ROS_IP:-}" SATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" diff --git a/src/run_scripts/aliases.sh b/src/run_scripts/aliases.sh index ebdb3e17..63830ef2 100755 --- a/src/run_scripts/aliases.sh +++ b/src/run_scripts/aliases.sh @@ -53,12 +53,12 @@ alias cb-gui="catkin build wxpython_gui" # runtime shortcuts # core - only on center run-core() { - roslaunch kamcore kamcore.launch data_mount_point:="$DATA_MOUNT_POINT" + ros2 run kamcore seed_redis_config /cfg/${SYSTEM_NAME}/config.yaml } alias run1-core="run-core" -alias run-daq="roslaunch --wait mcc_daq daq.launch" -alias run-ins="roslaunch --wait ins_driver ins.launch" +alias run-daq="ros2 launch mcc_daq daq.launch.xml" +alias run-ins="ros2 launch ins_driver ins.launch.xml" # ins & daq alias run2-daq="run-daq" @@ -67,10 +67,10 @@ alias run3-ins="run-ins" # todo: put this stuff in the .launch # bring up both cams run-rgb() { - roslaunch --wait prosilica_camera prosilica.launch ip:=${iprgb} system_name:=${NODE_HOSTNAME} trigger_mode:=syncin2 + ros2 launch prosilica_camera prosilica.launch.xml ip:=${iprgb} system_name:=${NODE_HOSTNAME} trigger_mode:=syncin2 } run-ir() { - roslaunch --wait kw_genicam_driver genicam_a6750.launch camera_ipv4:=${ipir} \ + ros2 launch kw_genicam_driver flir_a6750.launch.xml camera_ipv4:=${ipir} \ system_name:=${NODE_HOSTNAME} firmware_mode:=mono16 trigger_mode:=External } diff --git a/src/run_scripts/entry/cam_ir.sh b/src/run_scripts/entry/cam_ir.sh index ed9eab26..2cf77ccc 100755 --- a/src/run_scripts/entry/cam_ir.sh +++ b/src/run_scripts/entry/cam_ir.sh @@ -42,11 +42,10 @@ fi -ROSWAIT="--wait" CAM_PIXEL_FORMAT=${CAM_PIXEL_FORMAT:-mono16} CAM_TRIGGER_SOURCE=${CAM_TRIGGER_SOURCE:-External} CAM_TIMEOUT=${CAM_TIMEOUT:-3333} -DRIVER=$(cq ".devices.${DEV_ID}.model").launch +DRIVER=$(cq ".devices.${DEV_ID}.model").launch.xml # extra arguments to pass to roslaunch in the form of `argname1:=val argname2:=val` CAM_EXTRA_ARGS=${CAM_EXTRA_ARGS:-} LOGFILE="/tmp/roslaunch_err_${CAM_FOV}_${CAM_MODE}.log" @@ -61,7 +60,7 @@ EXTRA ARGS : ${CAM_EXTRA_ARGS} " if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then echo "/debug/rebuild set, triggering rebuild on startup" - catkin build kw_genicam_driver + colcon build --packages-select kw_genicam_driver if [[ $? -ne 0 ]]; then echo "Rebuild failed. Your code is in an unstable state" exit 1 @@ -70,22 +69,14 @@ if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then fi fi -exec roslaunch "${ROSWAIT}" kw_genicam_driver ${DRIVER} \ +RESPAWN=$([[ "${NORESPAWN}" == "true" ]] && echo false || echo true) +exec ros2 launch kw_genicam_driver ${DRIVER} \ system_name:=${NODE_HOSTNAME} \ - norespawn:="${NORESPAWN}" \ + respawn:=${RESPAWN} \ cam_fov:=${CAM_FOV} \ camera_ipv4:=${CAM_IP} \ camera_manufacturer:=FLIR \ firmware_mode:=${CAM_PIXEL_FORMAT} \ nextImage_timeout:=${CAM_TIMEOUT} \ info_verbosity:=$(/cfg/get ".verbosity") \ - ${CAM_EXTRA_ARGS} 2> >(tee -a "${LOGFILE}" >&2) & - -STAT_ROS=$! -wait $STAT_ROS -echo "roslaunch probably died with a 0 error code" -RES=$(grep -Po -e 'REQUIRED.+ has died' "${LOGFILE}") -if [[ -n $RES ]]; then - echo $RES - exit 1 -fi + ${CAM_EXTRA_ARGS} 2> >(tee -a "${LOGFILE}" >&2) diff --git a/src/run_scripts/entry/cam_param_monitor.sh b/src/run_scripts/entry/cam_param_monitor.sh index 47db051f..820628b6 100755 --- a/src/run_scripts/entry/cam_param_monitor.sh +++ b/src/run_scripts/entry/cam_param_monitor.sh @@ -6,4 +6,4 @@ echo "<=> <=> <=> CAM PARAM MONITOR <=> <=> <=> " source /entry/project.sh source /aliases.sh -exec roslaunch --wait kamcore cam_param_monitor.launch norespawn:=${NORESPAWN:-false} +exec ros2 launch kamcore cam_param_monitor.launch.xml diff --git a/src/run_scripts/entry/cam_prosilica.sh b/src/run_scripts/entry/cam_prosilica.sh index 524497fc..87b1292e 100755 --- a/src/run_scripts/entry/cam_prosilica.sh +++ b/src/run_scripts/entry/cam_prosilica.sh @@ -87,12 +87,11 @@ trap "errcho 'Caught SIGINT'; cleanup" SIGINT # Expected exit code from docker stop command. trap "errcho 'Caught SIGTERM'; cleanup" SIGTERM -ROSWAIT="--wait" LOGFILE="/tmp/roslaunch_err_${CAM_FOV}_${CAM_MODE}.log" if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then echo "/debug/rebuild set, triggering rebuild on startup" - catkin build prosilica_camera + colcon build --packages-select prosilica_camera if [[ $? -ne 0 ]]; then echo "Rebuild failed. Your code is in an unstable state" exit 1 @@ -101,21 +100,13 @@ if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then fi fi -exec roslaunch "${ROSWAIT}" prosilica_camera prosilica.launch \ +RESPAWN=$([[ "${NORESPAWN}" == "true" ]] && echo false || echo true) +exec ros2 launch prosilica_camera prosilica.launch.xml \ ip:=${CAM_IP} \ system_name:=${NODE_HOSTNAME} \ cameratype:=${CAM_MODE} \ cam_fov:=${CAM_FOV} \ trigger_mode:=${TRIGGER_MODE} \ - norespawn:=${NORESPAWN} \ + respawn:=${RESPAWN} \ GainMode:=$(cq ".launch.cam.${CAM_MODE}.GainMode") \ - GainValue:=$(cq ".launch.cam.${CAM_MODE}.GainValue") 2> >(tee -a "${LOGFILE}" >&2) & - -STAT_ROS=$! -wait $STAT_ROS -echo "roslaunch probably died with a 0 error code" -RES=$(grep -Po -e 'REQUIRED.+ has died' "${LOGFILE}") -if [[ -n $RES ]]; then - echo $RES - exit 1 -fi + GainValue:=$(cq ".launch.cam.${CAM_MODE}.GainValue") 2> >(tee -a "${LOGFILE}" >&2) diff --git a/src/run_scripts/entry/core_init.sh b/src/run_scripts/entry/core_init.sh new file mode 100755 index 00000000..74ba8846 --- /dev/null +++ b/src/run_scripts/entry/core_init.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Core bootstrap script. +# In ROS1 this container ran roscore + a keepalive node and loaded the global +# rosparam tree. ROS2 has no master and the config lives in Redis, so all +# that's left is seeding Redis with the static system config. + +echo "[ ] [ ] [ ] CORE INIT [ ] [ ] [ ] " +# dump the global config as a debugging step +cat /cfg/${SYSTEM_NAME}/config.yaml + +source /entry/project_env.sh + +ping -c1 kameramaster + +# this is a rolling counter just for fun, and also serves as something any client can always grab +REDIS_HOST=${REDIS_HOST:-nuvo0} +redis-client -h ${REDIS_HOST} incr term + +# Seed Redis with the static system config before anything starts, so kamcore +# nodes (cam_param_monitor, etc.) read /sys/arch from Redis without depending on +# the GUI. +ros2 run kamcore seed_redis_config /cfg/${SYSTEM_NAME}/config.yaml + +echo "Redis seeded. Core init complete; idling." +exec sleep infinity diff --git a/src/run_scripts/entry/daq.sh b/src/run_scripts/entry/daq.sh index 477801e0..2c024079 100755 --- a/src/run_scripts/entry/daq.sh +++ b/src/run_scripts/entry/daq.sh @@ -13,7 +13,7 @@ fi if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then echo "/debug/rebuild set, triggering rebuild on startup" - catkin build mcc_daq + colcon build --packages-select mcc_daq if [[ $? -ne 0 ]]; then echo "Rebuild failed. Your code is in an unstable state" exit 1 @@ -22,9 +22,10 @@ if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then fi fi +RESPAWN=$([[ "${NORESPAWN}" == "true" ]] && echo false || echo true) if [[ "$MCC_DAQ" == *"tty"* ]] ; then export DAQ_TTY="$MCC_DAQ" - exec roslaunch --wait ser_daq ser_daq.launch norespawn:="${NORESPAWN}" + exec ros2 launch ser_daq ser_daq.launch.xml else - exec roslaunch --wait mcc_daq daq.launch norespawn:="${NORESPAWN}" + exec ros2 launch mcc_daq daq.launch.xml respawn:=${RESPAWN} fi diff --git a/src/run_scripts/entry/fps_monitor.sh b/src/run_scripts/entry/fps_monitor.sh index 28799c1e..5057bb48 100755 --- a/src/run_scripts/entry/fps_monitor.sh +++ b/src/run_scripts/entry/fps_monitor.sh @@ -6,4 +6,4 @@ echo "<=> <=> <=> FPS MONITOR <=> <=> <=> " source /entry/project.sh source /aliases.sh -exec roslaunch --wait kamcore fps_monitor.launch norespawn:=${NORESPAWN:-false} +exec ros2 launch kamcore fps_monitor.launch.xml diff --git a/src/run_scripts/entry/imageview.sh b/src/run_scripts/entry/imageview.sh index 50e262ec..58563940 100755 --- a/src/run_scripts/entry/imageview.sh +++ b/src/run_scripts/entry/imageview.sh @@ -23,8 +23,6 @@ else COMPRESS_IMAGERY="false" fi -roslaunch --wait view_server image_view_server.launch \ - norespawn:="${NORESPAWN}" \ - system_name:=${NODE_HOSTNAME} \ +exec ros2 launch view_server image_view_server.launch.xml \ send_image_data:=${SEND_IMAGE_DATA} \ compress_imagery:=${COMPRESS_IMAGERY} diff --git a/src/run_scripts/entry/ins.sh b/src/run_scripts/entry/ins.sh index b878e719..0fe00d8c 100755 --- a/src/run_scripts/entry/ins.sh +++ b/src/run_scripts/entry/ins.sh @@ -6,9 +6,6 @@ echo "( ) ( ) ( ) INS ( ) ( ) ( ) " source /entry/project.sh source /aliases.sh -# INS really ought to be run with kamcore and hence should not use spoof here -if [[ -n ${SPOOF_INS} ]] ; then - echo "spoof mode" - ARG_SPOOF="spoof:=${SPOOF_INS}" -fi -exec roslaunch --wait ins_driver ins.launch ${ARG_SPOOF} norespawn:="${NORESPAWN}" +# Spoofing is controlled via the SPOOF_RATE / SPOOF_INS environment variables +RESPAWN=$([[ "${NORESPAWN}" == "true" ]] && echo false || echo true) +exec ros2 launch ins_driver ins.launch.xml respawn:=${RESPAWN} diff --git a/src/run_scripts/entry/master.sh b/src/run_scripts/entry/master.sh deleted file mode 100755 index 8b20abf5..00000000 --- a/src/run_scripts/entry/master.sh +++ /dev/null @@ -1,49 +0,0 @@ -#!/bin/bash - -# Core node startup script - -echo "[ ] [ ] [ ] KAMCORE [ ] [ ] [ ] " -# dump the global config as a debugging step -cat /cfg/${SYSTEM_NAME}/config.yaml - -source /entry/project_env.sh - - -function cleanup { - pkill -2 kamcore -} - -# Expected exit code from a Ctrl-C when in explicit docker run mode. -trap "errcho 'Caught SIGINT'; cleanup" SIGINT -# Expected exit code from docker stop command. -trap "errcho 'Caught SIGTERM'; cleanup" SIGTERM - -ping -c1 kameramaster - -# this is a rolling counter just for fun, and also serves as something any client can always grab -REDIS_HOST=${REDIS_HOST:-nuvo0} -redis-client -h ${REDIS_HOST} incr term - -# Seed Redis with the static system config before anything starts, so kamcore -# nodes (cam_param_monitor, etc.) read /sys/arch from Redis without depending on -# the GUI. Done before roscore so the --wait monitors can't start until it's up. -rosrun kamcore seed_redis_config.py /cfg/${SYSTEM_NAME}/config.yaml - -# Start core and block until it's up, then bootstrap parameters -roscore & - -# check that master is in fact up -FAIL_COUNT=0 -until /entry/rosnode_list.sh; do - sleep 1 - echo "Attempt $((++FAIL_COUNT))"; - if [[ $FAIL_COUNT -gt 3 ]]; then - errcho "Unable to contact ros master. Running WTF and aborting startup" - /entry/wat.sh - exit 1 - fi -done - -rosparam load /cfg/${SYSTEM_NAME}/config.yaml /cfg -exec roslaunch kamcore kamcore.launch data_mount_point:=$DATA_MOUNT_POINT \ - spoof_rate:="${SPOOF_RATE}" diff --git a/src/run_scripts/entry/rosnode_list.sh b/src/run_scripts/entry/rosnode_list.sh index 09f20c44..23f0c14d 100755 --- a/src/run_scripts/entry/rosnode_list.sh +++ b/src/run_scripts/entry/rosnode_list.sh @@ -1,8 +1,8 @@ #!/bin/bash -# Diagnostics +# Diagnostics -echo "[ ] [ ] [ ] ROSNODE CHECK [ ] [ ] [ ] " +echo "[ ] [ ] [ ] ROS2 NODE CHECK [ ] [ ] [ ] " source /entry/project_env.sh @@ -12,5 +12,4 @@ trap "errcho 'Caught SIGINT'; cleanup" SIGINT # Expected exit code from docker stop command. trap "errcho 'Caught SIGTERM'; cleanup" SIGTERM -exec rosnode list - +exec ros2 node list diff --git a/src/run_scripts/entry/shapefile_monitor.sh b/src/run_scripts/entry/shapefile_monitor.sh index 83aa0717..eebd33c9 100755 --- a/src/run_scripts/entry/shapefile_monitor.sh +++ b/src/run_scripts/entry/shapefile_monitor.sh @@ -6,4 +6,4 @@ echo "<=> <=> <=> CAM PARAM MONITOR <=> <=> <=> " source /entry/project.sh source /aliases.sh -exec roslaunch --wait kamcore shapefile_monitor.launch norespawn:=${NORESPAWN:-false} +exec ros2 launch kamcore shapefile_monitor.launch.xml diff --git a/src/run_scripts/entry/spoof_events.sh b/src/run_scripts/entry/spoof_events.sh index 79d0234d..021666a1 100755 --- a/src/run_scripts/entry/spoof_events.sh +++ b/src/run_scripts/entry/spoof_events.sh @@ -4,4 +4,4 @@ echo "( ) ( ) ( ) SPOOOOOOF INS ( ) ( ) ( ) " source /entry/project.sh -roslaunch --wait ins_driver spoof_events.launch +ros2 launch ins_driver spoof_events.launch.xml diff --git a/src/run_scripts/entry/viame.sh b/src/run_scripts/entry/viame.sh index 86272cef..17424ce1 100755 --- a/src/run_scripts/entry/viame.sh +++ b/src/run_scripts/entry/viame.sh @@ -29,8 +29,7 @@ if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then echo "/debug/rebuild set, triggering rebuild on startup" # todo: remove this shim # we have to clean first since the current docker image puts roskv in the wrong spot - catkin clean -y - catkin build sprokit_adapters + colcon build --packages-select sprokit_adapters if [[ $? -ne 0 ]]; then echo "Rebuild failed. Your code is in an unstable state" exit 1 @@ -130,7 +129,7 @@ echo "KAM_FLIGHT : ${KAM_FLIGHT}" echo "PIPEFILE that's in use: ${PIPEFILE}" printf " -\$ exec roslaunch sprokit_adapters sprokit_detector_fusion_adapter.launch \ +\$ exec ros2 launch sprokit_adapters sprokit_detector_fusion_adapter.launch.xml \ kwiver:=${WS_DEVEL} \ system_name:=${NODE_HOSTNAME} \ detector_node:=detector \ @@ -149,20 +148,16 @@ printf " norespawn:=${NORESPAWN:-false} \ image_list_dir:=${IMAGE_LIST_DIR} " -# TODO: -# Hack because I forgot this in the built image -rosparam set /${NODE_HOSTNAME}/detector/sync_q_size ${SYNC_Q_SIZE} +# Detection output dirs are read from the environment by the pipelines +export DETECTION_CSV_DIR +export IMAGE_LIST_DIR -# Use the fork syntax so we can catch if roslaunch exits (since it only exits 0) -# Manually catch failures here so at least we have the option of letting docker restart -roslaunch --wait sprokit_adapters sprokit_detector_fusion_adapter.launch \ - kwiver:=${WS_DEVEL} \ +RESPAWN=$([[ "${NORESPAWN:-false}" == "true" ]] && echo false || echo true) +exec ros2 launch sprokit_adapters sprokit_detector_fusion_adapter.launch.xml \ system_name:=${NODE_HOSTNAME} \ detector_node:=detector \ detection_pipefile:="${PIPEFILE}" \ pipeline_dir:="${PIPELINE_DIR}" \ - embed_det_chips:=true \ - pad_det_chip_percent:=50 \ det_topic:=/${NODE_HOSTNAME}/detections \ detector_id_string:=seal \ synchronized_images_in1:=/${NODE_HOSTNAME}/synched \ @@ -172,14 +167,4 @@ roslaunch --wait sprokit_adapters sprokit_detector_fusion_adapter.launch \ redis_uri:="tcp://${REDIS_HOST}:6379" \ ocv_num_threads:=4 \ sync_q_size:=${SYNC_Q_SIZE} \ - detection_csv_dir:=${DETECTION_CSV_DIR} \ - norespawn:=${NORESPAWN:-false} \ - image_list_dir:=${IMAGE_LIST_DIR} 2> >(tee -a /tmp/roslaunch_err.log >&2) & -STAT_ROS=$! -wait $STAT_ROS -echo "roslaunch probably died with a 0 error code" -RES=$(grep -Po -e 'REQUIRED.+ has died' /tmp/roslaunch_err.log) -if [[ -n $RES ]]; then - echo $RES - exit 1 -fi + respawn:=${RESPAWN} 2> >(tee -a /tmp/roslaunch_err.log >&2) diff --git a/tmux/nayak/env.sh b/tmux/nayak/env.sh index a6b993f6..6abf1543 100644 --- a/tmux/nayak/env.sh +++ b/tmux/nayak/env.sh @@ -29,15 +29,14 @@ unset _redis_elapsed echo "Redis successfully connected at ${REDIS_HOST}, starting." -export ROS_MASTER="$(cq .master_host)" export NODE_HOSTNAME=$(hostname) -export ROS_HOSTNAME=${NODE_HOSTNAME} -export ROS_MASTER_URI="http://${ROS_MASTER}:11311" +# ROS2: peer discovery via DDS; all hosts must share a domain id +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" export DOCKER_KAMERA_DIR="/root/kamera" export DATA_MOUNT_POINT=$(cq .local_ssd_mnt) export CAM_FOV=$(cq ".arch.hosts[\"${NODE_HOSTNAME}\"].fov") -export ROS_DISTRO="noetic" +export ROS_DISTRO="humble" export KAMERA_DNS_IP="192.168.88.1" export PROJ_DIR="/root/kamera" export PULSE_TTY=/dev/ttyS0 diff --git a/tmux/nayak/leader/supervisor.conf b/tmux/nayak/leader/supervisor.conf index 932f660e..71511265 100644 --- a/tmux/nayak/leader/supervisor.conf +++ b/tmux/nayak/leader/supervisor.conf @@ -40,8 +40,8 @@ command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh r user=user autostart=true -[program:roscore] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh roscore +[program:core_init] +command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh core_init user=user autostart=false diff --git a/tmux/taiga/env.sh b/tmux/taiga/env.sh index 5420de8c..56a23f1e 100644 --- a/tmux/taiga/env.sh +++ b/tmux/taiga/env.sh @@ -28,14 +28,14 @@ unset _redis_elapsed echo "Redis successfully connected at ${REDIS_HOST}, starting." -export ROS_HOSTNAME=$(cq ".master_host") export NODE_HOSTNAME=$(hostname) -export ROS_MASTER_URI="http://${ROS_HOSTNAME}:11311" +# ROS2: peer discovery via DDS; all hosts must share a domain id +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" export DOCKER_KAMERA_DIR="/root/kamera" export DATA_MOUNT_POINT=$(cq .local_ssd_mnt) export CAM_FOV=$(cq ".arch.hosts[\"${NODE_HOSTNAME}\"].fov") -export ROS_DISTRO="noetic" +export ROS_DISTRO="humble" export KAMERA_DNS_IP="192.168.88.1" export PULSE_TTY=/dev/ttyS0 export MCC_DAQ="/dev/$(readlink /dev/mcc_daq)" diff --git a/tmux/taiga/leader/supervisor.conf b/tmux/taiga/leader/supervisor.conf index a52f86bd..6157e801 100644 --- a/tmux/taiga/leader/supervisor.conf +++ b/tmux/taiga/leader/supervisor.conf @@ -40,8 +40,8 @@ command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh r user=user autostart=true -[program:roscore] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh roscore +[program:core_init] +command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh core_init user=user autostart=false From 302dbf0dbee66abf1df862d55601c4fde2d85f55 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 21:18:50 -0400 Subject: [PATCH 11/20] Target ROS2 Jazzy Jalisco (Ubuntu 24.04) - ROS_DISTRO humble -> jazzy in tmux env.sh and compose camera services - cv_bridge includes moved to cv_bridge.hpp (the .h shim is deprecated since Iron) - docker: core-ros base moves to nvidia/cuda ubuntu24.04, ROS2 apt source (noble) with keyring-based signing, ros-jazzy-* packages, and colcon in place of catkin tools; pip installs use --break-system-packages for PEP-668 (24.04) pythons - core-deps: jazzy equivalents of the perception deps; dropped ROS1-only nodelet/self_test/polled_camera/message-generation debs and the gtk2-era glademm libs that no longer exist on noble - core/detector images build with colcon --packages-up-to so unported ROS1 packages (phase_one, wxpython_gui, ...) are not touched; detector-viame-deps notes the VIAME base image must be rebuilt on 24.04 to host jazzy debs - activate_ros.bash sources the colcon install/setup.bash overlay and RCUTILS log format instead of devel/setup.bash + ROSCONSOLE_FORMAT gui.dockerfile intentionally left on catkin: wxpython_gui is still ROS1 and outside the nayak/taiga supervisor scope. --- compose/cam_ir.yml | 2 +- compose/cam_rgb.yml | 2 +- compose/cam_uv.yml | 2 +- docker/base/core-deps.dockerfile | 28 ++++++---------- docker/base/core-ros.dockerfile | 32 +++++++++---------- docker/base/detector-viame-deps.dockerfile | 24 +++++++------- docker/core.dockerfile | 8 ++--- docker/detector.dockerfile | 4 +-- scripts/activate_ros.bash | 14 +++----- .../kw_genicam_driver/src/driver_a6750.cpp | 2 +- src/cams/kw_genicam_driver/src/spec_a6750.cpp | 2 +- .../src/nodes/prosilica_node.cpp | 2 +- .../src/kw_detector_fusion_adapter.cpp | 2 +- tmux/nayak/env.sh | 2 +- tmux/taiga/env.sh | 2 +- 15 files changed, 57 insertions(+), 71 deletions(-) diff --git a/compose/cam_ir.yml b/compose/cam_ir.yml index a904f07c..5dbf20f0 100644 --- a/compose/cam_ir.yml +++ b/compose/cam_ir.yml @@ -8,7 +8,7 @@ services: tty: true environment: ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" - ROS_DISTRO: "humble" + ROS_DISTRO: "jazzy" REDIS_HOST: "${REDIS_HOST}" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "ir" diff --git a/compose/cam_rgb.yml b/compose/cam_rgb.yml index fbe9c0c6..4657f55b 100644 --- a/compose/cam_rgb.yml +++ b/compose/cam_rgb.yml @@ -9,7 +9,7 @@ services: tty: true environment: ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" - ROS_DISTRO: "humble" + ROS_DISTRO: "jazzy" REDIS_HOST: "${REDIS_HOST}" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "rgb" diff --git a/compose/cam_uv.yml b/compose/cam_uv.yml index efbc29f7..2b60ef41 100644 --- a/compose/cam_uv.yml +++ b/compose/cam_uv.yml @@ -10,7 +10,7 @@ services: environment: ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" REDIS_HOST: "${REDIS_HOST}" - ROS_DISTRO: "humble" + ROS_DISTRO: "jazzy" NODE_HOSTNAME: "${NODE_HOSTNAME}" CAM_MODE: "uv" CAM_FOV: "${CAM_FOV}" diff --git a/docker/base/core-deps.dockerfile b/docker/base/core-deps.dockerfile index 18df1ef9..964bc285 100644 --- a/docker/base/core-deps.dockerfile +++ b/docker/base/core-deps.dockerfile @@ -1,24 +1,14 @@ FROM kamera/base/core-ros:latest RUN apt-get update && apt-get install --no-install-recommends -y \ - ros-noetic-compressed-image-transport \ - ros-noetic-camera-info-manager \ - ros-noetic-image-view \ - ros-noetic-cv-bridge \ - ros-noetic-nodelet \ - ros-noetic-nodelet-topic-tools \ - ros-noetic-vision-opencv \ - ros-noetic-diagnostic-updater \ - ros-noetic-self-test \ - ros-noetic-polled-camera \ - ros-noetic-message-generation \ - ros-noetic-message-runtime \ - libgtkmm-2.4-1v5 \ - libglademm-2.4-1v5 \ - libgtkglextmm-x11-1.2-dev \ - libgtkglextmm-x11-1.2-0v5 \ - libglade2-dev \ - libglademm-2.4-dev \ + ros-jazzy-compressed-image-transport \ + ros-jazzy-image-transport \ + ros-jazzy-camera-info-manager \ + ros-jazzy-camera-calibration-parsers \ + ros-jazzy-cv-bridge \ + ros-jazzy-vision-opencv \ + ros-jazzy-diagnostic-updater \ + ros-jazzy-rosidl-default-generators \ && rm -rf /var/lib/apt/lists/* ## build deps @@ -40,7 +30,7 @@ RUN apt-get update -q && apt-get install --no-install-recommends -y \ usbutils \ && rm -rf /var/lib/apt/lists/* -RUN pip install --no-cache-dir \ +RUN pip install --break-system-packages --no-cache-dir \ pyserial \ osrf-pycommon \ shapely \ diff --git a/docker/base/core-ros.dockerfile b/docker/base/core-ros.dockerfile index 7fc23890..7876315c 100644 --- a/docker/base/core-ros.dockerfile +++ b/docker/base/core-ros.dockerfile @@ -1,12 +1,12 @@ # This image contains the base of the ROS/CUDA for the system, plus # a bunch of utility packages -FROM nvidia/cuda:12.6.2-devel-ubuntu20.04 AS base_cuda_ubuntu +FROM nvidia/cuda:12.6.2-devel-ubuntu24.04 AS base_cuda_ubuntu WORKDIR /root # setup environment ENV LANG C.UTF-8 ENV LC_ALL C.UTF-8 -ENV ROS_DISTRO noetic +ENV ROS_DISTRO jazzy ENV DEBIAN_FRONTEND noninteractive # setup timezone @@ -20,22 +20,24 @@ RUN echo 'Etc/UTC' > /etc/timezone && \ RUN apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ gnupg2 \ + curl \ + ca-certificates \ && rm -rf /var/lib/apt/lists/* -# setup sources.list -RUN echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros1-latest.list - -# setup keys -RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +# setup ROS2 apt source (Jazzy runs on Ubuntu 24.04 / noble) +RUN curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu noble main" \ + > /etc/apt/sources.list.d/ros2-latest.list # install ros packages RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-noetic-ros-core=1.5.0-1* \ - ros-noetic-ros-base=1.5.0-1* \ - ros-noetic-perception=1.5.0-1* \ - python3-catkin-tools \ + ros-jazzy-ros-core \ + ros-jazzy-ros-base \ + ros-jazzy-perception \ + python3-colcon-common-extensions \ python3-pip \ - ros-noetic-rqt-image-view \ + ros-jazzy-rqt-image-view \ && rm -rf /var/lib/apt/lists/* # ROS BUILD FINISHED @@ -56,9 +58,6 @@ RUN apt-get update -q && apt-get install --no-install-recommends -y \ sqlite3 \ python3-pip \ python3-rosdep \ - python3-rosinstall \ - python3-vcstools \ - python3-catkin-tools \ unzip \ && apt-get update -q && apt-get install --no-install-recommends -y \ autoconf \ @@ -76,8 +75,7 @@ RUN apt-get update -q && apt-get install --no-install-recommends -y \ ## ipython isn't strictly required (like most things in is kitchen sink image) but it's extremely useful for debugging -RUN pip install --upgrade --no-cache-dir pip \ - && pip install --no-cache-dir \ +RUN pip install --break-system-packages --no-cache-dir \ ipython \ ipdb \ pyserial \ diff --git a/docker/base/detector-viame-deps.dockerfile b/docker/base/detector-viame-deps.dockerfile index 12675132..082a8d61 100644 --- a/docker/base/detector-viame-deps.dockerfile +++ b/docker/base/detector-viame-deps.dockerfile @@ -1,11 +1,14 @@ # Build off the public VIAME docker build (with ITK support) +# NOTE (ROS2 port): Jazzy requires an Ubuntu 24.04 (noble) base. The VIAME +# image referenced here must be one built on 24.04; the old focal-based +# gpu-algorithms-seal tag cannot host Jazzy debs. FROM kitware/viame:gpu-algorithms-seal AS vb WORKDIR /root # setup environment ENV LANG C.UTF-8 ENV LC_ALL C.UTF-8 -ENV ROS_DISTRO noetic +ENV ROS_DISTRO jazzy ENV DEBIAN_FRONTEND noninteractive # install packages @@ -20,19 +23,18 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \ iputils-ping \ && rm -rf /var/lib/apt/lists/* -# setup sources.list -RUN echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros1-latest.list - -# setup keys -RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +# setup ROS2 apt source (Jazzy runs on Ubuntu 24.04 / noble) +RUN curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu noble main" \ + > /etc/apt/sources.list.d/ros2-latest.list # install ros packages RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-noetic-ros-core=1.5.0-1* \ - ros-noetic-ros-base=1.5.0-1* \ - ros-noetic-perception=1.5.0-1* \ - python3-catkin-tools \ - ros-noetic-rqt-image-view \ + ros-jazzy-ros-core \ + ros-jazzy-ros-base \ + ros-jazzy-perception \ + python3-colcon-common-extensions \ && rm -rf /var/lib/apt/lists/* # Build tools necessary for catkin and roskv diff --git a/docker/core.dockerfile b/docker/core.dockerfile index 7dc471b4..ceafe2ab 100644 --- a/docker/core.dockerfile +++ b/docker/core.dockerfile @@ -10,7 +10,6 @@ COPY . $REPO_DIR RUN rm -rf /entry \ && ln -sf $REPO_DIR/src/run_scripts/entry /entry \ && printf "\nsource /entry/project.sh\n" >> /root/.bashrc \ - && touch $REPO_DIR/.catkin_workspace \ && ln -sf $REPO_DIR/src/run_scripts/aliases.sh /aliases.sh \ && printf "\nsource /aliases.sh\n" >> /root/.bashrc @@ -19,10 +18,11 @@ RUN ln -sf $REPO_DIR/src/cfg /cfg RUN mkdir -p /root/.config/kamera && \ ln -sf $REPO_DIR/.dir /root/.config/kamera/repo_dir.bash -# Need to build phase_one first to generate SRV, then build backend +# Build the ported ROS2 packages (backend pulls in the whole nayak/taiga set); +# unported ROS1 packages (phase_one, wxpython_gui, ...) are skipped by +# --packages-up-to and would not build under Jazzy anyway RUN ln -sv /usr/bin/python3 /usr/bin/python || true -RUN [ "/bin/bash", "-c", "source ${REPO_DIR}/activate_ros.bash && catkin build phase_one"] -RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build -s backend"] +RUN [ "/bin/bash", "-c", "source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build --base-paths src --packages-up-to backend sprokit_adapters --cmake-args -DCMAKE_BUILD_TYPE=Release || colcon build --base-paths src --packages-up-to backend --cmake-args -DCMAKE_BUILD_TYPE=Release"] ENTRYPOINT ["/entry/project.sh"] CMD ["bash"] diff --git a/docker/detector.dockerfile b/docker/detector.dockerfile index b7872836..b87b779d 100644 --- a/docker/detector.dockerfile +++ b/docker/detector.dockerfile @@ -8,6 +8,6 @@ WORKDIR /root/kamera ENV REPO_DIR=/root/kamera ENV CMAKE_POLICY_VERSION_MINIMUM=3.5 -RUN ["/bin/bash", "-c", "source /entry/project.sh && \ +RUN ["/bin/bash", "-c", "source /opt/ros/${ROS_DISTRO}/setup.bash && \ source src/run_scripts/setup/setup_viame_build.sh && \ - catkin build sprokit_adapters"] + colcon build --base-paths src --packages-up-to sprokit_adapters --cmake-args -DCMAKE_BUILD_TYPE=Release"] diff --git a/scripts/activate_ros.bash b/scripts/activate_ros.bash index fe54ac21..0756d23a 100644 --- a/scripts/activate_ros.bash +++ b/scripts/activate_ros.bash @@ -4,17 +4,13 @@ # devel setup or the base ROS environment setup script. This cascades down # ROS versions when a development environment does not exist. -# may have changed -MASTER_HOST=$(echo $ROS_MASTER_URI| grep -Po -e '(?<=http:\/\/)([\w\.]+)(?=:)') - rosinfo () { printf "=== === === ACTIVATE ROS ${ROS_DISTRO} === === === === === DATA_MOUNT_POINT: ${DATA_MOUNT_POINT} CAM_FOV : ${CAM_FOV} HOSTNAME : `hostname` NODE_HOSTNAME : ${NODE_HOSTNAME} -ROS_HOST/IP : ${ROS_HOSTNAME} ${ROS_IP} -ROS_MASTER_URI : ${ROS_MASTER_URI} +ROS_DOMAIN_ID : ${ROS_DOMAIN_ID} REPO_DIR : ${REPO_DIR} " } @@ -24,7 +20,7 @@ rosinfo echo "Sourcing files and establishing environment" ## This presumes ROS_DISTRO is set -DEVEL_SETUP="${REPO_DIR}/devel/setup.bash" +DEVEL_SETUP="${REPO_DIR}/install/setup.bash" VERSION_SETUP_PATH="/opt/ros/${ROS_DISTRO}/setup.bash" if [ -f "${VERSION_SETUP_PATH}" ] then @@ -37,10 +33,10 @@ fi if [ -f "${DEVEL_SETUP}" ] then - echo "Sourcing workspace devel setup" + echo "Sourcing colcon workspace setup" source "${DEVEL_SETUP}" else - echo "WARNING: Found no ROS devel setup script in workspace: ${DEVEL_SETUP}" + echo "WARNING: Found no colcon workspace setup script: ${DEVEL_SETUP}" fi ### ok ros devel/setup.bash does some weird stuff with path so we have to make sure it's still set up correctly @@ -54,4 +50,4 @@ fi echo "activate_ros::PATH: [${PATH}]" # this sets the logging format -export ROSCONSOLE_FORMAT='${walltime}: ${message}' +export RCUTILS_CONSOLE_OUTPUT_FORMAT='{time}: {message}' diff --git a/src/cams/kw_genicam_driver/src/driver_a6750.cpp b/src/cams/kw_genicam_driver/src/driver_a6750.cpp index 91b3d9a7..33f85325 100644 --- a/src/cams/kw_genicam_driver/src/driver_a6750.cpp +++ b/src/cams/kw_genicam_driver/src/driver_a6750.cpp @@ -15,7 +15,7 @@ #include "std_msgs/msg/int8.hpp" #include "std_msgs/msg/header.hpp" -#include +#include #include #include #include diff --git a/src/cams/kw_genicam_driver/src/spec_a6750.cpp b/src/cams/kw_genicam_driver/src/spec_a6750.cpp index e989ac9b..17b52a0b 100644 --- a/src/cams/kw_genicam_driver/src/spec_a6750.cpp +++ b/src/cams/kw_genicam_driver/src/spec_a6750.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include using std::string; using std::map; diff --git a/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp b/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp index 74042a63..72571bb3 100644 --- a/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp +++ b/src/cams/prosilica_camera/src/nodes/prosilica_node.cpp @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include diff --git a/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp b/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp index 3ae7886a..5cf39e86 100644 --- a/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp +++ b/src/kitware-ros-pkg/sprokit_adapters/src/kw_detector_fusion_adapter.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include diff --git a/tmux/nayak/env.sh b/tmux/nayak/env.sh index 6abf1543..14dd7ada 100644 --- a/tmux/nayak/env.sh +++ b/tmux/nayak/env.sh @@ -36,7 +36,7 @@ export DOCKER_KAMERA_DIR="/root/kamera" export DATA_MOUNT_POINT=$(cq .local_ssd_mnt) export CAM_FOV=$(cq ".arch.hosts[\"${NODE_HOSTNAME}\"].fov") -export ROS_DISTRO="humble" +export ROS_DISTRO="jazzy" export KAMERA_DNS_IP="192.168.88.1" export PROJ_DIR="/root/kamera" export PULSE_TTY=/dev/ttyS0 diff --git a/tmux/taiga/env.sh b/tmux/taiga/env.sh index 56a23f1e..cbcfffeb 100644 --- a/tmux/taiga/env.sh +++ b/tmux/taiga/env.sh @@ -35,7 +35,7 @@ export DOCKER_KAMERA_DIR="/root/kamera" export DATA_MOUNT_POINT=$(cq .local_ssd_mnt) export CAM_FOV=$(cq ".arch.hosts[\"${NODE_HOSTNAME}\"].fov") -export ROS_DISTRO="humble" +export ROS_DISTRO="jazzy" export KAMERA_DNS_IP="192.168.88.1" export PULSE_TTY=/dev/ttyS0 export MCC_DAQ="/dev/$(readlink /dev/mcc_daq)" From 6824729ffbb02139ceeab227276cc6119c2ff83c Mon Sep 17 00:00:00 2001 From: romleiaj Date: Thu, 2 Jul 2026 21:20:25 -0400 Subject: [PATCH 12/20] Base detector image on kitware/viame:gpu-algorithms The current gpu-algorithms tag is built on Ubuntu 24.04, so it can host the Jazzy debs directly - drops the stale focal-based gpu-algorithms-seal tag and the rebuild caveat. --- docker/base/detector-viame-deps.dockerfile | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/docker/base/detector-viame-deps.dockerfile b/docker/base/detector-viame-deps.dockerfile index 082a8d61..c3723408 100644 --- a/docker/base/detector-viame-deps.dockerfile +++ b/docker/base/detector-viame-deps.dockerfile @@ -1,8 +1,7 @@ -# Build off the public VIAME docker build (with ITK support) -# NOTE (ROS2 port): Jazzy requires an Ubuntu 24.04 (noble) base. The VIAME -# image referenced here must be one built on 24.04; the old focal-based -# gpu-algorithms-seal tag cannot host Jazzy debs. -FROM kitware/viame:gpu-algorithms-seal AS vb +# Build off the public VIAME docker build (with ITK support). +# The current gpu-algorithms tag is built on Ubuntu 24.04 (noble), which is +# what Jazzy requires. +FROM kitware/viame:gpu-algorithms AS vb WORKDIR /root # setup environment From 0a7773246bdeeb7f330d562e225da7c944476f7d Mon Sep 17 00:00:00 2001 From: romleiaj Date: Fri, 3 Jul 2026 21:38:27 -0400 Subject: [PATCH 13/20] Port phase_one (Phase One iXM RGB driver) to ROS2 - phase_one_standalone becomes a plain rclcpp node; services are declared with the ~/ prefix so they resolve to the same //rgb//... names the GUI and cam_param_monitor call. The old ROS_NAMESPACE env plumbing is replaced by a launch namespace arg (cam_phaseone.sh updated to ros2 launch) - srvs (Get/SetPhaseOneParameter, Get[Compressed]ImageView) generated via rosidl; cam_param_monitor's optional phase_one import now works on ROS2 systems that install this package - Event/image matching uses the shared cam_utils EventCache with explicit event_num (header.seq removed in ROS2); the debayer-queue seq map and save_every_x modulo now key on event_num - Dropped the nodelet variants (phase_one_nodelet, view_server_nodelet, phase_one_node loader) and the local phase_one_utils copy replaced by cam_utils; boost::filesystem -> std::filesystem - backend metapackage depends on phase_one again --- src/backend/package.xml | 1 + src/cams/phase_one/CMakeLists.txt | 310 ++---------- .../phase_one/include/phase_one/phase_one.h | 92 ++-- .../include/phase_one/phase_one_utils.h | 96 ---- src/cams/phase_one/launch/phase_one.launch | 19 - .../launch/phase_one_standalone.launch | 26 - .../launch/phase_one_standalone.launch.xml | 27 ++ src/cams/phase_one/nodelet_plugins.xml | 12 - src/cams/phase_one/package.xml | 71 +-- src/cams/phase_one/setup.py | 10 - src/cams/phase_one/src/phase_one_node.cpp | 16 - src/cams/phase_one/src/phase_one_nodelet.cpp | 451 ------------------ .../phase_one/src/phase_one_standalone.cpp | 362 +++++++------- src/cams/phase_one/src/phase_one_utils.cpp | 167 ------- .../phase_one/src/view_server_nodelet.cpp | 142 ------ src/run_scripts/entry/cam_phaseone.sh | 20 +- 16 files changed, 323 insertions(+), 1499 deletions(-) delete mode 100644 src/cams/phase_one/include/phase_one/phase_one_utils.h delete mode 100644 src/cams/phase_one/launch/phase_one.launch delete mode 100644 src/cams/phase_one/launch/phase_one_standalone.launch create mode 100644 src/cams/phase_one/launch/phase_one_standalone.launch.xml delete mode 100644 src/cams/phase_one/nodelet_plugins.xml delete mode 100644 src/cams/phase_one/setup.py delete mode 100755 src/cams/phase_one/src/phase_one_node.cpp delete mode 100755 src/cams/phase_one/src/phase_one_nodelet.cpp delete mode 100644 src/cams/phase_one/src/phase_one_utils.cpp delete mode 100755 src/cams/phase_one/src/view_server_nodelet.cpp diff --git a/src/backend/package.xml b/src/backend/package.xml index c1d5cc98..9573ccff 100644 --- a/src/backend/package.xml +++ b/src/backend/package.xml @@ -22,6 +22,7 @@ view_server cam_utils prosilica_camera + phase_one kw_genicam_driver diff --git a/src/cams/phase_one/CMakeLists.txt b/src/cams/phase_one/CMakeLists.txt index 96e21896..ad170b2b 100644 --- a/src/cams/phase_one/CMakeLists.txt +++ b/src/cams/phase_one/CMakeLists.txt @@ -1,8 +1,11 @@ -cmake_minimum_required(VERSION 2.9...3.13) +cmake_minimum_required(VERSION 3.13) project(phase_one) include(FetchContent) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + string(TOLOWER ${CMAKE_SYSTEM_NAME} SYSTEM_NAME_LC) set(SDK_PACKAGE_EXT ".tgz") set(SDK_MAJOR_VERSION "3") @@ -29,23 +32,8 @@ FetchContent_Populate(CameraSDK) find_package(CameraSDK CONFIG REQUIRED HINTS ${CMAKE_CURRENT_BINARY_DIR}/CameraSDK) # Leave it to a copy for the GPU version, rather than fetching -#FetchContent_Declare(ImageSDK -# URL https://developer.phaseone.com/sdk/${SDK_MAJOR_VERSION}.${SDK_MINOR_VERSION}/releases/imagesdk/${SDK_MAJOR_VERSION}/p1imagesdk-${SYSTEM_NAME_LC}${LINUX_ARCH}${SDK_PACKAGE_EXT} -# SOURCE_DIR ImageSDK -#) -# -#message(STATUS "Downloading ImageSDK...") -# -#FetchContent_Populate(ImageSDK) - find_package(ImageSDK CONFIG REQUIRED HINTS lib/ImageSDKCuda) -## Compile as C++17, supported in ROS Noetic and newer -add_compile_options(-std=c++17) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(OpenCV REQUIRED COMPONENTS opencv_core @@ -54,30 +42,21 @@ find_package(OpenCV REQUIRED CONFIG ) -find_package(catkin REQUIRED COMPONENTS - nodelet - roscpp - std_msgs - std_srvs - sensor_msgs - custom_msgs - image_transport - cv_bridge - message_generation - roskv - diagnostic_updater -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(custom_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(cam_utils REQUIRED) +find_package(roskv REQUIRED) +find_package(rosidl_default_generators REQUIRED) -## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) -## Find CUDA find_package(CUDA QUIET) -if(CUDA_FOUND) - enable_language(CUDA) - message(STATUS "CUDA found: ${CUDA_VERSION}") - message(STATUS "CUDA libraries: ${CUDA_LIBRARIES}") -else() +if(NOT CUDA_FOUND) message(WARNING "CUDA not found - nvjpeg will not be available") endif() @@ -85,272 +64,69 @@ endif() if(CUDA_FOUND) find_library(NVJPEG_LIBRARY NAMES nvjpeg - PATHS - ${CUDA_TOOLKIT_ROOT_DIR}/lib64 - ${CUDA_TOOLKIT_ROOT_DIR}/lib - /usr/local/cuda/lib64 - /usr/local/cuda/lib - NO_DEFAULT_PATH + HINTS ${CUDA_TOOLKIT_ROOT_DIR}/lib64 ${CUDA_TOOLKIT_ROOT_DIR}/lib ) - if(NVJPEG_LIBRARY) - message(STATUS "nvjpeg library found: ${NVJPEG_LIBRARY}") set(NVJPEG_FOUND TRUE) + message(STATUS "nvjpeg library found: ${NVJPEG_LIBRARY}") else() - message(WARNING "nvjpeg library not found - nvjpeg encoding will not be available") set(NVJPEG_FOUND FALSE) + message(WARNING "nvjpeg library not found - nvjpeg encoding will not be available") endif() # Find nvjpeg header find_path(NVJPEG_INCLUDE_DIR NAMES nvjpeg.h - PATHS - ${CUDA_TOOLKIT_ROOT_DIR}/include - /usr/local/cuda/include - NO_DEFAULT_PATH + HINTS ${CUDA_TOOLKIT_ROOT_DIR}/include ) - if(NVJPEG_INCLUDE_DIR) message(STATUS "nvjpeg headers found: ${NVJPEG_INCLUDE_DIR}") add_definitions(-DHAVE_NVJPEG) else() message(WARNING "nvjpeg headers not found") - set(NVJPEG_FOUND FALSE) endif() -else() - set(NVJPEG_FOUND FALSE) endif() - -## 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 exec_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 exec_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 - SetPhaseOneParameter.srv - GetPhaseOneParameter.srv - GetCompressedImageView.srv - GetImageView.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 - sensor_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 exec_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 phase_one - DEPENDS OpenCV -# CATKIN_DEPENDS other_catkin_pkg +## Services +rosidl_generate_interfaces(${PROJECT_NAME} + srv/GetPhaseOneParameter.srv + srv/SetPhaseOneParameter.srv + srv/GetCompressedImageView.srv + srv/GetImageView.srv + DEPENDENCIES std_msgs sensor_msgs ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include ${Boost_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +include_directories(include ${Boost_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) if(NVJPEG_FOUND AND NVJPEG_INCLUDE_DIR) include_directories(${NVJPEG_INCLUDE_DIR} ${CUDA_INCLUDE_DIRS}) endif() -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/phase_one.cpp -# ) +add_executable(${PROJECT_NAME}_standalone src/phase_one_standalone.cpp) +ament_target_dependencies(${PROJECT_NAME}_standalone + rclcpp std_msgs sensor_msgs custom_msgs image_transport cv_bridge cam_utils roskv) -## 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}) -set(SRC - src/view_server_nodelet.cpp - src/phase_one_nodelet.cpp - src/phase_one_standalone.cpp - ) - -# build and install the nodelet - -add_library(${PROJECT_NAME} ${SRC}) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(phase_one PRIVATE +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp) +target_link_libraries(${PROJECT_NAME}_standalone + "${cpp_typesupport_target}" ${OpenCV_LIBS} - ${catkin_LIBRARIES} CameraSDK::CameraSdkCpp ImageSDK::ImageSdkCpp ) if(NVJPEG_FOUND) - target_link_libraries(phase_one PRIVATE + target_link_libraries(${PROJECT_NAME}_standalone ${NVJPEG_LIBRARY} ${CUDA_LIBRARIES} ${CUDA_CUDART_LIBRARY} ) endif() -## 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/phase_one_node.cpp) -add_executable(${PROJECT_NAME}_standalone src/phase_one_standalone.cpp src/phase_one_utils.cpp) - -target_link_libraries(phase_one_node PRIVATE - ${catkin_LIBRARIES} -) - -target_link_libraries(phase_one_standalone PRIVATE - ${OpenCV_LIBS} - ${catkin_LIBRARIES} - CameraSDK::CameraSdkCpp - ImageSDK::ImageSdkCpp -) -if(NVJPEG_FOUND) - target_link_libraries(phase_one_standalone PRIVATE - ${NVJPEG_LIBRARY} - ${CUDA_LIBRARIES} - ${CUDA_CUDART_LIBRARY} - ) -endif() - -## 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}) -# 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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_phase_one.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(TARGETS ${PROJECT_NAME}_standalone + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.launch.xml") +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/cams/phase_one/include/phase_one/phase_one.h b/src/cams/phase_one/include/phase_one/phase_one.h index 392ccab3..f841a2dc 100644 --- a/src/cams/phase_one/include/phase_one/phase_one.h +++ b/src/cams/phase_one/include/phase_one/phase_one.h @@ -12,33 +12,42 @@ #include #include #include +#include #include -#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// rclcpp logging shims to keep ROS1-style call sites +#define ROS_INFO(...) RCLCPP_INFO(rclcpp::get_logger("phase_one"), __VA_ARGS__) +#define ROS_WARN(...) RCLCPP_WARN(rclcpp::get_logger("phase_one"), __VA_ARGS__) +#define ROS_ERROR(...) RCLCPP_ERROR(rclcpp::get_logger("phase_one"), __VA_ARGS__) +#define ROS_INFO_STREAM(args) RCLCPP_INFO_STREAM(rclcpp::get_logger("phase_one"), args) +#define ROS_WARN_STREAM(args) RCLCPP_WARN_STREAM(rclcpp::get_logger("phase_one"), args) +#define ROS_ERROR_STREAM(args) RCLCPP_ERROR_STREAM(rclcpp::get_logger("phase_one"), args) namespace phase_one @@ -53,8 +62,8 @@ namespace phase_one // Shutdown threads and camera safely ~PhaseOne(); - // Init call if thread-based - void init(); + // Init call; the node owns all ROS interfaces + void init(rclcpp::Node::SharedPtr node); // Definition of init call, connect to camera, instantiate threads virtual void onInit(); @@ -75,7 +84,7 @@ namespace phase_one std::string format); // Compress JPEG using nvjpeg (GPU-accelerated) - bool compressJpegNvjpeg(const cv::Mat& bgr_image, + bool compressJpegNvjpeg(const cv::Mat& bgr_image, std::vector& output, int quality = 90); @@ -88,31 +97,31 @@ namespace phase_one // ROS service call, grabs a parameter or lists of parameters from the camera // and returns the string values - bool getPhaseOneParameter(phase_one::GetPhaseOneParameter::Request& req, - phase_one::GetPhaseOneParameter::Response& resp); + void getPhaseOneParameter(const std::shared_ptr req, + std::shared_ptr resp); // ROS service call, sets the list of param=value calls requested on // the camera - bool setPhaseOneParameter(phase_one::SetPhaseOneParameter::Request& req, - phase_one::SetPhaseOneParameter::Response& resp); + void setPhaseOneParameter(const std::shared_ptr req, + std::shared_ptr resp); // ROS service call, given a homography, return the compressed image chip // of that warp - bool getCompressedImageView(phase_one::GetCompressedImageView::Request& req, - phase_one::GetCompressedImageView::Response& resp); + void getCompressedImageView(const std::shared_ptr req, + std::shared_ptr resp); // ROS service call, given a homography, return the raw image chip of that // warp - bool getImageView(custom_msgs::RequestImageView::Request& req, - custom_msgs::RequestImageView::Response& resp); + void getImageView(const std::shared_ptr req, + std::shared_ptr resp); // ROS subscriber, listens for "event" messages published from the INS, and // when received, adds those to the current EventCache - void eventCallback (const boost::shared_ptr& msg); + void eventCallback (const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg); // ROS subscriber, listens for "detection list" messages published from the detector, // and when received, adds these to the detection cache - void detectionListCallback (const boost::shared_ptr& msg); + void detectionListCallback (const custom_msgs::msg::ImageSpaceDetectionList::ConstSharedPtr& msg); private: // Phase One P1::CameraSdk::Camera camera; @@ -121,16 +130,17 @@ namespace phase_one P1::ImageSdk::JpegConfig jpegConfig; P1::CameraSdk::Listener listener; // ROS - ros::ServiceServer image_view_service_; - ros::ServiceServer compressed_image_view_service_; - ros::ServiceServer get_param_service_; - ros::ServiceServer set_param_service_; - ros::Subscriber event_sub_; - ros::Subscriber detection_sub_; + rclcpp::Node::SharedPtr node_; + rclcpp::Service::SharedPtr image_view_service_; + rclcpp::Service::SharedPtr compressed_image_view_service_; + rclcpp::Service::SharedPtr get_param_service_; + rclcpp::Service::SharedPtr set_param_service_; + rclcpp::Subscription::SharedPtr event_sub_; + rclcpp::Subscription::SharedPtr detection_sub_; image_transport::Publisher image_pub; - ros::Publisher stat_pub_; + rclcpp::Publisher::SharedPtr stat_pub_; cv_bridge::CvImage img_bridge; - ros::Time frame_recv_time_; + rclcpp::Time frame_recv_time_; // ROS params std::string ip_address_; std::string trigger_mode_; @@ -172,14 +182,12 @@ namespace phase_one // custom ArchiverOpts arch_opts_ = ArchiverOpts::from_env(); std::shared_ptr envoy_; - custom_msgs::GSOF_EVT event_; // store the last received event + custom_msgs::msg::GsofEvt event_; // store the last received event // Holds events from the INS in a map to be searched for and matched // to incoming images EventCache event_cache; - // Debugger output - diagnostic_updater::Updater updater; }; } -#endif //PHASE_ONE_UTILS_H +#endif //PHASE_ONE_H diff --git a/src/cams/phase_one/include/phase_one/phase_one_utils.h b/src/cams/phase_one/include/phase_one/phase_one_utils.h deleted file mode 100644 index 42474d53..00000000 --- a/src/cams/phase_one/include/phase_one/phase_one_utils.h +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once -#ifndef PHASE_ONE_UTILS_H -#define PHASE_ONE_UTILS_H -#include -#include -#include -// ROS -#include -// Custom -#include - - -const ros::Duration ZERO_DURATION(0); -// Used when you need something to trigger immediately, -// but where a truly zero duration may do weird things, e.g. div/0 -const ros::Duration ALMOST_INSTANT{0, 10}; -const ros::Duration MICROSECOND{0, 1000}; - -// Get the absolute value of a ROS duration -ros::Duration rosabs(ros::Duration dur); - -/* This class holds the events published from the INS upon each trigger, - indexed by the system time that event was published. When an image message - is received, this cache is `searched` for the closest event matching the - system time of the image received. These are then fused, and the header - of the image is changed to match the GPS time of the event, and the image - is saved under that GPS time. -*/ -class EventCache { -public: - // specifying some sane defaults - EventCache(); - EventCache(ros::Duration tol, ros::Duration delay); - - // Set the maximum amount of time allowed between an event message - // time received and an image message time received to allow a match - // between the 2. Allows for slop in estimate of delay - void set_tolerance(ros::Duration const &tolerance); - - // Set the expected delay between an image trigger and the time it is - // actually received (including exposure, network transfer, etc.). - // Close to 0 for small images (e.g. IR), up to a second for longer - // exposure large imagery (e.g. Phase One) - void set_delay(double delay); - - void set_delay(ros::Duration const &delay); - - // Insert event message `msg` at time `t` into this map. - void push_back(ros::Time const &t, const boost::shared_ptr& msg); - - // Return size of this cache - int size(); - - // Print out all headers in this cache - void show(); - - // Search this cache for an event closest to `image_time`, the system - // time the image was received. - // If found, change the header `head` to the GPS time of the event and - // return true. If `remove_when_found`, delete cached event upon - // a successful find. - bool search(ros::Time image_time, std_msgs::Header &head, bool remove_when_found); - - bool search(ros::Time image_time, std_msgs::Header &head) { - return search(image_time, head, true); - } - - // Remove all events older than `stale_time` from this cache. - void purge(); - - private: - // Tolerance allowed in the expected delay - ros::Duration tol; - // Expected delay from event to image - ros::Duration delay; - // Set time to remove messages older than when `purge` is called - ros::Duration stale_time{5}; - // Lock for thread safety - std::mutex mutex_; - // Data structure containing all event messages mapped to their - // sys_time (time they were published) - std::map event_map; -}; - - -// This function takes in a ROS standard list of params, delimited by: -// param1=val1,param2=val2,param3=val3,...,paramN=valN -// and returns a map of {param1: val1, param2: val2, etc.} -std::map parseParams(std::string parameters); - -// Takes in a filename, returns a vector of strings, each one mapping -// to a line in the given file. -std::vector loadFile(std::string filename); - - -#endif //PHASE_ONE_UTILS_H \ No newline at end of file diff --git a/src/cams/phase_one/launch/phase_one.launch b/src/cams/phase_one/launch/phase_one.launch deleted file mode 100644 index 1c494df9..00000000 --- a/src/cams/phase_one/launch/phase_one.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - diff --git a/src/cams/phase_one/launch/phase_one_standalone.launch b/src/cams/phase_one/launch/phase_one_standalone.launch deleted file mode 100644 index 5d4760c5..00000000 --- a/src/cams/phase_one/launch/phase_one_standalone.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/src/cams/phase_one/launch/phase_one_standalone.launch.xml b/src/cams/phase_one/launch/phase_one_standalone.launch.xml new file mode 100644 index 00000000..c8d19517 --- /dev/null +++ b/src/cams/phase_one/launch/phase_one_standalone.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/cams/phase_one/nodelet_plugins.xml b/src/cams/phase_one/nodelet_plugins.xml deleted file mode 100644 index 4fc39eb7..00000000 --- a/src/cams/phase_one/nodelet_plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - Nodelet for interfacing with PhaseOne cameras. - - - - - Nodelet for displaying parts of images. - - - diff --git a/src/cams/phase_one/package.xml b/src/cams/phase_one/package.xml index a65e3726..b41531fb 100644 --- a/src/cams/phase_one/package.xml +++ b/src/cams/phase_one/package.xml @@ -1,66 +1,31 @@ - + + phase_one - 0.0.0 - The phase_one package + 1.0.0 + Phase One iXM RGB camera driver (CameraSDK/ImageSDK based) - - - Adam Romlein Apache 2.0 + ament_cmake + rosidl_default_generators - - - - + rclcpp + std_msgs + sensor_msgs + custom_msgs + image_transport + cv_bridge + cam_utils + roskv + libopencv-dev + rosidl_default_runtime - - - - + rosidl_interface_packages - - - - - - - - - - - - - - - - - - - - - - - nodelet - roskv - custom_msgs - message_generation - nodelet - custom_msgs - roskv - message_runtime - - - catkin - - - - - - + ament_cmake diff --git a/src/cams/phase_one/setup.py b/src/cams/phase_one/setup.py deleted file mode 100644 index ac7f7e8c..00000000 --- a/src/cams/phase_one/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python -from distutils.core import setup - -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup(packages=['phase_one'], - package_dir={'': 'src'}) - - -setup(**d) diff --git a/src/cams/phase_one/src/phase_one_node.cpp b/src/cams/phase_one/src/phase_one_node.cpp deleted file mode 100755 index ca6fc80c..00000000 --- a/src/cams/phase_one/src/phase_one_node.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "phase_one"); - - nodelet::Loader manager(false); - nodelet::M_string remappings(ros::names::getRemappings()); - nodelet::V_string my_argv(argv + 1, argv + argc); - - manager.load(ros::this_node::getName(), "phase_one", remappings, my_argv); - - ros::spin(); - return 0; -} diff --git a/src/cams/phase_one/src/phase_one_nodelet.cpp b/src/cams/phase_one/src/phase_one_nodelet.cpp deleted file mode 100755 index 4d9f1d89..00000000 --- a/src/cams/phase_one/src/phase_one_nodelet.cpp +++ /dev/null @@ -1,451 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace P1::CameraSdk; -using namespace P1::ImageSdk; - -using std::cout; -using std::endl; - -namespace phase_one -{ - class PhaseOneNodelet : public nodelet::Nodelet - { - public: - PhaseOneNodelet() - { - } - - private: - bool running_ = false; - int counter = 0; - std::thread capture_thread_; - image_transport::Publisher image_pub; - cv_bridge::CvImage img_bridge; - P1::CameraSdk::Camera camera; - P1::ImageSdk::DecodeConfig decodeConfig; - P1::ImageSdk::ConvertConfig convertConfig; - P1::ImageSdk::JpegConfig jpegConfig; - P1::CameraSdk::Listener listener; - ros::ServiceServer get_param_service_; - ros::ServiceServer set_param_service_; - std::map property_to_id_; - std::map property_to_type_; - - - ~PhaseOneNodelet() { - ROS_INFO("Phase One Camera: Shutting down"); - // signal running_ threads and wait until they finish - running_ = false; - if (capture_thread_.joinable()) - { - capture_thread_.join(); - } - // Disable image receiving - camera.Close(); - }; - - virtual void onInit() { - ROS_INFO("PhaseOne Driver: Initialization"); - // ROS initialization - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - image_transport::ImageTransport it(nh); - image_pub = it.advertise("image_raw", 1); - - // Phase one Initialization - P1::ImageSdk::Initialize(); - P1::ImageSdk::SetSensorProfilesLocation( - "/home/user/phaseOne/phase_one_ws/build/phase_one/ImageSDK/SensorProfiles"); - get_param_service_ = pnh.advertiseService("get_phaseone_parameter", - &PhaseOneNodelet::getPhaseOneParameter, this); - set_param_service_ = pnh.advertiseService("set_phaseone_parameter", - &PhaseOneNodelet::setPhaseOneParameter, this); - connectToIPCamera("192.168.1.6"); - auto list = camera.AllPropertyIds(); - for (auto propertyId : list) { - P1::CameraSdk::PropertySpecification property = camera.PropertySpec(propertyId); - std::string name = property.mName; - auto val = camera.Property(propertyId); - std::cout << name << " | " << val.ToString() << "\n"; - } - //connectToUSBCamera(); - getPropertyMaps(); - - running_ = true; - //config.SetCrop(5000, 5000, 1920, 1080); - //config.SetOutputHeight(0.1) - capture_thread_ = std::thread(&PhaseOneNodelet::capture, this); - }; - - int connectToUSBCamera() { - cout << "Probing for available USB cameras..." << endl; - std::vector list; - try { - list = Camera::AvailableCameras(); - for(auto cam : list) - { - ROS_INFO_STREAM(" * " << cam.mName << " (" - << cam.mSerialNum << ")"); - } - } - catch (P1::ImageSdk::SdkException exception) - { - // Exception from ImageSDK - ROS_ERROR_STREAM("ImageSDK Exception: " << exception.what() - << " Code:" << exception.mCode); - return -1; - } - catch (P1::CameraSdk::SdkException exception) - { - // Exception from CameraSDK - std::cout << "CameraSDK Exception: " << exception.what() << " Code:" - << exception.mErrorCode << - std::endl; - return -1; - } - catch (...) - { - // Any other exception - just in case - std::cout << "Argh - we got an exception" << std::endl; - return -1; - } - - if (list.size() <= 0) - { - cout << "Could not find any USB cameras!" << endl; - return -1; - } else { - ROS_INFO_STREAM("Found " << list.size() << " camera" - << (list.size() == 1 ? "" : "s")); - this->camera = P1::CameraSdk::Camera::OpenUsbCamera(); - return 0; - } - }; - - int connectToIPCamera(std::string ip) { - ROS_INFO_STREAM("Attempting to connect to IP connected camera at " << ip << "..."); - try { - this->camera = P1::CameraSdk::Camera::OpenIpCamera(ip); - } - catch (P1::ImageSdk::SdkException exception) - { - // Exception from ImageSDK - std::cout << "ImageSDK Exception: " << exception.what() << " Code:" << exception.mCode << - std::endl; - return -1; - } - catch (P1::CameraSdk::SdkException exception) - { - // Exception from CameraSDK - std::cout << "CameraSDK Exception: " << exception.what() << " Code:" - << exception.mErrorCode << - std::endl; - return -1; - } - catch (...) - { - // Any other exception - just in case - std::cout << "Argh - we got an exception" << std::endl; - return -1; - } - - ROS_INFO("IP Camera connected!"); - return 0; - }; - - int capture() { - ROS_INFO("Capture thread started"); - P1::CameraSdk::Listener imgListener; - imgListener.EnableNotification(camera, - P1::CameraSdk::EventType::CameraImageReady); - camera.Subscriptions()->FullImages()->Subscribe(); - // Necessary to sleep, otherwise TriggerCapture will start before EnableImageReceiving is done - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - try { - while (running_) { - auto tic1 = std::chrono::high_resolution_clock::now(); - ROS_INFO("Triggering camera capture"); - camera.TriggerCapture(); - - ROS_INFO("Waiting for image from camera..."); - // Wait for image - P1::CameraSdk::NotificationEventPtr event = - imgListener.WaitForNotification(10000); - ROS_INFO("Image received!"); - auto iiqImage = event->FullImage(); - - // Write RGB bitmap to file - //std::fstream iiqFile("bits.iiq", std::ios::binary | std::ios::trunc | - // std::ios::out); - //iiqFile.write((char*)imageFile.Data.get(), imageFile.size); - //iiqFile.close(); - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic1; - ROS_INFO_STREAM("Time to fetch image off camera was: " << dt.count() / 1e9 << "s"); - - - auto tic2 = std::chrono::high_resolution_clock::now(); - //P1::ImageSdk::RawImage new_image("bits.iiq"); - P1::ImageSdk::RawImage image(iiqImage->Data(), - iiqImage->DataSizeBytes()); - //P1::ImageSdk::SensorBayerOutput decodedImage = - // image.Decode(decodeConfig); - //P1::ImageSdk::SensorBayerOutput decodedImage = - // image.Decode(decodeConfig); - //std::cout << "Decoded image size: " << decodedImage.ByteSize() - // << " Height=" << decodedImage.FullHeight() - // << " Width=" << decodedImage.FullWidth() << std::endl; - // convertConfig.WhiteBalanceGainRange - P1::ImageSdk::BitmapImage bitmap = convertConfig.ApplyTo(image); - toc = std::chrono::high_resolution_clock::now(); - dt = toc - tic2; - ROS_INFO_STREAM("Time to demosaic iiq to raw was: " - << dt.count() / 1e9 << "s"); - - ROS_INFO_STREAM("Received Image Height=" << bitmap.Height() << " Width= " - << bitmap.Width() << "Size= " << bitmap.ByteSize() << std::endl); - - // Toss raw image in cv mat - auto tic3 = std::chrono::high_resolution_clock::now(); - //cv::Mat cvImage_raw = cv::Mat(cv::Size( - // bitmap.Width(), bitmap.Height()), CV_16UC3); - //cout << "Dumping data into mat.\n"; - //cvImage_raw.data = bitmap.Data().get(); - //cv::Mat cvImage = cv::Mat(cv::Size( - // bitmap.Width(), bitmap.Height()), CV_8UC3); - //cout << "Convert data into mat.\n"; - //cvImage_raw.convertTo(cvImage, CV_8UC3, 1/256.0); - //cv::cvtColor(cvImage_raw, cvImage, cv::COLOR_BGR2RGB); - ROS_INFO_STREAM("Dumping image."); - bool is_archiving = true; - if (is_archiving == true) { - std::string fname = dumpImage(image, bitmap, "/home/user/test.jpg"); - } - toc = std::chrono::high_resolution_clock::now(); - dt = toc - tic3; - ROS_INFO_STREAM("Time to compress image to jpeg and write to disk was: " - << dt.count() / 1e9 << "s"); - - P1::ImageSdk::TagId ids; - P1::ImageSdk::ImageTag tag = image.GetTag(ids.DateTime); - ROS_INFO_STREAM("Tag: " << tag.ToString()); - - // Create a shared pointer for intra-process zero-copy - //sensor_msgs::ImagePtr img_msg(new sensor_msgs::Image); - //std_msgs::Header header; // empty header - //header.seq = counter; // user defined counter - //header.stamp = ros::Time::now(); // time - //img_bridge = cv_bridge::CvImage(header, - // sensor_msgs::image_encodings::RGB8, cvImage_raw); - //img_bridge.toImageMsg(*img_msg); - //image_pub.publish(img_msg); - //toc = std::chrono::high_resolution_clock::now(); - //dt = toc - tic3; - //ROS_INFO_STREAM("Time to compress image to jpeg and publish was: " - // << dt.count() / 1e9 << "s"); - //return 0; - //counter++; - toc = std::chrono::high_resolution_clock::now(); - dt = toc - tic1; - ROS_INFO_STREAM("Time to for total call of camera trigger " - "to disk: " << dt.count() / 1e9 << "s"); - - } - } - catch (P1::ImageSdk::SdkException exception) - { - // Exception from ImageSDK - ROS_ERROR_STREAM("ImageSDK Exception: " << exception.what() << " Code:" - << exception.mCode << std::endl); - return -1; - } - catch (P1::CameraSdk::SdkException exception) - { - // Exception from CameraSDK - ROS_ERROR_STREAM("CameraSDK Exception: " << exception.what() << " Code:" - << exception.mErrorCode << std::endl); - running_ = false; - return -1; - } - catch (...) - { - // Any other exception - just in case - std::cout << "Argh - we got an exception" << std::endl; - return -1; - } - return 0; - }; - - std::string dumpImage(P1::ImageSdk::RawImage rawImage, - P1::ImageSdk::BitmapImage bitmap, - const std::string &filename) - { - auto start_db = ros::Time::now(); - boost::filesystem::path path_filename{filename}; - boost::filesystem::create_directories(path_filename.parent_path()); - ROS_INFO("Writing image."); - jpegConfig.quality = 80; - P1::ImageSdk::JpegWriter(filename, bitmap, rawImage, jpegConfig); - //P1::ImageSdk::TiffConfig tiffConfig; - //P1::ImageSdk::TiffWriter(filename, bitmap, rawImage, tiffConfig); - return filename; - } - - void getPropertyMaps() { - ROS_INFO("Grabbing properties from camera."); - std::vector propertyIdList = camera.AllPropertyIds(); - for (int propertyId : propertyIdList) - { - // Get the property specification for the current propertyId - P1::CameraSdk::PropertySpecification property = camera.PropertySpec(propertyId); - std::string name = property.mName; - property_to_id_[name] = propertyId; - property_to_type_[name] = property.mValue; - } - }; - - bool getPhaseOneParameter(phase_one::GetPhaseOneParameter::Request& req, - phase_one::GetPhaseOneParameter::Response& resp) - { - if (property_to_id_.size() > 0) - { - try - { - std::string name = req.name.c_str(); - int id = property_to_id_[ name ]; - P1::CameraSdk::PropertyValue pv = camera.Property(id); - resp.value = pv.ToString(); - resp.message = "ok"; - } - catch (const std::exception& ex) - { - ROS_ERROR_STREAM("Cannot get parameter: " << ex.what()); - resp.message = ex.what(); - } - } - - return true; - - }; - - bool setPhaseOneParameter(phase_one::SetPhaseOneParameter::Request& req, - phase_one::SetPhaseOneParameter::Response& resp) - { - ROS_INFO("Received Request to Set Parameter(s)."); - if (property_to_id_.size() > 0) - { - try - { - std::string parameters = req.parameters.c_str(); - std::map name_to_value = - parseParams(parameters); - - for (auto const& it: name_to_value) { - std::string name = it.first; - std::string val = it.second; - int id = property_to_id_[ name ]; - auto cam_val = camera.Property(id); - ROS_INFO_STREAM("Previous value: " << cam_val.ToString()); - ROS_INFO_STREAM("name: " << name << " value: " - << val << " id: " << id << "\n"); - std::string s1 = property_to_type_[ name ].ToString(); - if (s1.find("Enum") != std::string::npos || - s1.find("Bool") != std::string::npos || - s1.find("Int") != std::string::npos) { - P1::CameraSdk::PropertyValue pv; - pv.mType = property_to_type_[name].mType; - pv.mInt = std::stoi(val); - ROS_INFO_STREAM("Setting Int property: " << name << - " : to value : " << val << " :\n"); - camera.SetProperty(id, pv); - } else if (s1.find("Float") != std::string::npos) { - P1::CameraSdk::PropertyValue pv; - pv.mType = property_to_type_[name].mType; - ROS_INFO_STREAM("Type: " << property_to_type_[name].mType << " Value: " - << property_to_type_[name].ToString()); - pv.mDouble = std::stod(val); - ROS_INFO_STREAM("Setting Double property: " << name << - " : to value : " << val << " :\n"); - camera.SetProperty(id, pv); - } else { - P1::CameraSdk::PropertyValue pv; - pv.mType = property_to_type_[name].mType; - pv.mString = val; - ROS_INFO_STREAM("Setting String property: " << name << - " : to value : " << val << " :\n"); - camera.SetProperty(id, pv); - } - cam_val = camera.Property(id); - ROS_INFO_STREAM("New value: " << cam_val.ToString()); - } - } - catch (const std::exception& ex) - { - ROS_ERROR_STREAM("Cannot set parameter: " << ex.what()); - resp.message = ex.what(); - return false; - } - resp.message = "ok"; - } - return true; - }; - - std::map parseParams(std::string parameters) { - // Iterate through parameters organized by name=value, separated by - // commas. Return a map of parameters to values. - std::map param_to_value; - std::string delimiter1 = ","; - std::string delimiter2 = "="; - size_t pos = 0; - std::string token; - std::string name; - std::string value; - // Always run at least once even if there's no delimiter in request - while ((pos = parameters.find(delimiter1)) != std::string::npos) { - token = parameters.substr(0, pos); - name = token.substr(0, token.find(delimiter2)); - token.erase(0, token.find(delimiter2) + delimiter2.length()); - value = token; - std::cout << name << std::endl; - std::cout << value << std::endl; - param_to_value[name] = value; - parameters.erase(0, pos + delimiter1.length()); - } - token = parameters; - name = token.substr(0, token.find(delimiter2)); - token.erase(0, token.find(delimiter2) + delimiter2.length()); - value = token; - std::cout << name << std::endl; - std::cout << value << std::endl; - param_to_value[name] = value; - - return param_to_value; - }; - }; - -PLUGINLIB_EXPORT_CLASS(phase_one::PhaseOneNodelet, nodelet::Nodelet); -} diff --git a/src/cams/phase_one/src/phase_one_standalone.cpp b/src/cams/phase_one/src/phase_one_standalone.cpp index 704238e7..ef85a708 100755 --- a/src/cams/phase_one/src/phase_one_standalone.cpp +++ b/src/cams/phase_one/src/phase_one_standalone.cpp @@ -10,36 +10,33 @@ #include #include #include -#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include - -#include -#include -#include -#include -#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include #include #ifdef HAVE_NVJPEG @@ -48,9 +45,9 @@ #include #endif -#ifdef HAVE_NVJPEG namespace phase_one { +#ifdef HAVE_NVJPEG // Helper function to convert nvjpeg status to string const char* nvjpeg_error_string(nvjpegStatus_t s) { switch (s) { @@ -238,8 +235,9 @@ namespace phase_one { } - void PhaseOne::init() + void PhaseOne::init(rclcpp::Node::SharedPtr node) { + node_ = node; PhaseOne::onInit(); } @@ -262,20 +260,17 @@ namespace phase_one void PhaseOne::onInit() { ROS_INFO("PhaseOne Driver: Initialization"); // ROS initialization - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - image_transport::ImageTransport it(nh); - image_pub = it.advertise("image_raw", 1, true); - stat_pub_ = nh.advertise("/stat", 3); + image_pub = image_transport::create_publisher(node_.get(), "image_raw"); + stat_pub_ = node_->create_publisher("/stat", 3); ROS_INFO("Loading ROS parameters."); - pnh.param("ip_address", ip_address_, ""); - pnh.param("num_threads", num_threads_, 28); - pnh.param("trigger_mode", trigger_mode_, "manual"); //manual, auto - pnh.param("auto_trigger_rate", auto_trigger_rate_, 1.0); - pnh.param("cam_chan", cam_channel_, "nochan"); - pnh.param("cam_fov", cam_fov_, "nofov"); - pnh.param("hostname", hostname, "casX"); + ip_address_ = node_->declare_parameter("ip_address", std::string("")); + num_threads_ = node_->declare_parameter("num_threads", 28); + trigger_mode_ = node_->declare_parameter("trigger_mode", std::string("manual")); //manual, auto + auto_trigger_rate_ = node_->declare_parameter("auto_trigger_rate", 1.0); + cam_channel_ = node_->declare_parameter("cam_chan", std::string("nochan")); + cam_fov_ = node_->declare_parameter("cam_fov", std::string("nofov")); + hostname = node_->declare_parameter("hostname", std::string("casX")); // connect to redis ROS_INFO("Cameratype: %s/%s", cam_fov_.c_str(), cam_channel_.c_str()); @@ -316,7 +311,7 @@ namespace phase_one static double min_image_delay = 0.95; // should depend on exposure time event_cache.set_delay(min_image_delay); - event_cache.set_tolerance(ros::Duration(0.49)); + event_cache.set_tolerance(rclcpp::Duration::from_seconds(0.49)); to_process_filename_ = base_dir + "/to_process.txt"; processed_filename_ = base_dir + "/processed.txt"; std::vector to_process_images = loadFile(to_process_filename_); @@ -372,19 +367,43 @@ namespace phase_one capture_thread_ = std::thread(&PhaseOne::capture, this); demosaic_thread_ = std::thread(&PhaseOne::demosaic, this); - get_param_service_ = pnh.advertiseService("get_phaseone_parameter", - &PhaseOne::getPhaseOneParameter, this); - set_param_service_ = pnh.advertiseService("set_phaseone_parameter", - &PhaseOne::setPhaseOneParameter, this); - image_view_service_ = pnh.advertiseService("get_image_view", - &PhaseOne::getImageView, this); - compressed_image_view_service_ = pnh.advertiseService("get_compressed_image_view", - &PhaseOne::getCompressedImageView, this); + // "~/" services resolve under the node's namespace + name, matching the + // ROS1 private-nodehandle layout the GUI and cam_param_monitor call + get_param_service_ = node_->create_service( + "~/get_phaseone_parameter", + [this](const std::shared_ptr req, + std::shared_ptr resp) { + getPhaseOneParameter(req, resp); + }); + set_param_service_ = node_->create_service( + "~/set_phaseone_parameter", + [this](const std::shared_ptr req, + std::shared_ptr resp) { + setPhaseOneParameter(req, resp); + }); + image_view_service_ = node_->create_service( + "~/get_image_view", + [this](const std::shared_ptr req, + std::shared_ptr resp) { + getImageView(req, resp); + }); + compressed_image_view_service_ = node_->create_service( + "~/get_compressed_image_view", + [this](const std::shared_ptr req, + std::shared_ptr resp) { + getCompressedImageView(req, resp); + }); // Subscribers - event_sub_ = pnh.subscribe("/event", 1, &PhaseOne::eventCallback, this); + event_sub_ = node_->create_subscription( + "/event", 1, + [this](const custom_msgs::msg::GsofEvt::ConstSharedPtr msg) { eventCallback(msg); }); std::string det_topic = "/" + hostname + "/detections"; - detection_sub_ = pnh.subscribe(det_topic, 1, &PhaseOne::detectionListCallback, this); + detection_sub_ = node_->create_subscription( + det_topic, 1, + [this](const custom_msgs::msg::ImageSpaceDetectionList::ConstSharedPtr msg) { + detectionListCallback(msg); + }); }; int PhaseOne::connectToIPCamera(std::string ip) { @@ -451,9 +470,9 @@ namespace phase_one // Create stat msg auto nodeName = "/" + hostname + "/rgb/rgb_driver"; - custom_msgs::Stat stat_msg; + custom_msgs::msg::Stat stat_msg; std::stringstream link; - stat_msg.header.stamp = ros::Time::now(); + stat_msg.header.stamp = node_->now(); stat_msg.trace_topic = nodeName + "/publishImage"; stat_msg.node = nodeName; @@ -467,9 +486,9 @@ namespace phase_one ROS_INFO_STREAM("|CAPTURE| wait time: " << wait_dt.count() / 1e9 << "s"); std::shared_ptr iiqImage; P1::ImageSdk::RawImage image; - ros::Time frame_recv_time_; + rclcpp::Time frame_recv_time_; if ( event && event->Type().id == P1::CameraSdk::EventType::CameraImageReady.id ) { - frame_recv_time_ = ros::Time::now(); + frame_recv_time_ = node_->now(); ROS_INFO("|CAPTURE| image received!"); iiqImage = event->FullImage(); P1::ImageSdk::RawImage image(iiqImage->Data(), @@ -487,25 +506,21 @@ namespace phase_one // mutex locks goes out of scope, should release } else { ROS_WARN("|CAPTURE| No image received within timeout!"); - //running_ = false; timed_out = true; break; - //continue; } - auto tic3 = std::chrono::high_resolution_clock::now(); // Sync event cache - std_msgs::Header gps_header; + std_msgs::msg::Header gps_header; + uint64_t event_num = 0; ROS_INFO("|CAPTURE| Searching event cache."); - bool success = event_cache.search(frame_recv_time_, gps_header); + bool success = event_cache.search(frame_recv_time_, gps_header, event_num); if (success != true) { ROS_WARN("|CAPTURE| Could not find event message to associate image to! Skipping."); - // TODO: sub in current time? seems dangerous - //gps_header.stamp = ros::Time::now(); continue; } else { ROS_WARN("|CAPTURE| Found matching event!"); - link << nodeName << "/event/" << gps_header.seq; // link this trace to the event trace + link << nodeName << "/event/" << event_num; // link this trace to the event trace stat_msg.link = link.str(); stat_msg.note = "success"; } @@ -514,17 +529,16 @@ namespace phase_one if (is_archiving) { // Write raw iiq to file long int sec = gps_header.stamp.sec; - long int nsec = gps_header.stamp.nsec; - int seq = gps_header.seq; + long int nsec = gps_header.stamp.nanosec; + int seq = (int) event_num; std::string fname = ArchiverHelper::generateFilename(envoy_, arch_opts_, sec, nsec); // Adds an additional dir layer for raw images fname.insert(fname.find(base_dir) + base_dir.size(), "/iiq_buffer"); - //std::string fname = "/mnt/data/test/" + iiqImage->FileName(); fname.replace(fname.find("jpg"), 3, "IIQ"); // Replaces "jpg" with "IIQ" ROS_INFO_STREAM("|CAPTURE| Write file: " << fname); - boost::filesystem::path path_fname{fname}; + std::filesystem::path path_fname{fname}; try { - boost::filesystem::create_directories(path_fname.parent_path()); + std::filesystem::create_directories(path_fname.parent_path()); std::fstream iiqFile(fname, std::ios::binary | std::ios::trunc | std::ios::out); iiqFile.write((char*)iiqImage->Data().get(), iiqImage->DataSizeBytes()); iiqFile.close(); @@ -552,27 +566,22 @@ namespace phase_one total_counter++; envoy_->put("/sys/" + hostname + "/p1debayerq/total", std::to_string(total_counter)); - } catch (boost::filesystem::filesystem_error &e) { + } catch (std::filesystem::filesystem_error &e) { ROS_ERROR("|CAPTURE| Archive Failed [%d]: %s", e.code().value(), e.what()); } catch (const std::ios_base::failure& e) { ROS_ERROR("|CAPTURE| I/O error writing IIQ file: %s", e.what()); } } - //P1::ImageSdk::ImageTag tag = image.GetTag(ids.DateTime); - //ROS_INFO_STREAM("Tag: " << tag.ToString()); // seems to want to take from queue, rather than straight `image` P1::ImageSdk::RawImage raw_img; raw_img = image_q_.front(); P1::ImageSdk::BitmapImage preview = raw_img.GetPreview(); cv::Mat cvImage_raw = cv::Mat(cv::Size(preview.Width(), preview.Height()), CV_8UC3); cvImage_raw.data = preview.Data().get(); - sensor_msgs::Image output_msg; + sensor_msgs::msg::Image output_msg; try { - std_msgs::Header header; - //P1::ImageSdk::TagId ids; - //header.seq = tag.Value(); - header.seq = gps_header.seq; + std_msgs::msg::Header header; header.stamp = gps_header.stamp; img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, cvImage_raw); @@ -617,10 +626,7 @@ namespace phase_one output_msg.header.frame_id = frame_id.str(); image_pub.publish(output_msg); - stat_pub_.publish(stat_msg); - //long int sec = output_msg.header.stamp.sec; - //long int nsec = output_msg.header.stamp.nsec; - //std::string fname = ArchiverHelper::generateFilename(envoy_, arch_opts_, sec, nsec); + stat_pub_->publish(stat_msg); auto toc = std::chrono::high_resolution_clock::now(); auto dt = toc - tic1; @@ -642,6 +648,7 @@ namespace phase_one camera.Close(); int ret = connectToIPCamera(ip_address_); ret = capture(); + (void) ret; } }// catch-all for exceptions during capture catch (P1::ImageSdk::SdkException exception) @@ -838,6 +845,7 @@ namespace phase_one return false; } #else + (void) bgr_image; (void) output; (void) quality; ROS_ERROR("compressJpegNvjpeg: nvjpeg not available (compiled without HAVE_NVJPEG)"); return false; #endif @@ -848,10 +856,9 @@ namespace phase_one const std::string &filename, std::string format) { - auto start_db = ros::Time::now(); try { - boost::filesystem::path path_filename{filename}; - boost::filesystem::create_directories(path_filename.parent_path()); + std::filesystem::path path_filename{filename}; + std::filesystem::create_directories(path_filename.parent_path()); if (format == "jpg") { int quality = std::stoi(envoy_->get("/sys/arch/jpg/quality")); jpegConfig.quality = quality; @@ -901,8 +908,8 @@ namespace phase_one std::vector compression_params; compression_params.push_back(cv::IMWRITE_JPEG_QUALITY); compression_params.push_back(quality); - boost::filesystem::path path_filename{filename}; - boost::filesystem::create_directories(path_filename.parent_path()); + std::filesystem::path path_filename{filename}; + std::filesystem::create_directories(path_filename.parent_path()); cv::imwrite(filename, cvImage, compression_params); } auto tocj = std::chrono::high_resolution_clock::now(); @@ -939,43 +946,40 @@ namespace phase_one P1::CameraSdk::PropertySpecification property = camera.PropertySpec(propertyId); std::string name = property.mName; property_to_id_[name] = propertyId; - property_to_type_[name] = property.mValue; + property_to_type[name] = property.mValue; } }; - bool PhaseOne::getPhaseOneParameter(phase_one::GetPhaseOneParameter::Request& req, - phase_one::GetPhaseOneParameter::Response& resp) + void PhaseOne::getPhaseOneParameter(const std::shared_ptr req, + std::shared_ptr resp) { if (property_to_id_.size() > 0) { try { - std::string name = req.name.c_str(); + std::string name = req->name.c_str(); int id = property_to_id_[ name ]; P1::CameraSdk::PropertyValue pv = camera.Property(id); - resp.value = pv.ToString(); - resp.message = "ok"; - ROS_INFO_STREAM("|GET| Parameter " << name << " returning value " << resp.value); + resp->value = pv.ToString(); + resp->message = "ok"; + ROS_INFO_STREAM("|GET| Parameter " << name << " returning value " << resp->value); } catch (const std::exception& ex) { ROS_ERROR_STREAM("Cannot get parameter: " << ex.what()); - resp.message = ex.what(); + resp->message = ex.what(); } } - - return true; - }; - bool PhaseOne::setPhaseOneParameter(phase_one::SetPhaseOneParameter::Request& req, - phase_one::SetPhaseOneParameter::Response& resp) + void PhaseOne::setPhaseOneParameter(const std::shared_ptr req, + std::shared_ptr resp) { if (property_to_id_.size() > 0) { try { - std::string parameters = req.parameters.c_str(); + std::string parameters = req->parameters.c_str(); std::map name_to_value = parseParams(parameters); @@ -1010,32 +1014,30 @@ namespace phase_one catch (const std::exception& ex) { ROS_ERROR_STREAM("Cannot set parameter: " << ex.what()); - resp.message = ex.what(); - return false; + resp->message = ex.what(); + return; } - resp.message = "ok"; + resp->message = "ok"; } - return true; }; - bool PhaseOne::getCompressedImageView(phase_one::GetCompressedImageView::Request& req, - phase_one::GetCompressedImageView::Response& resp) { + void PhaseOne::getCompressedImageView(const std::shared_ptr req, + std::shared_ptr resp) { // DOES NOT WORK CURRENTLY - auto tic = std::chrono::high_resolution_clock::now(); ROS_INFO("Received Request for Compresssed Image View."); P1::ImageSdk::RawImage raw_img; std::unique_lock lock(mtx); if ( !image_q_.empty() ) { raw_img = image_q_.front(); } else { - resp.success = false; - return false; + resp->success = false; + return; } // manually release lock early lock.unlock(); // Extract destination points from source image given a generic homography - std::vector H = req.homography; + std::vector H = req->homography; // Construct proper dimensional homography cv::Mat warp_matrix; warp_matrix = cv::Mat::eye(3, 3, CV_32F); @@ -1058,13 +1060,13 @@ namespace phase_one cv::perspectiveTransform(srcPoints, dstPoints, warp_matrix); } catch(...) { ROS_ERROR("CV Warp exception in translating homography to points."); - resp.success = false; - return false; + resp->success = false; + return; } int x = dstPoints[0].x; int y = dstPoints[0].y; - int h = req.output_height; - int w = req.output_width; + int h = req->output_height; + int w = req->output_width; P1::ImageSdk::ConvertConfig config; config.SetCrop(x, y, w, h); @@ -1080,20 +1082,23 @@ namespace phase_one // Exception from ImageSDK ROS_ERROR_STREAM("|DEMOSAIC| ImageSDK Exception: " << exception.what() << " Code:" << exception.mCode << std::endl); - return false; + resp->success = false; + return; } catch (P1::CameraSdk::SdkException exception) { // Exception from CameraSDK ROS_ERROR_STREAM("|DEMOSAIC| CameraSDK Exception: " << exception.what() << " Code:" << exception.mErrorCode << std::endl); - return false; + resp->success = false; + return; } catch (...) { // Any other exception - just in case ROS_WARN("|DEMOSAIC| Argh - we got an exception in debayering."); - return false; + resp->success = false; + return; } ROS_INFO_STREAM(bitmap.Height() << " bitmap " << bitmap.Width() << "\n"); auto toc1 = std::chrono::high_resolution_clock::now(); @@ -1103,12 +1108,11 @@ namespace phase_one cvImage_raw.data = bitmap.Data().get(); try { - std_msgs::Header header; - header.seq = 0; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = node_->now(); img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, cvImage_raw); - sensor_msgs::CompressedImage output_msg; + sensor_msgs::msg::CompressedImage output_msg; output_msg.format = "jpg"; output_msg.header = header; int quality = std::stoi(envoy_->get("/sys/arch/jpg/quality")); @@ -1145,48 +1149,44 @@ namespace phase_one ROS_INFO("Calling tocompressimage"); output_msg.data = buffer; img_bridge.toCompressedImageMsg(output_msg); - resp.success = true; - resp.image = output_msg; + resp->success = true; + resp->image = output_msg; } catch (...) { ROS_ERROR("CV Warp exception."); - resp.success = false; - return false; + resp->success = false; + return; }; - return true; - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic; - ROS_INFO_STREAM("View Server: Time to process image request was: " << dt.count() / 1e9 << "s\n"); - return true; }; - bool PhaseOne::getImageView(custom_msgs::RequestImageView::Request& req, - custom_msgs::RequestImageView::Response& resp) { + void PhaseOne::getImageView(const std::shared_ptr req, + std::shared_ptr resp) { auto tic = std::chrono::high_resolution_clock::now(); ROS_INFO("|VIEW SERVER| : Received Request for image view."); P1::ImageSdk::RawImage raw_img; std::unique_lock lock(mtx); - bool staleH = req.homography == lastH; - bool staleSat = req.show_saturated_pixels == last_show_sat; - lastH = req.homography; - last_show_sat = req.show_saturated_pixels; + std::vector req_homography(req->homography.begin(), req->homography.end()); + bool staleH = req_homography == lastH; + bool staleSat = req->show_saturated_pixels == last_show_sat; + lastH = req_homography; + last_show_sat = req->show_saturated_pixels; if ( staleH && staleSat && !new_image) { - resp.success = true; - resp.image = sensor_msgs::Image(); - return true; + resp->success = true; + resp->image = sensor_msgs::msg::Image(); + return; } if ( !image_q_.empty() ) { raw_img = image_q_.front(); } else { - resp.success = false; - resp.image = sensor_msgs::Image(); - return false; + resp->success = false; + resp->image = sensor_msgs::msg::Image(); + return; } // Reset global tracking of a "new image" received new_image = false; // manually release lock early lock.unlock(); // Extract destination points from source image given a generic homography - std::vector H = req.homography; + std::vector H = req_homography; // Construct proper dimensional homography cv::Mat warp_matrix; warp_matrix = cv::Mat::eye(3, 3, CV_32F); @@ -1209,13 +1209,13 @@ namespace phase_one cv::perspectiveTransform(srcPoints, dstPoints, warp_matrix); } catch(...) { ROS_ERROR("|VIEW SERVER| : CV Warp exception in translating homography to points."); - resp.success = false; - return false; + resp->success = false; + return; } int x = dstPoints[0].x; int y = dstPoints[0].y; - int out_h = req.output_height; - int out_w = req.output_width; + int out_h = req->output_height; + int out_w = req->output_width; double scale = H[0]; double float_h = scale * out_h; double float_w = scale * out_w; @@ -1223,10 +1223,10 @@ namespace phase_one int h = (int) float_h; int w = (int) float_w; // Bounds checking - if (x < 0) x = 0; if (x > raw_img.Width()) x = raw_img.Width(); - if (y < 0) y = 0; if (y > raw_img.Height()) y = raw_img.Height(); - if (h < 0) h = 0; if (h > raw_img.Height() - y) h = raw_img.Height() - y; - if (w < 0) w = 0; if (w > raw_img.Width() - x) w = raw_img.Width() - x; + if (x < 0) x = 0; if (x > (int) raw_img.Width()) x = raw_img.Width(); + if (y < 0) y = 0; if (y > (int) raw_img.Height()) y = raw_img.Height(); + if (h < 0) h = 0; if (h > (int) raw_img.Height() - y) h = raw_img.Height() - y; + if (w < 0) w = 0; if (w > (int) raw_img.Width() - x) w = raw_img.Width() - x; // All this gets a nice crop out of the image quite quickly (0.2s at worst) P1::ImageSdk::ConvertConfig config; config.SetCrop(x, y, w, h); @@ -1245,23 +1245,23 @@ namespace phase_one // Exception from ImageSDK ROS_ERROR_STREAM("|VIEW SERVER| ImageSDK Exception: " << exception.what() << " Code:" << exception.mCode << std::endl); - resp.success = false; - return false; + resp->success = false; + return; } catch (P1::CameraSdk::SdkException exception) { // Exception from CameraSDK ROS_ERROR_STREAM("|VIEW SERVER| CameraSDK Exception: " << exception.what() << " Code:" << exception.mErrorCode << std::endl); - resp.success = false; - return false; + resp->success = false; + return; } catch (...) { // Any other exception - just in case ROS_WARN("|VIEW SERVER| Argh - we got an exception in debayering."); - resp.success = false; - return false; + resp->success = false; + return; } auto toc1 = std::chrono::high_resolution_clock::now(); auto dt1 = toc1 - tic1; @@ -1269,7 +1269,7 @@ namespace phase_one cv::Mat cvImage_raw = cv::Mat(cv::Size(bitmap.Width(), bitmap.Height()), CV_8UC3); cvImage_raw.data = bitmap.Data().get(); try { - if (req.show_saturated_pixels) { + if (req->show_saturated_pixels) { int maxval = 255; cv::Scalar sat_pix = cv::Scalar(maxval, maxval, maxval); cv::Mat mask; @@ -1279,52 +1279,49 @@ namespace phase_one } } catch (const std::exception& e) { ROS_ERROR_STREAM("|VIEW SERVER| Caught generic exception in sat pixels: " << e.what()); - return false; + resp->success = false; + return; } try { - std_msgs::Header header; - header.seq = 0; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = node_->now(); img_bridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, cvImage_raw); - sensor_msgs::Image output_msg; + sensor_msgs::msg::Image output_msg; img_bridge.toImageMsg(output_msg); - resp.success = true; - resp.image = output_msg; + resp->success = true; + resp->image = output_msg; } catch (...) { ROS_ERROR("|VIEW SERVER| : CV Warp exception."); - resp.success = false; - return false; + resp->success = false; + return; }; auto toc = std::chrono::high_resolution_clock::now(); auto dt = toc - tic; ROS_INFO_STREAM("|VIEW SERVER| : Time to process image request was: " << dt.count() / 1e9 << "s\n"); - return true; }; - void PhaseOne::eventCallback (const boost::shared_ptr& msg) { - ROS_INFO("[%d]<1> eventCallback <> %2.2f", msg->header.seq, msg->header.stamp.toSec()); - //event_ = *msg; - event_cache.push_back(msg->sys_time, msg); + void PhaseOne::eventCallback (const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) { + ROS_INFO("[%lu]<1> eventCallback <> %2.2f", (unsigned long) msg->event_num, + rclcpp::Time(msg->header.stamp).seconds()); + event_cache.push_back(rclcpp::Time(msg->sys_time), msg); // TODO: hardcoded, but is OK for now auto nodeName = "/" + hostname + "/rgb/rgb_driver"; - custom_msgs::Stat stat_msg; + custom_msgs::msg::Stat stat_msg; std::stringstream link; - stat_msg.header.stamp = ros::Time::now(); + stat_msg.header.stamp = node_->now(); stat_msg.trace_header = (*msg).header; stat_msg.trace_topic = nodeName + "/eventCallback"; stat_msg.node = nodeName; - link << nodeName << "/event/" << msg->header.seq; // link this trace to the event trace + link << nodeName << "/event/" << msg->event_num; // link this trace to the event trace stat_msg.link = link.str(); - stat_pub_.publish(stat_msg); + stat_pub_->publish(stat_msg); event_cache.purge(); - //watchdog.check(); - //event_cache.show(); }; - void PhaseOne::detectionListCallback (const boost::shared_ptr& msg) { - ROS_INFO("[%d]<1> detectionListCallback <> %2.2f", - msg->header.seq, msg->header.stamp.toSec()); + void PhaseOne::detectionListCallback (const custom_msgs::msg::ImageSpaceDetectionList::ConstSharedPtr& msg) { + ROS_INFO("<1> detectionListCallback <> %2.2f", + rclcpp::Time(msg->header.stamp).seconds()); // this fname is the RGB jpeg filename (always) of the triplet, // even if that file doesn't yet exist std::string fname = msg->header.frame_id; @@ -1341,7 +1338,6 @@ namespace phase_one } lock.unlock(); int num_detections = msg->detections.size(); - std::string effort_topic = "/sys/arch/effort"; effort = envoy_->get("/sys/arch/effort"); std::string x_image_topic = "/sys/effort_metadata_dict/" + effort + "/save_every_x_image"; save_every_x = std::stoi(envoy_->get(x_image_topic)); @@ -1414,13 +1410,15 @@ namespace phase_one int main(int argc, char** argv) { - ros::init(argc, argv, "phase_one_standalone"); + rclcpp::init(argc, argv); + auto node = std::make_shared("phase_one_standalone"); phase_one::PhaseOne cls; - cls.init(); - ros::Rate r(10); - while (cls.running_) { - ros::spinOnce(); - r.sleep(); + cls.init(node); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2); + executor.add_node(node); + while (cls.running_ && rclcpp::ok()) { + executor.spin_some(std::chrono::milliseconds(100)); } + rclcpp::shutdown(); return 0; } diff --git a/src/cams/phase_one/src/phase_one_utils.cpp b/src/cams/phase_one/src/phase_one_utils.cpp deleted file mode 100644 index a888274b..00000000 --- a/src/cams/phase_one/src/phase_one_utils.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#include -#include -#include -// ROS -#include -// Custom -#include -#include - - -ros::Duration rosabs(ros::Duration dur) { - if (dur < ZERO_DURATION) - return -dur; - return dur; -} - - -EventCache::EventCache() { - ros::Duration tol(0.499); - ros::Duration delay(0); - this->set_tolerance(tol); - this->set_delay(delay); -} - -EventCache::EventCache(ros::Duration tol, ros::Duration delay) { - this->set_tolerance(tol); - this->set_delay(delay); -} - -void EventCache::set_tolerance(ros::Duration const &tolerance) { - std::lock_guard guard(mutex_); - this->tol = tolerance; -} - -void EventCache::set_delay(ros::Duration const &delay) { - std::lock_guard guard(mutex_); - this->delay = delay; -} - -void EventCache::set_delay(double delay) { - std::lock_guard guard(mutex_); - this->delay = ros::Duration{delay}; -} - -void EventCache::push_back(ros::Time const &t, const boost::shared_ptr& msg) { - std::lock_guard guard(mutex_); - if (!msg->sys_time.isValid()) { - ROS_ERROR("Zero/invalid sys_time encountered in EventCache::push_back()"); - return; - } - if (!msg->gps_time.isValid()) { - ROS_ERROR("Zero/invalid gps_time encountered in EventCache::push_back()"); - return; - } - event_map.emplace(t, custom_msgs::GSOF_EVT(*msg)); -} - -int EventCache::size() { - std::lock_guard guard(mutex_); - return (int) event_map.size(); -} - -void EventCache::show() { - std::lock_guard guard(mutex_); - for (auto it = event_map.begin(); it != event_map.end(); ++it) { - std::cout << it->second.header; - } - std::cout << "\n---" << std::endl; -} - -bool EventCache::search(ros::Time image_time, std_msgs::Header &head, bool remove_when_found) { - std::lock_guard guard(mutex_); - if (!image_time.isValid()) { - ROS_ERROR("zero/invalid image_time encountered in EventCache::search()"); - return false; - } - int count = 0; - // we want the lowest corrected time - ros::Duration best_time{999,999}; - ros::Time best_sys_time; // best matching system time, aka key - ROS_INFO_STREAM("Image time is: " << image_time.toSec() << std::endl); - std::map event_map_copy(event_map); - for (const auto pair: event_map_copy) { - auto actual_delay = rosabs(pair.second.sys_time - image_time); - auto corrected_delay = rosabs(actual_delay - delay); - if (corrected_delay < tol) { - std::cout << "img: " << image_time << " sys: " << pair.second.sys_time << " cdt: " << corrected_delay << " ad: " << actual_delay; - count++; - if (corrected_delay < best_time ) { - best_time = corrected_delay; - head.stamp = pair.second.gps_time; - head.seq = pair.second.header.seq; - best_sys_time = pair.second.sys_time; - std::cout << " *"; - } - std::cout << std::endl; - } else { - // std::cout << count << ": " << it->first.toSec() << " dt: " << corrected_delay << " ad: " << actual_delay << std::endl; - } - } -// std::cout << "\n---" << std::endl; - ROS_INFO_STREAM("Matched " << count << "/" << event_map_copy.size() << " time headers" << std::endl); - if (count >= 1) { - if (remove_when_found && best_sys_time.isValid()) { - event_map.erase(best_sys_time); - } - return true; - } - return false; -} - -void EventCache::purge() { - std::lock_guard guard(mutex_); - auto now = ros::Time::now(); - std::map event_map_copy(event_map); - for ( const auto pair : event_map_copy ) { - auto age = now - pair.second.sys_time ; - if (age > stale_time) { - event_map.erase(pair.first); - } - } -} - - -std::map parseParams(std::string parameters) { - // Iterate through parameters organized by name=value, separated by - // commas. Return a map of parameters to values. - std::map param_to_value; - std::string delimiter1 = ","; - std::string delimiter2 = "="; - size_t pos = 0; - std::string token; - std::string name; - std::string value; - // Always run at least once even if there's no delimiter in request - while ((pos = parameters.find(delimiter1)) != std::string::npos) { - token = parameters.substr(0, pos); - name = token.substr(0, token.find(delimiter2)); - token.erase(0, token.find(delimiter2) + delimiter2.length()); - value = token; - param_to_value[name] = value; - parameters.erase(0, pos + delimiter1.length()); - } - token = parameters; - name = token.substr(0, token.find(delimiter2)); - token.erase(0, token.find(delimiter2) + delimiter2.length()); - value = token; - param_to_value[name] = value; - return param_to_value; -}; - - -std::vector loadFile(std::string filename) { - std::vector lines; - std::ifstream inputFile(filename); - // Check if the file exists and can be opened - if (!inputFile.is_open()) { - std::cout << "File " << filename << " does not exist or cannot be opened." << std::endl; - return lines; - } else { - std::string line; - while (std::getline(inputFile, line)) { - lines.push_back(line); - } - } - return lines; -}; \ No newline at end of file diff --git a/src/cams/phase_one/src/view_server_nodelet.cpp b/src/cams/phase_one/src/view_server_nodelet.cpp deleted file mode 100755 index ed49731f..00000000 --- a/src/cams/phase_one/src/view_server_nodelet.cpp +++ /dev/null @@ -1,142 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace P1::CameraSdk; -using namespace P1::ImageSdk; - -using std::cout; -using std::endl; - -namespace phase_one -{ - class ViewServerNodelet : public nodelet::Nodelet - { - public: - ViewServerNodelet() - { - } - - private: - std::queue image_q_; - cv_bridge::CvImage img_bridge; - P1::CameraSdk::Camera camera; - P1::ImageSdk::DecodeConfig decodeConfig; - P1::ImageSdk::ConvertConfig config; - ros::ServiceServer image_view_service_; - - virtual void onInit() { - ROS_INFO("Image View Server: Initialization"); - // ROS initialization - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - image_transport::ImageTransport it(nh); - image_transport::Subscriber sub = it.subscribe("/image_raw", 1, - &ViewServerNodelet::image_callback, this); - image_view_service_ = pnh.advertiseService("get_image_view", - &ViewServerNodelet::getImageView, this); - ROS_INFO("Image View Server: Finished"); - //ros::spin(); - - // Phase one Initialization - //P1::ImageSdk::Initialize(); - //decodeConfig = P1::ImageSdk::DecodeConfig::Defaults; - //P1::ImageSdk::SetSensorProfilesLocation( - // "/home/squadx/noaa/phaseOne/build/ImageSDK/SensorProfiles"); - }; - - void image_callback(const sensor_msgs::ImageConstPtr& msg) { - // Keep a running most-recent queue of size 1 - auto tic = std::chrono::high_resolution_clock::now(); - if ( !image_q_.empty() ) { - image_q_.pop(); - image_q_.push(msg); - } else { - image_q_.push(msg); - } - ROS_INFO_STREAM("Size of image queue is: " << image_q_.size()); - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic; - ROS_INFO_STREAM("View Server: Time to receive image camera was: " << dt.count() / 1e9 << "s\n"); - }; - - bool getImageView(phase_one::GetImageView::Request& req, - phase_one::GetImageView::Response& resp) { - auto tic = std::chrono::high_resolution_clock::now(); - ROS_INFO("Received Request for image view."); - sensor_msgs::Image msg; - if ( !image_q_.empty() ) { - msg = *image_q_.front(); - } else { - resp.success = false; - return false; - } - - cv_bridge::CvImagePtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - resp.success = false; - return false; - } - cv::Mat cv_image = cv_ptr->image.clone(); - ROS_INFO_STREAM("View Server: Received image of width: " << cv_image.size().width << - " and height: " << cv_image.size().height << std::endl); - - try { - std::vector H = req.homography; - int h = req.output_height; - int w = req.output_width; - int interp = req.interpolation; - - cv::Mat imgWarp = cv::Mat(cv::Size(w, h), CV_8UC3); - ROS_INFO("Warping image."); - cv::warpPerspective(cv_image, imgWarp, H, cv::Point(w, h), interp); - ROS_INFO("Finished warping image."); - - img_bridge = cv_bridge::CvImage(msg.header, - sensor_msgs::image_encodings::RGB8, imgWarp); - sensor_msgs::Image output_msg; - img_bridge.toImageMsg(output_msg); - - resp.success = true; - resp.image = output_msg; - } catch (...) { - ROS_ERROR("CV Warp exception."); - resp.success = false; - return false; - } - - auto toc = std::chrono::high_resolution_clock::now(); - auto dt = toc - tic; - ROS_INFO_STREAM("View Server: Time to process image request was: " << dt.count() / 1e9 << "s\n"); - return true; - }; - }; - - -PLUGINLIB_EXPORT_CLASS(phase_one::ViewServerNodelet, nodelet::Nodelet); -} diff --git a/src/run_scripts/entry/cam_phaseone.sh b/src/run_scripts/entry/cam_phaseone.sh index 3c1ed22a..14e810fb 100755 --- a/src/run_scripts/entry/cam_phaseone.sh +++ b/src/run_scripts/entry/cam_phaseone.sh @@ -90,12 +90,9 @@ trap "errcho 'Caught SIGINT'; cleanup" SIGINT # Expected exit code from docker stop command. trap "errcho 'Caught SIGTERM'; cleanup" SIGTERM -ROSWAIT="--wait" -LOGFILE="/tmp/roslaunch_err_${CAM_FOV}_${CAM_MODE}.log" - if [[ $(redis-cli --raw -h $REDIS_HOST get /debug/rebuild ) == "true" ]]; then echo "/debug/rebuild set, triggering rebuild on startup" - catkin build phase_one + colcon build --packages-select phase_one if [[ $? -ne 0 ]]; then echo "Rebuild failed. Your code is in an unstable state" exit 1 @@ -108,22 +105,13 @@ echo "Building to_process.txt" rm -f /mnt/data/to_process.txt find /mnt/data/iiq_buffer -name "*.IIQ" > /mnt/data/to_process.txt -export ROS_NAMESPACE="/${NODE_HOSTNAME}/${CAM_MODE}" -exec roslaunch "${ROSWAIT}" phase_one phase_one_standalone.launch \ +RESPAWN=$([[ "${NORESPAWN}" == "true" ]] && echo false || echo true) +exec ros2 launch phase_one phase_one_standalone.launch.xml \ ip_address:=${CAM_IP} \ system_name:=${NODE_HOSTNAME} \ cam_mode:=${CAM_MODE} \ hostname:=${NODE_HOSTNAME} \ trigger_mode:=${TRIGGER_MODE} \ - norespawn:=${NORESPAWN} \ + respawn:=${RESPAWN} \ num_threads:=28 \ auto_trigger_rate:=1.0 - -STAT_ROS=$! -wait $STAT_ROS -echo "roslaunch probably died with a 0 error code" -RES=$(grep -Po -e 'REQUIRED.+ has died' "${LOGFILE}") -if [[ -n $RES ]]; then - echo $RES - exit 1 -fi From 8e5f9f38b81d5e4bf1c324151f561a12923a06bf Mon Sep 17 00:00:00 2001 From: romleiaj Date: Fri, 3 Jul 2026 21:44:32 -0400 Subject: [PATCH 14/20] Port wxpython_gui (system control panel) to ROS2 - New wxpython_gui.rosnode module owns a single rclpy node serviced by a background MultiThreadedExecutor thread, exposing the imperative surface the wx GUI needs: Subscriber, a synchronous ServiceProxy (call_async + wait, raising ServiceException on unavailable/timeout, matching the rospy semantics every call site already handles), Rate, logging, and stamp_to_sec - gui.py / UpdateImageThread.py call sites moved from rospy to rosnode; GSOF_INS -> GsofIns; header.stamp.to_sec() -> stamp_to_sec(); Subscriber.unregister -> destroy_subscription; clean rclpy shutdown wired into the window-close handler - The single rospy.set_param('/sys/arch/is_archiving') becomes a Redis kv.put - there is no global param server in ROS2 and every consumer already reads that key from Redis - cfg.py/utils.py drop now-unused rospy imports (ros_immediate was dead) - Package converted to ament_python with a system_control_panel console script (node script moved into the package); launch converted to ROS2 XML; stale .pyc files dropped from the repo - gui.sh launches via ros2 launch and checks ROS_DOMAIN_ID instead of ROS_MASTER_URI; start_gui.sh no longer spawns a local roscore; gui.dockerfile builds with colcon --packages-up-to wxpython_gui ins_driver (base image must be Jazzy, tracked with the gui-deps image) --- compose/gui.yml | 2 - docker/gui.dockerfile | 13 +- .../wxpython_gui/CMakeLists.txt | 201 ------------------ .../launch/system_control_panel.launch | 5 - .../launch/system_control_panel.launch.xml | 4 + src/kitware-ros-pkg/wxpython_gui/package.xml | 78 ++----- .../wxpython_gui/resource/wxpython_gui | 0 src/kitware-ros-pkg/wxpython_gui/setup.cfg | 4 + src/kitware-ros-pkg/wxpython_gui/setup.py | 34 ++- .../src/wxpython_gui/UpdateImageThread.py | 30 +-- .../wxpython_gui/src/wxpython_gui/cfg.py | 3 - .../wxpython_gui/src/wxpython_gui/rosnode.py | 175 +++++++++++++++ .../wxpython_gui/system_control_panel/gui.py | 57 ++--- .../system_control_panel_main.py} | 2 - .../wxpython_gui/src/wxpython_gui/utils.py | 1 - src/run_scripts/entry/gui.sh | 7 +- tmux/nayak/start_gui.sh | 4 - tmux/taiga/start_gui.sh | 4 - 18 files changed, 284 insertions(+), 340 deletions(-) delete mode 100644 src/kitware-ros-pkg/wxpython_gui/CMakeLists.txt delete mode 100644 src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch create mode 100644 src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch.xml create mode 100644 src/kitware-ros-pkg/wxpython_gui/resource/wxpython_gui create mode 100644 src/kitware-ros-pkg/wxpython_gui/setup.cfg create mode 100644 src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/rosnode.py rename src/kitware-ros-pkg/wxpython_gui/{scripts/system_control_panel_node.py => src/wxpython_gui/system_control_panel_main.py} (97%) diff --git a/compose/gui.yml b/compose/gui.yml index e471f023..7d202324 100644 --- a/compose/gui.yml +++ b/compose/gui.yml @@ -10,8 +10,6 @@ services: environment: REDIS_HOST: "${REDIS_HOST}" ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" - ROS_IP: "${ROS_IP:-}" - ROS_HOSTNAME: "${ROS_HOSTNAME:-}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" GUI_CFG_DIR: "${GUI_CFG_DIR}" DISPLAY: "${DISPLAY}" diff --git a/docker/gui.dockerfile b/docker/gui.dockerfile index fdbf0e66..be7f7433 100644 --- a/docker/gui.dockerfile +++ b/docker/gui.dockerfile @@ -24,7 +24,6 @@ COPY --chown=user:user . $REPO_DIR RUN rm -rf /entry \ && ln -sf $REPO_DIR/src/run_scripts/entry /entry \ && printf "\nsource /entry/project.sh\n" >> /home/user/.bashrc \ - && touch $REPO_DIR/.catkin_workspace \ && ln -sf $REPO_DIR/src/run_scripts/aliases.sh /aliases.sh \ && printf "\nsource /aliases.sh\n" >> /home/user/.bashrc @@ -37,15 +36,15 @@ RUN ln -sv /usr/bin/python3 /usr/bin/python || true RUN find /home/user -not -user user -execdir chown user {} \+ # Install kamera for wxpython_gui imports. --no-deps: deps come from the base -# image (a full install trips on ROS's distutils PyYAML). -# --ignore-requires-python: ROS Noetic pins python 3.8, below our 3.10 floor. -RUN pip install --no-cache-dir matplotlib \ - && pip install --no-cache-dir --no-deps --ignore-requires-python -e $REPO_DIR +# image (a full install trips on ROS's distutils PyYAML). --break-system-packages: +# Ubuntu 24.04 marks its python as externally managed. Jazzy's python 3.12 clears +# our 3.10 floor, so the Noetic-era --ignore-requires-python is gone. +RUN pip install --break-system-packages --no-cache-dir matplotlib \ + && pip install --break-system-packages --no-cache-dir --no-deps -e $REPO_DIR # use the exec form of run because we need bash syntax USER user -RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build wxpython_gui "] -RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build ins_driver "] +RUN [ "/bin/bash", "-c", "source /opt/ros/${ROS_DISTRO}/setup.bash && colcon build --base-paths src --packages-up-to wxpython_gui ins_driver --cmake-args -DCMAKE_BUILD_TYPE=Release"] USER root RUN find /home/user -not -user user -execdir chown user {} \+ USER user diff --git a/src/kitware-ros-pkg/wxpython_gui/CMakeLists.txt b/src/kitware-ros-pkg/wxpython_gui/CMakeLists.txt deleted file mode 100644 index 8e3a9490..00000000 --- a/src/kitware-ros-pkg/wxpython_gui/CMakeLists.txt +++ /dev/null @@ -1,201 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(wxpython_gui) - -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-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 - cv_bridge - rospy - sensor_msgs - std_msgs - image_view -) - -## 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 -# sensor_msgs# 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 you 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 focus_camera -# CATKIN_DEPENDS cv_bridge opencv2 rospy sensor_msgs std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/focus_camera.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/focus_camera_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/system_control_panel_node.py - 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_focus_camera.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) diff --git a/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch b/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch deleted file mode 100644 index 2369f5c2..00000000 --- a/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch.xml b/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch.xml new file mode 100644 index 00000000..eef0fa0a --- /dev/null +++ b/src/kitware-ros-pkg/wxpython_gui/launch/system_control_panel.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/kitware-ros-pkg/wxpython_gui/package.xml b/src/kitware-ros-pkg/wxpython_gui/package.xml index 23d75501..f50fc5c7 100644 --- a/src/kitware-ros-pkg/wxpython_gui/package.xml +++ b/src/kitware-ros-pkg/wxpython_gui/package.xml @@ -1,62 +1,26 @@ - - + + + wxpython_gui - 0.0.0 - GUIs implemented with wxPython. + 1.0.0 + KAMERA system control panel GUI (wxPython) + + Adam Romlein + Apache 2.0 + + rclpy + std_msgs + sensor_msgs + custom_msgs + cv_bridge + roskv + python3-numpy + python3-opencv + python3-redis + python3-yaml - - - - Matt Brown - Michael McDermott - - - - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - rospkg - sensor_msgs - std_msgs - custom_msgs - cv_bridge - image_view - genpy - python3-numpy - python3-wxgtk4.0 - sensor_models - roskv - - - - + + ament_python diff --git a/src/kitware-ros-pkg/wxpython_gui/resource/wxpython_gui b/src/kitware-ros-pkg/wxpython_gui/resource/wxpython_gui new file mode 100644 index 00000000..e69de29b diff --git a/src/kitware-ros-pkg/wxpython_gui/setup.cfg b/src/kitware-ros-pkg/wxpython_gui/setup.cfg new file mode 100644 index 00000000..9bb20440 --- /dev/null +++ b/src/kitware-ros-pkg/wxpython_gui/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/wxpython_gui +[install] +install_scripts=$base/lib/wxpython_gui diff --git a/src/kitware-ros-pkg/wxpython_gui/setup.py b/src/kitware-ros-pkg/wxpython_gui/setup.py index 188a9222..8d7ec117 100644 --- a/src/kitware-ros-pkg/wxpython_gui/setup.py +++ b/src/kitware-ros-pkg/wxpython_gui/setup.py @@ -1,10 +1,32 @@ -#!/usr/bin/env python3 +from glob import glob + from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup -d = generate_distutils_setup( - packages=["wxpython_gui"], +package_name = "wxpython_gui" + +setup( + name=package_name, + version="1.0.0", + packages=[ + package_name, + package_name + ".system_control_panel", + ], package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ("share/" + package_name + "/shapefiles", glob("shapefiles/*")), + ], + install_requires=["setuptools"], + zip_safe=False, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="KAMERA system control panel GUI (wxPython)", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "system_control_panel = wxpython_gui.system_control_panel_main:main", + ], + }, ) - -setup(**d) diff --git a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/UpdateImageThread.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/UpdateImageThread.py index f6f2bb92..4961f60a 100644 --- a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/UpdateImageThread.py +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/UpdateImageThread.py @@ -2,7 +2,7 @@ from __future__ import division, print_function import datetime import threading -import rospy +from wxpython_gui import rosnode as ros from io import BytesIO import cv2 import sys @@ -97,21 +97,21 @@ def run(self): """ if not self.sub: - self._image_service = rospy.ServiceProxy( + self._image_service = ros.ServiceProxy( self._ros_srv_topic, RequestImageView, persistent=True ) - self._compress_image_service = rospy.ServiceProxy( + self._compress_image_service = ros.ServiceProxy( "%s/compressed" % self._ros_srv_topic, RequestCompressedImageView, persistent=False, ) else: sub_topic = os.path.join("/", self._node_host, self._chan, "image_raw") - self._sub_to_images = rospy.Subscriber( + self._sub_to_images = ros.Subscriber( sub_topic, Image, self.process_pub_image ) im_rate = 5 - rate = rospy.Rate(im_rate) + rate = ros.Rate(im_rate) while True: if self._stop: return None # Check for a request to stop. @@ -123,7 +123,7 @@ def run(self): rate.sleep() # if ret is None or ret is False: # h = std_msgs.msg.Header() - # h.stamp = rospy.Time.now() + # h.stamp = now # self.update_status_msg(h) # format_status() # print("Stopping requests.") @@ -134,7 +134,7 @@ def run(self): pass # wx noise except Exception as e: rate.sleep() - self._image_service = rospy.ServiceProxy( + self._image_service = ros.ServiceProxy( self._ros_srv_topic, RequestImageView, persistent=True ) exc_type, exc_obj, exc_tb = sys.exc_info() @@ -152,7 +152,7 @@ def invalidate_cache(self): try: pass - except rospy.service.ServiceException: + except ros.ServiceException: pass def get_homography(self, preview=False): @@ -214,7 +214,7 @@ def get_new_raw_image(self, release=0): try: homography, output_height, output_width = self.get_homography() - # py3 rospy is strict: int fields reject floats (incl. numpy). + # rosidl is strict: int fields reject floats (incl. numpy). output_height = int(output_height) output_width = int(output_width) contrast_strength = int(SYS_CFG["ir_contrast_strength"]) @@ -278,22 +278,22 @@ def get_new_raw_image(self, release=0): # print("Time to update frame was %0.3fs" % (toc - tic)) return True - except rospy.service.ServiceException as e: + except ros.ServiceException as e: # Too noisy - # rospy.logwarn('Service failed: {}'.format(e)) - self._image_service = rospy.ServiceProxy( + # ros.logwarn('Service failed: {}'.format(e)) + self._image_service = ros.ServiceProxy( self._ros_srv_topic, RequestImageView, persistent=True ) return except RuntimeError as e: - rospy.logwarn(e) + ros.logwarn(e) return def update_status_msg(self, img_header): # type: (std_msgs.msg.Header) -> str """Update the status bar for an image view""" - t = img_header.stamp.to_sec() + t = ros.stamp_to_sec(img_header.stamp) t = datetime.datetime.utcfromtimestamp(t) # string = format_status(timeval=t, num_dropped=0) # Don't let a missing redis key block imagery dispatch. @@ -304,7 +304,7 @@ def update_status_msg(self, img_header): timeval=t, ) except Exception as e: - rospy.logwarn_throttle(10, "Could not format status: {}".format(e)) + ros.logwarn_throttle(10, "Could not format status: {}".format(e)) return None wx.CallAfter(self._parent.update_status_msg, string) return string diff --git a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/cfg.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/cfg.py index c36c0f73..a7628476 100644 --- a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/cfg.py +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/cfg.py @@ -9,7 +9,6 @@ from collections import OrderedDict from functools import reduce -import rospy from cv_bridge import CvBridge, CvBridgeError from roskv.impl.redis_envoy import RedisEnvoy as ImplEnvoy @@ -241,8 +240,6 @@ def save_config_settings(): # =================== DEFINE GLOBALS =============================== -# Need a vanilla one for binary insert -ros_immediate = rospy.Duration(nsecs=1) # Instantiate CvBridge bridge = CvBridge() diff --git a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/rosnode.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/rosnode.py new file mode 100644 index 00000000..ad811e75 --- /dev/null +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/rosnode.py @@ -0,0 +1,175 @@ +# -*- coding: utf-8 -*- +"""rclpy backend for the wx GUI. + +The GUI runs the wx main loop on the main thread and talks to ROS from wx +event handlers and worker threads. This module owns a single rclpy node +serviced by a background executor thread, and exposes the small imperative +surface the GUI needs (subscriptions, synchronous service calls, logging). +""" +from __future__ import division, print_function + +import threading +import time as _time + +import rclpy +import rclpy.logging +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor + +_node = None # type: Node +_executor = None +_spin_thread = None +_log = rclpy.logging.get_logger("wxpython_gui") +_throttle_marks = {} + + +class ServiceException(Exception): + """Raised when a service is unavailable or the call fails/times out.""" + pass + + +class _ServiceNamespace(object): + """Back-compat shim so call sites can reference ros.service.ServiceException.""" + ServiceException = ServiceException + + +service = _ServiceNamespace() + + +def init_node(name, anonymous=False): + """Initialize rclpy, create the GUI node, and start the background spinner.""" + global _node, _executor, _spin_thread + if _node is not None: + return _node + if anonymous: + name = "%s_%d" % (name, int(_time.time() * 1e6) % 1000000) + rclpy.init() + _node = rclpy.create_node(name) + _executor = MultiThreadedExecutor(num_threads=4) + _executor.add_node(_node) + _spin_thread = threading.Thread(target=_executor.spin, daemon=True) + _spin_thread.start() + return _node + + +def node(): + if _node is None: + raise RuntimeError("rosnode.init_node() must be called first") + return _node + + +def is_shutdown(): + return not rclpy.ok() + + +def now_sec(): + return node().get_clock().now().nanoseconds * 1e-9 + + +def stamp_to_sec(stamp): + """builtin_interfaces/Time -> float unix seconds.""" + return stamp.sec + stamp.nanosec * 1e-9 + + +def Subscriber(topic, msg_type, callback, callback_args=None, queue_size=10): + if callback_args is not None: + wrapped = lambda msg: callback(msg, callback_args) + else: + wrapped = callback + return node().create_subscription(msg_type, topic, wrapped, queue_size) + + +class ServiceProxy(object): + """Synchronous service client mirroring rospy.ServiceProxy call semantics. + + Calls block the calling (wx/worker) thread while the background executor + services the future. Raises ServiceException on unavailability or timeout. + """ + + def __init__(self, topic, srv_type, persistent=False, wait_timeout=2.0, + call_timeout=30.0): + self._topic = topic + self._srv_type = srv_type + self._wait_timeout = wait_timeout + self._call_timeout = call_timeout + self._client = node().create_client(srv_type, topic) + + def call(self, *args, **kwargs): + if args: + # map positional args onto request fields in declaration order + req = self._srv_type.Request() + fields = list(req.get_fields_and_field_types().keys()) + for value, field in zip(args, fields): + setattr(req, field, value) + for key, value in kwargs.items(): + setattr(req, key, value) + else: + req = self._srv_type.Request(**kwargs) + + if not self._client.wait_for_service(timeout_sec=self._wait_timeout): + raise ServiceException( + "service [%s] unavailable" % self._topic) + future = self._client.call_async(req) + deadline = _time.time() + self._call_timeout + while not future.done(): + if _time.time() > deadline: + self._client.remove_pending_request(future) + raise ServiceException( + "service [%s] call timed out" % self._topic) + if not rclpy.ok(): + raise ServiceException( + "service [%s] interrupted by shutdown" % self._topic) + _time.sleep(0.005) + if future.exception() is not None: + raise ServiceException( + "service [%s] call failed: %s" % (self._topic, future.exception())) + return future.result() + + __call__ = call + + +class Rate(object): + def __init__(self, hz): + self._period = 1.0 / hz + self._last = _time.monotonic() + + def sleep(self): + elapsed = _time.monotonic() - self._last + remaining = self._period - elapsed + if remaining > 0: + _time.sleep(remaining) + self._last = _time.monotonic() + + +def loginfo(msg, *args): + _log.info(str(msg) % args if args else str(msg)) + + +def logwarn(msg, *args): + _log.warning(str(msg) % args if args else str(msg)) + + +def logerr(msg, *args): + _log.error(str(msg) % args if args else str(msg)) + + +def logwarn_throttle(period, msg): + key = str(msg)[:64] + now = _time.monotonic() + last = _throttle_marks.get(key, 0) + if now - last >= period: + _throttle_marks[key] = now + _log.warning(str(msg)) + + +def shutdown(): + global _node, _executor, _spin_thread + if _executor is not None: + _executor.shutdown() + if _node is not None: + _node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + _node = None + _executor = None + _spin_thread = None diff --git a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel/gui.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel/gui.py index 43b97910..44117ff7 100644 --- a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel/gui.py +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel/gui.py @@ -39,9 +39,9 @@ import shapefile # ROS imports -import rospy +from wxpython_gui import rosnode as ros import std_msgs.msg -from custom_msgs.msg import GSOF_INS +from custom_msgs.msg import GsofIns from sensor_msgs.msg import Image, CompressedImage from cv_bridge import CvBridge, CvBridgeError @@ -318,7 +318,7 @@ def __init__( self.update_show_hide() # Set up ROS connections. - rospy.init_node(node_name, anonymous=True) + ros.init_node(node_name, anonymous=True) # --------------------------- Image Streams -------------------------- # These will all be compressed images fit to the panel. @@ -440,25 +440,25 @@ def __init__( self.lat0 = None self.lon0 = None self.h0 = None - self.ins_state_sub = rospy.Subscriber( - topic_names["nav_odom_topic"], GSOF_INS, self.ins_state_ros, queue_size=1 + self.ins_state_sub = ros.Subscriber( + topic_names["nav_odom_topic"], GsofIns, self.ins_state_ros, queue_size=1 ) - self.raw_msg_sub = rospy.Subscriber( + self.raw_msg_sub = ros.Subscriber( "/rawmsg", std_msgs.msg.String, self.cb_raw_message_popup, queue_size=1 ) # -------------------------------------------------------------------- # ------------------------- Add To Event Log-------------------------- - self._left_sys_event_log_srv = rospy.ServiceProxy( + self._left_sys_event_log_srv = ros.ServiceProxy( topic_names["left_sys_event_log_srv"], AddToEventLog, persistent=False ) - self._center_sys_event_log_srv = rospy.ServiceProxy( + self._center_sys_event_log_srv = ros.ServiceProxy( topic_names["center_sys_event_log_srv"], AddToEventLog, persistent=False ) - self._right_sys_event_log_srv = rospy.ServiceProxy( + self._right_sys_event_log_srv = ros.ServiceProxy( topic_names["right_sys_event_log_srv"], AddToEventLog, persistent=False ) # -------------------------------------------------------------------- @@ -557,7 +557,7 @@ def __init__( # -------------------------------------------------------------------- - # rospy.add_client_shutdown_hook(self.on_close_button) + # ros shutdown hook handled in on_close_button self.system = SystemCommands(self.hosts) self.Bind(wx.EVT_CLOSE, self.when_closed) @@ -656,7 +656,7 @@ def _system_sanity_worker(self, hosts): entry["ssd_err"] = True unmounts.append(host) except (requests.exceptions.RequestException, ValueError, KeyError): - rospy.logwarn( + ros.logwarn( "Could not access disk info from system %s." % host ) results[host] = entry @@ -678,7 +678,7 @@ def _system_sanity_worker(self, hosts): # Attempt to remount any host that responded but wasn't mounted. if unmounts: - rospy.logerr( + ros.logerr( "ERROR: One or more hosts has an ssd mount issue: {}".format( unmounts ) @@ -693,7 +693,7 @@ def _system_sanity_worker(self, hosts): except requests.exceptions.RequestException: pass if nas_unmounts: - rospy.logerr( + ros.logerr( "ERROR: One or more hosts has a NAS mount issue: {}".format( nas_unmounts ) @@ -1698,7 +1698,7 @@ def on_slow_timer(self, event): def on_timer(self, event): """Manages all updates that should happen at fixed rate.""" tic = time.time() - if rospy.is_shutdown(): + if ros.is_shutdown(): self.on_close_button(None) # The startup unclip runs before GTK finalizes panel sizes, so labels @@ -1741,7 +1741,7 @@ def on_timer(self, event): zoom.update_all_if_needed() fit.update_all_if_needed() except AttributeError as e: - rospy.logwarn(e) + ros.logwarn(e) # self._image_inspection_frame.Close() # self._image_inspection_frame = None pass @@ -1971,7 +1971,7 @@ def update_project_flight_params(self, collecting=None): "base": SYS_CFG["arch"]["base"], } if collecting is not None: - rospy.set_param("/sys/arch/is_archiving", int(collecting)) + kv.put("/sys/arch/is_archiving", int(collecting)) redis_dict.update({"is_archiving": int(collecting)}) sysdir = get_arch_path() SYS_CFG["syscfg_dir"] = sysdir @@ -2236,7 +2236,7 @@ def message_popup_throttle(self, txt, throttle=None): td_throttle = datetime.timedelta(seconds=throttle) dt = now - self.last_popup if dt < td_throttle: - rospy.logwarn("suppressed, dt too short: {} : {}".format(dt, txt)) + ros.logwarn("suppressed, dt too short: {} : {}".format(dt, txt)) return icon = wx.ICON_ERROR if "error" in txt.lower() else wx.ICON_INFORMATION dlg = wx.MessageDialog(self, txt, "Info", wx.OK | icon) @@ -2246,20 +2246,20 @@ def message_popup_throttle(self, txt, throttle=None): def ins_state_ros(self, msg): """ - :param msg: INS POSAVX message. - :type msg: POSAVX + :param msg: INS GsofIns message. + :type msg: GsofIns """ # Throttle to 10hz - if msg.header.stamp.to_sec() - self.last_ins_time < 0.1: + if ros.stamp_to_sec(msg.header.stamp) - self.last_ins_time < 0.1: return - self.last_ins_time = msg.header.stamp.to_sec() + self.last_ins_time = ros.stamp_to_sec(msg.header.stamp) wx.CallAfter(self.ins_state, msg) def ins_state(self, msg): """ :param msg: INS message. - :type msg: POSAVX + :type msg: GsofIns """ if self._spoof_gps: @@ -2835,20 +2835,20 @@ def on_ir_nuc(self, event=None): """Request that the IR cameras execute NUC.""" for host in self.hosts: topic = os.path.join("/", host, "ir", "nuc") - service = rospy.ServiceProxy(topic, CamSetAttr, persistent=False) + service = ros.ServiceProxy(topic, CamSetAttr, persistent=False) # These values aren't used currently, just an overload to trigger nuc msg = "/{}/{}/{}:={}".format(host, "ir", "nuc", "manual") try: resp = service.call(name="nuc", value="manual") - except rospy.service.ServiceException: + except ros.ServiceException: errmsg = "Attempted to set `{}`, but system did not respond".format(msg) - rospy.logerr(errmsg) + ros.logerr(errmsg) continue if not resp: errmsg = "Attempted to set `{}`, but it failed".format(msg) - rospy.logerr(errmsg) + ros.logerr(errmsg) continue - rospy.loginfo(msg) + ros.loginfo(msg) self.add_to_event_log( "command sent: Request to manual NUC cameras {}. ".format(msg) ) @@ -2920,7 +2920,7 @@ def when_closed(self, event=None): # self.on_resize.Unbind(wx.EVT_SIZE) save_config_settings() self.timer.Unbind(wx.EVT_TIMER) - self.ins_state_sub.unregister() + ros.node().destroy_subscription(self.ins_state_sub) try: self._metadata_entry_frame.Close() except: @@ -2931,6 +2931,7 @@ def when_closed(self, event=None): except: pass + ros.shutdown() event.Skip() diff --git a/src/kitware-ros-pkg/wxpython_gui/scripts/system_control_panel_node.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel_main.py similarity index 97% rename from src/kitware-ros-pkg/wxpython_gui/scripts/system_control_panel_node.py rename to src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel_main.py index 204ce629..fd6a9186 100755 --- a/src/kitware-ros-pkg/wxpython_gui/scripts/system_control_panel_node.py +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/system_control_panel_main.py @@ -6,7 +6,6 @@ import os import wx -import rospy from wxpython_gui.system_control_panel.gui import MainFrame from roskv.impl.redis_envoy import RedisEnvoy @@ -15,7 +14,6 @@ def main(): node_name = "system_control_panel_node" - name_space = rospy.get_namespace() envoy = RedisEnvoy(os.environ["REDIS_HOST"], client_name=node_name) enabled = envoy.get("/sys/enabled") diff --git a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/utils.py b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/utils.py index 15fb37e4..388b7fec 100644 --- a/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/utils.py +++ b/src/kitware-ros-pkg/wxpython_gui/src/wxpython_gui/utils.py @@ -6,7 +6,6 @@ from collections import OrderedDict from collections.abc import Iterable -import rospy def make_path(path, from_file=False, verbose=False): diff --git a/src/run_scripts/entry/gui.sh b/src/run_scripts/entry/gui.sh index 6a40ee9b..2a440e37 100755 --- a/src/run_scripts/entry/gui.sh +++ b/src/run_scripts/entry/gui.sh @@ -12,7 +12,7 @@ source ${KAM_REPO_DIR}/src/cfg/cfg-aliases.sh # get cq - ConfigQuery source /entry/project.sh source /aliases.sh -for VNAME in CFG_ALIAS_SET ROS_MASTER_URI DATA_MOUNT_POINT +for VNAME in CFG_ALIAS_SET ROS_DOMAIN_ID DATA_MOUNT_POINT do if [[ -z "${!VNAME}" ]] then @@ -25,12 +25,9 @@ done NODE_HOSTNAME=${NODE_HOSTNAME:-undefined} -#pip install --user "src/core/roskv/[redis]" -#catkin build custom_msgs roskv -#TODO TESTING CHANGES DON"T SAVE if [[ -n "${START_IN_SHELL}" ]]; then bash else - exec roslaunch --wait wxpython_gui system_control_panel.launch + exec ros2 launch wxpython_gui system_control_panel.launch.xml fi \ No newline at end of file diff --git a/tmux/nayak/start_gui.sh b/tmux/nayak/start_gui.sh index 34917088..2cf47ea2 100755 --- a/tmux/nayak/start_gui.sh +++ b/tmux/nayak/start_gui.sh @@ -7,10 +7,6 @@ xhost + source $DIR/startup.sh source $DIR/env.sh -if [ ${REDIS_HOST} = "localhost" ]; then - roscore& -fi - echo "Start gui." cd $DIR/../.. diff --git a/tmux/taiga/start_gui.sh b/tmux/taiga/start_gui.sh index 34917088..2cf47ea2 100755 --- a/tmux/taiga/start_gui.sh +++ b/tmux/taiga/start_gui.sh @@ -7,10 +7,6 @@ xhost + source $DIR/startup.sh source $DIR/env.sh -if [ ${REDIS_HOST} = "localhost" ]; then - roscore& -fi - echo "Start gui." cd $DIR/../.. From eb4446d3051c1d2df27da77ebab227322c33d040 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Fri, 3 Jul 2026 21:56:46 -0400 Subject: [PATCH 15/20] Port all remaining ROS1 code to ROS2; repo is now ROS1-free - sysinfo: syscall service node -> rclpy console script (ament_python); syscall.sh uses ros2 launch - kamerahealth: missed_frame_node and exit_code_node -> rclpy (ament_python); health_node.py deleted - it was a byte-for-byte copy of sysinfo's syscall node, never a health check - testbed: test_roskv exerciser -> rclcpp/ament - sensor_simulator: camera/INS simulators -> rclpy console scripts; GSOF_INS -> GsofIns, genpy stamps -> builtin_interfaces - sprokit_adapters debug scripts (publish_sync_msgs, rebroadcast, save_images/chips) and kw_genicam display_latency -> rclpy; scripts now installed by the package - bag_file_explode: rosbag_to_png rewritten on the 'rosbags' pip library, which reads the legacy ROS1 .bag archives this tool exists for (plus ROS2 bags) with no ROS1 install; drops the py2-era multiprocessing time-slicing for a straightforward sequential read - Deleted kwiver_ros_param_interface and rqt_sprokit_adapter: both were already CATKIN_IGNOREd (disabled even under ROS1) and are built on ROS1-only mechanisms (global param server, dynamic_reconfigure rqt) with no ROS2 equivalent or consumer - Entry/dev scripts: spoofins/publish_sync_msgs/healthcheck/health_cam -> ros2 equivalents; wat.sh rewritten around ros2 doctor (no master to hunt for); aliases.sh dev shortcuts -> ros2/colcon; deleted debay.sh and ros2jaeger.sh (referenced packages that do not exist in the repo) and EXPORT_ROS_MASTER.sh --- .../scripts/display_latency.py | 17 +- src/core/kamerahealth/CMakeLists.txt | 199 ---------------- src/core/kamerahealth/launch/exit_code.launch | 19 -- .../kamerahealth/launch/exit_code.launch.xml | 13 ++ src/core/kamerahealth/launch/health.launch | 14 -- .../kamerahealth/launch/missed_frame.launch | 14 -- .../launch/missed_frame.launch.xml | 8 + src/core/kamerahealth/nodes/exit_code_node.py | 35 --- src/core/kamerahealth/nodes/health_node.py | 43 ---- src/core/kamerahealth/package.xml | 64 +---- .../{__init__.py => resource/kamerahealth} | 0 src/core/kamerahealth/setup.cfg | 4 + src/core/kamerahealth/setup.py | 41 ++-- .../src/kamerahealth/exit_code_node.py | 51 ++++ .../kamerahealth}/missed_frame_node.py | 36 ++- src/core/testbed/CMakeLists.txt | 145 +----------- src/core/testbed/launch/test_roskv.launch | 15 -- src/core/testbed/launch/test_roskv.launch.xml | 5 + src/core/testbed/package.xml | 63 +---- src/core/testbed/src/test_roskv.cpp | 19 +- .../bag_file_explode/CMakeLists.txt | 202 +--------------- .../bag_file_explode/package.xml | 65 +----- .../bag_file_explode/scripts/rosbag_to_png.py | 220 ++++++------------ .../kwiver_ros_param_interface/CATKIN_IGNORE | 0 .../kwiver_ros_param_interface/CMakeLists.txt | 59 ----- .../kwiver_ros_param_interface/README.md | 1 - .../dynamic_config_ros.cxx | 50 ---- .../dynamic_config_ros.h | 40 ---- .../kwiver_ros_param_interface.h | 31 --- .../kwiver_ros_param_interface/package.xml | 31 --- .../register_algorithms.cxx | 35 --- .../rqt_sprokit_adapter/CATKIN_IGNORE | 0 .../rqt_sprokit_adapter/CMakeLists.txt | 196 ---------------- .../rqt_sprokit_adapter/package.xml | 56 ----- .../rqt_sprokit_adapter/plugin.xml | 17 -- .../resource/rqt_sprokit_adapter.ui | 84 ------- .../scripts/rqt_sprokit_adapter | 10 - .../rqt_sprokit_adapter/setup.py | 14 -- .../src/rqt_sprokit_adapter/__init__.py | 0 .../rqt_sprokit_adapter/sprokit_adapter.py | 130 ----------- .../sensor_simulator/CMakeLists.txt | 201 ---------------- .../launch/simulate_cameras.launch | 19 -- .../launch/simulate_cameras.launch.xml | 11 + .../launch/simulate_cameras_one_sys.launch | 15 -- .../launch/simulate_ins.launch | 48 ---- .../launch/simulate_ins.launch.xml | 34 +++ .../sensor_simulator/package.xml | 63 +---- .../resource/sensor_simulator} | 0 .../sensor_simulator/scripts/simulate_ins.py | 89 ------- .../sensor_simulator/setup.cfg | 4 + src/kitware-ros-pkg/sensor_simulator/setup.py | 34 ++- .../src/sensor_simulator/camera_simulator.py | 34 +-- .../sensor_simulator}/simulate_cameras.py | 46 ++-- .../src/sensor_simulator/simulate_ins.py | 98 ++++++++ .../sprokit_adapters/CMakeLists.txt | 6 + .../scripts/publish_sync_msgs.py | 56 +++-- .../rebroadcast_infrequent_detections.py | 123 +++++----- .../scripts/save_detection_chips_to_disk.py | 60 +++-- .../scripts/save_images_to_disk.py | 71 +++--- src/process/sysinfo/CMakeLists.txt | 199 ---------------- src/process/sysinfo/launch/syscall.launch | 14 -- src/process/sysinfo/launch/syscall.launch.xml | 8 + src/process/sysinfo/nodes/__init__.py | 0 src/process/sysinfo/nodes/syscall_node.py | 43 ---- src/process/sysinfo/package.xml | 62 +---- .../sysinfo/resource/sysinfo} | 0 src/process/sysinfo/scripts/__init__.py | 0 src/process/sysinfo/setup.cfg | 4 + src/process/sysinfo/setup.py | 32 ++- .../sysinfo/sysinfo}/__init__.py | 0 src/process/sysinfo/sysinfo/syscall_node.py | 59 +++++ src/run_scripts/EXPORT_ROS_MASTER.sh | 4 - src/run_scripts/aliases.sh | 42 ++-- src/run_scripts/diag/rosnet.sh | 7 +- src/run_scripts/entry/debay.sh | 9 - src/run_scripts/entry/health_cam.sh | 2 +- src/run_scripts/entry/healthcheck.sh | 2 +- src/run_scripts/entry/publish_sync_msgs.sh | 8 +- src/run_scripts/entry/ros2jaeger.sh | 11 - src/run_scripts/entry/spoofins.sh | 2 +- src/run_scripts/entry/syscall.sh | 7 +- src/run_scripts/entry/wat.sh | 35 +-- src/run_scripts/run_detector.sh | 3 +- 83 files changed, 809 insertions(+), 2802 deletions(-) delete mode 100644 src/core/kamerahealth/CMakeLists.txt delete mode 100644 src/core/kamerahealth/launch/exit_code.launch create mode 100644 src/core/kamerahealth/launch/exit_code.launch.xml delete mode 100644 src/core/kamerahealth/launch/health.launch delete mode 100644 src/core/kamerahealth/launch/missed_frame.launch create mode 100644 src/core/kamerahealth/launch/missed_frame.launch.xml delete mode 100755 src/core/kamerahealth/nodes/exit_code_node.py delete mode 100755 src/core/kamerahealth/nodes/health_node.py rename src/core/kamerahealth/{__init__.py => resource/kamerahealth} (100%) create mode 100644 src/core/kamerahealth/setup.cfg create mode 100755 src/core/kamerahealth/src/kamerahealth/exit_code_node.py rename src/core/kamerahealth/{nodes => src/kamerahealth}/missed_frame_node.py (70%) delete mode 100644 src/core/testbed/launch/test_roskv.launch create mode 100644 src/core/testbed/launch/test_roskv.launch.xml delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/CATKIN_IGNORE delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/CMakeLists.txt delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/README.md delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.cxx delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.h delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/include/kwiver_ros_param_interface/kwiver_ros_param_interface.h delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/package.xml delete mode 100644 src/kitware-ros-pkg/kwiver_ros_param_interface/register_algorithms.cxx delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/CATKIN_IGNORE delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/CMakeLists.txt delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/package.xml delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/plugin.xml delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/resource/rqt_sprokit_adapter.ui delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/scripts/rqt_sprokit_adapter delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/setup.py delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/__init__.py delete mode 100644 src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/sprokit_adapter.py delete mode 100755 src/kitware-ros-pkg/sensor_simulator/CMakeLists.txt delete mode 100644 src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch create mode 100644 src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch.xml delete mode 100644 src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras_one_sys.launch delete mode 100644 src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch create mode 100644 src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch.xml rename src/{core/kamerahealth/nodes/__init__.py => kitware-ros-pkg/sensor_simulator/resource/sensor_simulator} (100%) mode change 100755 => 100644 delete mode 100755 src/kitware-ros-pkg/sensor_simulator/scripts/simulate_ins.py create mode 100644 src/kitware-ros-pkg/sensor_simulator/setup.cfg rename src/kitware-ros-pkg/sensor_simulator/{scripts => src/sensor_simulator}/simulate_cameras.py (50%) create mode 100755 src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_ins.py delete mode 100644 src/process/sysinfo/CMakeLists.txt delete mode 100644 src/process/sysinfo/launch/syscall.launch create mode 100644 src/process/sysinfo/launch/syscall.launch.xml delete mode 100755 src/process/sysinfo/nodes/__init__.py delete mode 100755 src/process/sysinfo/nodes/syscall_node.py rename src/{core/kamerahealth/scripts/__init__.py => process/sysinfo/resource/sysinfo} (100%) delete mode 100644 src/process/sysinfo/scripts/__init__.py create mode 100644 src/process/sysinfo/setup.cfg rename src/{core/kamerahealth/src => process/sysinfo/sysinfo}/__init__.py (100%) create mode 100755 src/process/sysinfo/sysinfo/syscall_node.py delete mode 100755 src/run_scripts/EXPORT_ROS_MASTER.sh delete mode 100755 src/run_scripts/entry/debay.sh delete mode 100755 src/run_scripts/entry/ros2jaeger.sh diff --git a/src/cams/kw_genicam_driver/scripts/display_latency.py b/src/cams/kw_genicam_driver/scripts/display_latency.py index d015c4a9..156eabcc 100755 --- a/src/cams/kw_genicam_driver/scripts/display_latency.py +++ b/src/cams/kw_genicam_driver/scripts/display_latency.py @@ -2,13 +2,14 @@ from __future__ import print_function import time -import rospy +import rclpy +from rclpy.node import Node from sensor_msgs.msg import Image def cb(msg): now = time.time() - msg_time = msg.header.stamp.secs + (msg.header.stamp.nsecs / 1000000000.0) + msg_time = msg.header.stamp.sec + (msg.header.stamp.nanosec / 1000000000.0) print("===") print("now : %f" % now) print("msg time : %f" % msg_time) @@ -16,8 +17,12 @@ def cb(msg): print("now delta : %f" % (now - msg_time)) -rospy.init_node("latency_reader", anonymous=True) -rospy.Subscriber("/test/camera/cueing/0/image_raw", Image, - cb, queue_size=1) +def main(args=None): + rclpy.init(args=args) + node = Node("latency_reader") + node.create_subscription(Image, "/test/camera/cueing/0/image_raw", cb, 1) + rclpy.spin(node) -rospy.spin() + +if __name__ == "__main__": + main() diff --git a/src/core/kamerahealth/CMakeLists.txt b/src/core/kamerahealth/CMakeLists.txt deleted file mode 100644 index 9235f6bc..00000000 --- a/src/core/kamerahealth/CMakeLists.txt +++ /dev/null @@ -1,199 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kamerahealth) - -## 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 - custom_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 exec_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 exec_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 exec_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 nexus -# CATKIN_DEPENDS roscpp 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}/nexus.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/nexus_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_nexus.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) diff --git a/src/core/kamerahealth/launch/exit_code.launch b/src/core/kamerahealth/launch/exit_code.launch deleted file mode 100644 index 3783e56d..00000000 --- a/src/core/kamerahealth/launch/exit_code.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/src/core/kamerahealth/launch/exit_code.launch.xml b/src/core/kamerahealth/launch/exit_code.launch.xml new file mode 100644 index 00000000..74f83d28 --- /dev/null +++ b/src/core/kamerahealth/launch/exit_code.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/src/core/kamerahealth/launch/health.launch b/src/core/kamerahealth/launch/health.launch deleted file mode 100644 index b100ec16..00000000 --- a/src/core/kamerahealth/launch/health.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/core/kamerahealth/launch/missed_frame.launch b/src/core/kamerahealth/launch/missed_frame.launch deleted file mode 100644 index 34af3486..00000000 --- a/src/core/kamerahealth/launch/missed_frame.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/core/kamerahealth/launch/missed_frame.launch.xml b/src/core/kamerahealth/launch/missed_frame.launch.xml new file mode 100644 index 00000000..4164fe88 --- /dev/null +++ b/src/core/kamerahealth/launch/missed_frame.launch.xml @@ -0,0 +1,8 @@ + + + + + + + diff --git a/src/core/kamerahealth/nodes/exit_code_node.py b/src/core/kamerahealth/nodes/exit_code_node.py deleted file mode 100755 index c51c64e9..00000000 --- a/src/core/kamerahealth/nodes/exit_code_node.py +++ /dev/null @@ -1,35 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import sys -import rospy -from std_msgs.msg import Int8 - - -def cb_shutdown(msg): - rospy.signal_shutdown("received shutdown request message: {}".format(msg.data)) - - -def main(): - print("agrv: {}".format(sys.argv)) - rospy.init_node("exit_code_node") - exit_code = rospy.get_param("exit_code_node/exit_code", 0) - do_spin = rospy.get_param("exit_code_node/spin", False) - rospy.Subscriber("/shutdown", Int8, cb_shutdown, queue_size=10) - print("param spin: {} exit_code: {}".format(do_spin, exit_code)) - - if do_spin: - rospy.spin() - - if exit_code == 0: - rospy.loginfo("Clean shutdown requested") - else: - rospy.logwarn("Code shutdown requested: {}".format(exit_code)) - - sys.exit(exit_code) - - -if __name__ == "__main__": - main() - # test with the following: - # roslaunch kamerahealth exit_code.launch exit_code:=1 spin:=0 || echo "exit=$?" diff --git a/src/core/kamerahealth/nodes/health_node.py b/src/core/kamerahealth/nodes/health_node.py deleted file mode 100755 index 16c60b93..00000000 --- a/src/core/kamerahealth/nodes/health_node.py +++ /dev/null @@ -1,43 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import sys -import subprocess -import shlex -import rospy -from custom_msgs.srv import SysCall - -USE_SHELL = False - - -def syscall_cb(msg): - cmdlist = shlex.split(msg.cmd) - rospy.loginfo(cmdlist) - try: - proc = subprocess.Popen( - cmdlist, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=USE_SHELL - ) - except Exception as exc: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("subprocess failed: {}: {}".format(exc_type, value)) - return ('', '{}: {}'.format(exc_type, value)) - try: - outs, errs = proc.communicate() - except Exception as exc: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("subprocess failed: {}: {}".format(exc_type, value)) - proc.kill() - outs, errs = proc.communicate() - stdout = outs.decode() if outs else "" - stderr = errs.decode() if errs else "" - return (stdout, stderr) - - -def main(): - rospy.init_node("syscall") - syscall_service = rospy.Service("syscall", SysCall, syscall_cb) - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/src/core/kamerahealth/package.xml b/src/core/kamerahealth/package.xml index d901f887..04dc8004 100644 --- a/src/core/kamerahealth/package.xml +++ b/src/core/kamerahealth/package.xml @@ -1,68 +1,20 @@ - + + kamerahealth - 0.1.5 - Provides system info + 1.0.0 + Health monitoring utilities for KAMERA - - - Adam Romlein - Michael McDermott - - - - - Apache 2.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - custom_msgs - rospy - std_msgs - rospy + rclpy std_msgs custom_msgs + python3-numpy - - - - + + ament_python diff --git a/src/core/kamerahealth/__init__.py b/src/core/kamerahealth/resource/kamerahealth similarity index 100% rename from src/core/kamerahealth/__init__.py rename to src/core/kamerahealth/resource/kamerahealth diff --git a/src/core/kamerahealth/setup.cfg b/src/core/kamerahealth/setup.cfg new file mode 100644 index 00000000..97490824 --- /dev/null +++ b/src/core/kamerahealth/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/kamerahealth +[install] +install_scripts=$base/lib/kamerahealth diff --git a/src/core/kamerahealth/setup.py b/src/core/kamerahealth/setup.py index 6cf2a914..bbbe7bdc 100755 --- a/src/core/kamerahealth/setup.py +++ b/src/core/kamerahealth/setup.py @@ -1,16 +1,29 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['kamerahealth'], - package_dir={'': 'src'}, - install_requires=['flask', 'redis', 'six'], - scripts=["scripts/y2j", "scripts/check_drop_rate.py"] - # entry_points={'console_scripts': [ - # 'redis_arpd=kamerahealth.redis_arpd:main', - # 'y2j=kamerahealth.y2j:main', - # ]}, - ) +from setuptools import setup -setup(**d) +package_name = "kamerahealth" + +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Health monitoring utilities for KAMERA", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "missed_frame_node = kamerahealth.missed_frame_node:main", + "exit_code_node = kamerahealth.exit_code_node:main", + ], + }, +) diff --git a/src/core/kamerahealth/src/kamerahealth/exit_code_node.py b/src/core/kamerahealth/src/kamerahealth/exit_code_node.py new file mode 100755 index 00000000..d190ccdd --- /dev/null +++ b/src/core/kamerahealth/src/kamerahealth/exit_code_node.py @@ -0,0 +1,51 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- + +import sys + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int8 + + +class ExitCodeNode(Node): + """Test node: exits with a configurable code, optionally spinning until a + /shutdown message arrives.""" + + def __init__(self): + super().__init__("exit_code_node") + self.exit_code = self.declare_parameter("exit_code", 0).value + self.do_spin = self.declare_parameter("spin", False).value + self.shutdown_requested = False + self.create_subscription(Int8, "/shutdown", self.cb_shutdown, 10) + print("param spin: {} exit_code: {}".format(self.do_spin, self.exit_code)) + + def cb_shutdown(self, msg): + self.get_logger().info( + "received shutdown request message: {}".format(msg.data)) + self.shutdown_requested = True + + +def main(args=None): + print("argv: {}".format(sys.argv)) + rclpy.init(args=args) + node = ExitCodeNode() + + if node.do_spin: + while rclpy.ok() and not node.shutdown_requested: + rclpy.spin_once(node, timeout_sec=0.1) + + if node.exit_code == 0: + node.get_logger().info("Clean shutdown requested") + else: + node.get_logger().warning( + "Code shutdown requested: {}".format(node.exit_code)) + + exit_code = node.exit_code + node.destroy_node() + rclpy.shutdown() + sys.exit(exit_code) + + +if __name__ == "__main__": + main() diff --git a/src/core/kamerahealth/nodes/missed_frame_node.py b/src/core/kamerahealth/src/kamerahealth/missed_frame_node.py similarity index 70% rename from src/core/kamerahealth/nodes/missed_frame_node.py rename to src/core/kamerahealth/src/kamerahealth/missed_frame_node.py index 53f0d3c1..dd8899ba 100755 --- a/src/core/kamerahealth/nodes/missed_frame_node.py +++ b/src/core/kamerahealth/src/kamerahealth/missed_frame_node.py @@ -6,11 +6,16 @@ import json import sys import numpy as np -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Header from custom_msgs.msg import Stat +def stamp_to_sec(stamp): + return stamp.sec + stamp.nanosec * 1e-9 + + class LowpassIIR(object): """ @@ -40,15 +45,18 @@ def state(self): return self._state -class MissedFrameAgg(object): +class MissedFrameAgg(Node): def __init__(self): + super().__init__("missed_listener") self.missed_frames = [] self.intervals = [] self._last_time = time.time() self.period_iir = None self.init_latch = True - self.sub_missed_frames = rospy.Subscriber("/missed_frames", Header, self.missed_msg_cb) - self.sub_errstat = rospy.Subscriber("/errstat", Stat, self.errstat_cb) + self.sub_missed_frames = self.create_subscription( + Header, "/missed_frames", self.missed_msg_cb, 10) + self.sub_errstat = self.create_subscription( + Stat, "/errstat", self.errstat_cb, 10) now = int(time.time()) self.out_file = '/mnt/flight_data/miketest/missed_agg/{}.jsonl'.format(now) @@ -68,13 +76,13 @@ def trig_freq_cb(self, msg): def errstat_cb(self, msg): header = msg.trace_header - data = {'type': 'errstat', 'time': str(header.stamp.to_sec()), 'frame_id': header.frame_id, 'note': msg.note, + data = {'type': 'errstat', 'time': str(stamp_to_sec(header.stamp)), 'frame_id': header.frame_id, 'note': msg.note, 'link': msg.link} with open(self.out_file, 'a') as fp: json.dump(data, fp) fp.write('\n') - rospy.loginfo('{}: {}'.format(msg.link, msg.note)) + self.get_logger().info('{}: {}'.format(msg.link, msg.note)) def missed_msg_cb(self, msg): self.missed_frames.append(msg) @@ -84,21 +92,27 @@ def missed_msg_cb(self, msg): hz = 1.0 / iir_s med = np.median(self.intervals) now = time.time() - rospy.loginfo( + self.get_logger().info( "{: <14}: last interval: {: >2.3f} iir: {: >2.3f}s iir {: >2.3f}Hz Median: {: 2.3f}s {: 2.3f}Hz".format( msg.frame_id, elapsed, iir_s, hz, med, 1.0/med, ) ) - data = { 'type': 'missed_frames', 'time': str(msg.stamp.to_sec()), 'frame_id': msg.frame_id, 'elapsed': elapsed, 'iir_s': iir_s, 'med_hz': 1.0/med} + data = { 'type': 'missed_frames', 'time': str(stamp_to_sec(msg.stamp)), 'frame_id': msg.frame_id, 'elapsed': elapsed, 'iir_s': iir_s, 'med_hz': 1.0/med} with open(self.out_file, 'a') as fp: json.dump(data, fp) fp.write('\n') -def main(): - rospy.init_node("missed_listener") +def main(args=None): + rclpy.init(args=args) app = MissedFrameAgg() - rospy.spin() + try: + rclpy.spin(app) + except KeyboardInterrupt: + pass + finally: + app.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/core/testbed/CMakeLists.txt b/src/core/testbed/CMakeLists.txt index 11163806..f163fdaf 100644 --- a/src/core/testbed/CMakeLists.txt +++ b/src/core/testbed/CMakeLists.txt @@ -1,151 +1,26 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(testbed) include(CMakePrintHelpers) -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -## 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 - roskv -) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(roskv REQUIRED) find_package(nlohmann_json REQUIRED) - -# <------------ add hiredis dependency ---------------> find_path(HIREDIS_HEADER hiredis) find_path(REDIS_PLUS_PLUS_HEADER sw) find_library(HIREDIS_LIB hiredis) find_library(REDIS_PLUS_PLUS_LIB redis++) -## NOTE: this should be *sw* NOT *redis++* - - - - - -## 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() - - -################################### -## 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 nexus -# CATKIN_DEPENDS roscpp 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} -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS}) add_executable(test_roskv_node src/test_roskv.cpp) - - +ament_target_dependencies(test_roskv_node rclcpp roskv) target_link_libraries(test_roskv_node - ${catkin_LIBRARIES} - ${nlohmann_json_LIBRARIES} + nlohmann_json::nlohmann_json ${HIREDIS_LIB} ${REDIS_PLUS_PLUS_LIB}) target_include_directories(test_roskv_node PUBLIC ${REDIS_PLUS_PLUS_HEADER}) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/nexus.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/nexus_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 roskv -# 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_nexus.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(TARGETS test_roskv_node DESTINATION lib/${PROJECT_NAME}) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) -cmake_print_variables(nlohmann_json_LIBRARIES REDIS_PLUS_PLUS_HEADER REDIS_PLUS_PLUS_LIB) +ament_package() diff --git a/src/core/testbed/launch/test_roskv.launch b/src/core/testbed/launch/test_roskv.launch deleted file mode 100644 index 455ae73e..00000000 --- a/src/core/testbed/launch/test_roskv.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/src/core/testbed/launch/test_roskv.launch.xml b/src/core/testbed/launch/test_roskv.launch.xml new file mode 100644 index 00000000..30749a22 --- /dev/null +++ b/src/core/testbed/launch/test_roskv.launch.xml @@ -0,0 +1,5 @@ + + + + diff --git a/src/core/testbed/package.xml b/src/core/testbed/package.xml index 0bda4301..db51fd41 100644 --- a/src/core/testbed/package.xml +++ b/src/core/testbed/package.xml @@ -1,67 +1,20 @@ - + + testbed - 0.1.0 + 1.0.0 Testing ROS packaging and stuff - - - Adam Romlein - - - - - Apache 2.0 + ament_cmake - - - - - - - - - - + rclcpp + roskv + nlohmann-json-dev - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - roskv - rospy - std_msgs - rospy - std_msgs - roskv - - - - - + ament_cmake diff --git a/src/core/testbed/src/test_roskv.cpp b/src/core/testbed/src/test_roskv.cpp index d6afff9a..e9e5975f 100644 --- a/src/core/testbed/src/test_roskv.cpp +++ b/src/core/testbed/src/test_roskv.cpp @@ -6,7 +6,9 @@ #include #include #include -#include +#include + +#define ROS_INFO(...) RCLCPP_INFO(rclcpp::get_logger("test_roskv"), __VA_ARGS__) // No Color #define NC "\033[0m" @@ -169,21 +171,20 @@ bool test_envoy() { } int main(int argc,char** argv) { - ros::init(argc, argv, "test_roskv"); - ros::start(); - ros::NodeHandle nh; - auto end_ = nh.createTimer(ros::Duration(1.0), - [](const ros::TimerEvent &event) { + rclcpp::init(argc, argv); + auto nh = std::make_shared("test_roskv"); + auto end_ = nh->create_wall_timer(std::chrono::seconds(1), + []() { ROS_GREEN("complete!"); - ros::shutdown(); - }, true, true); + rclcpp::shutdown(); + }); ROS_INFO("ros started"); test_json(); test_redis(); test_roskv(); test_with_env(); test_envoy(); - ros::spin(); + rclcpp::spin(nh); std::cout << "clean exit" << std::endl; return 0; } \ No newline at end of file diff --git a/src/kitware-ros-pkg/bag_file_explode/CMakeLists.txt b/src/kitware-ros-pkg/bag_file_explode/CMakeLists.txt index 2a5087bd..c7de4822 100644 --- a/src/kitware-ros-pkg/bag_file_explode/CMakeLists.txt +++ b/src/kitware-ros-pkg/bag_file_explode/CMakeLists.txt @@ -1,199 +1,13 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(bag_file_explode) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +find_package(ament_cmake REQUIRED) -## 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 - cv_bridge - rosbag - rospy +install(PROGRAMS + scripts/rosbag_to_png.py + scripts/explode_bag_files.sh + scripts/explode_directories.sh + DESTINATION lib/${PROJECT_NAME} ) -## 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 bag_file_explode -# CATKIN_DEPENDS 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}/bag_file_explode.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/bag_file_explode_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_bag_file_explode.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) +ament_package() diff --git a/src/kitware-ros-pkg/bag_file_explode/package.xml b/src/kitware-ros-pkg/bag_file_explode/package.xml index e4d7baa1..024672f9 100644 --- a/src/kitware-ros-pkg/bag_file_explode/package.xml +++ b/src/kitware-ros-pkg/bag_file_explode/package.xml @@ -1,64 +1,21 @@ - + + bag_file_explode - 0.1.0 - The bag_file_explode package + 1.0.0 + Explode legacy bag files into images + nav yaml (uses the rosbags library) - - - - Adam Romlein - - - - - + Adam Romlein + Adam Romlein Apache 2.0 + ament_cmake - - - - - - - - - - - Adam Romlein - + python3-opencv + python3-numpy + python3-yaml - - - - - - - - - - - - - - - - - - - - - catkin - - cv_bridge - rosbag - rospy - - - - - + ament_cmake diff --git a/src/kitware-ros-pkg/bag_file_explode/scripts/rosbag_to_png.py b/src/kitware-ros-pkg/bag_file_explode/scripts/rosbag_to_png.py index b4d013f7..cdf823fc 100755 --- a/src/kitware-ros-pkg/bag_file_explode/scripts/rosbag_to_png.py +++ b/src/kitware-ros-pkg/bag_file_explode/scripts/rosbag_to_png.py @@ -1,128 +1,80 @@ #! /usr/bin/python +"""Explode image/odometry topics from a bag file into PNGs + nav yaml. + +ROS2 port: reads bags via the `rosbags` library (pip install rosbags), which +understands both ROS1 .bag files (the legacy data this tool exists for) and +ROS2 bag directories, without needing a ROS environment at all. +""" from __future__ import print_function import argparse import logging -import multiprocessing as mp import os -from kamera.sensor_models import euler_from_quaternion -from kamera.sensor_models.nav_conversions import enu_quat_to_ned_quat import cv2 -from cv_bridge import CvBridge, CvBridgeError import numpy as np -import rosbag -import rospy import yaml -import time +from rosbags.highlevel import AnyReader +from rosbags.image import message_to_cvimage + +from kamera.sensor_models import euler_from_quaternion +from kamera.sensor_models.nav_conversions import enu_quat_to_ned_quat logging.basicConfig() LOG = logging.getLogger(__name__) LOG.setLevel(logging.INFO) -# First hard-coded mapping of ROS topic to output filename suffix part. -to_save = { - '/ros/topic': 'fname1', -} -save_dir = 'output' -filename = 'bagfile' -# Multiple shared memory objects to prevent mp errors -count = mp.Manager().list() -ids = mp.Manager().list() -check_dir = mp.Manager().list() +def stamp_to_sec(stamp): + # rosbags exposes ROS1 stamps as .sec/.nanosec too + return stamp.sec + stamp.nanosec * 1e-9 -def save_times(args): - start = rospy.Time.from_sec(args[0]) - end = rospy.Time.from_sec(args[1]) - - with rosbag.Bag(filename, 'r') as bag: - map(topic_wrapper, bag.read_messages(topics=to_save.keys(), - start_time=start, - end_time=end)) def odom_to_yaml(msg, directory): - """Process INS Odometry message. - - :param msg: Odometry message. - :type msg: Odometry - - """ - + """Process INS Odometry message.""" pose = msg.pose.pose lat = pose.position.y lon = pose.position.x - alt = pose.position.z - + alt = pose.position.z # ENU quaternion quat = np.array([pose.orientation.x, pose.orientation.y, - pose.orientation.z, pose.orientation.w]) + pose.orientation.z, pose.orientation.w]) yaw = euler_from_quaternion(enu_quat_to_ned_quat(quat), - axes='rzyx')[0]*180/np.pi + axes='rzyx')[0] * 180 / np.pi # Saves navigation info into a yaml file to be dynamically loaded later - yaml_lat = ('lat: ' + str(lat) + '\n') - yaml_lon = ('lon: ' + str(lon) + '\n') - yaml_alt = ('alt: ' + str(alt) + '\n') - yaml_yaw = ('yaw: ' + str(yaw) + '\n') - LOG.info("Logging Nav info into %s/nav_odom.yaml"%directory) - odom_yaml = open(os.path.join(directory, "nav_odom.yaml"), "w+") - odom_yaml.write(yaml_lat + yaml_lon + yaml_alt + yaml_yaw) - odom_yaml.close() - - -def topic_wrapper(args): - topic, msg, t = args - # Ensure nav yaml is only written once - if to_save[topic] == 'nav' and len(count) == 0: - odom_to_yaml(msg, str(save_dir)) - count.append(0) - elif to_save[topic] == 'nav' and len(count) != 0: - pass - else: - save_image(msg, to_save[topic], str(save_dir)) - - -def save_image(msg, name, directory): + LOG.info("Logging Nav info into %s/nav_odom.yaml" % directory) + with open(os.path.join(directory, "nav_odom.yaml"), "w+") as odom_yaml: + odom_yaml.write('lat: %s\n' % lat) + odom_yaml.write('lon: %s\n' % lon) + odom_yaml.write('alt: %s\n' % alt) + odom_yaml.write('yaw: %s\n' % yaw) + + +def save_image(msg, name, directory, ids): image_dir = os.path.join(directory, name) + os.makedirs(image_dir, exist_ok=True) - # Ensure makedirs is only called once to prevent error - if not os.path.isdir(image_dir): - if image_dir not in check_dir: - try: - os.makedirs(image_dir) - except OSError as e: - if e.errno!=os.errno.EEXIST: - raise - pass - check_dir.append(image_dir) - - frame_id = name + ": \"" + msg.header.frame_id + '\"\n' + frame_id = name + ': "' + msg.header.frame_id + '"\n' if frame_id not in ids: ids.append(frame_id) - # Use a CvBridge to convert ROS images to OpenCV images so they can be - # saved. - bridge = CvBridge() - try: - if msg.encoding == "bgr8": - cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") - elif msg.encoding == "rgb8": - cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") + if msg.encoding in ("bgr8", "rgb8"): + cv_image = message_to_cvimage(msg, "bgr8") elif msg.encoding == "32FC1": # Depth image. # NOTE: Assuming Zed camera properties. - raw_image = bridge.imgmsg_to_cv2(msg, "32FC1") + raw_image = message_to_cvimage(msg, "32FC1") # Make sense of nan/inf values raw_image[np.isnan(raw_image)] = 0 raw_image[np.isinf(raw_image)] = 0 # maybe 20? # Zed max range should only be 20 (meters) assert not (raw_image > 20).any(), \ - "Zed sensor is no supposed to report values over 20! " \ + "Zed sensor is not supposed to report values over 20! " \ "(found some...)" # Scale remaining non-zero values to 8-bit range, # cast to 8-bit image. @@ -131,75 +83,55 @@ def save_image(msg, name, directory): raise RuntimeError("Unexpected image format/encoding: '%s'" % msg.encoding) - timestr = "%.6f" % msg.header.stamp.to_sec() - image_name = str(image_dir)+"/"+timestr+"_"+name+".png" + timestr = "%.6f" % stamp_to_sec(msg.header.stamp) + image_name = os.path.join(image_dir, "%s_%s.png" % (timestr, name)) LOG.info("Saving image: %s" % image_name) cv2.imwrite(image_name, cv_image) - except CvBridgeError as e: + except Exception as e: LOG.error(str(e)) -class ImageCreator (object): - - def __init__(self): - global save_dir - global filename - global to_save - - # Get parameters as arguments to 'rosrun my_package bag_to_images.py - # ', where save_dir and filename exist relative to - # this executable file. - parser = argparse.ArgumentParser() - parser.add_argument('yaml_config', - help="YAML config file mapping topics to extract " - "with the output subdirectories to extract " - "to.") - parser.add_argument('output_dir', - help="Directory to output image sub-directories " - "to.") - parser.add_argument('bag_filepath', - help="Filesystem path to the bag file to explode.") - args, unknown = parser.parse_known_args() - - to_save = yaml.load(open(args.yaml_config)) - save_dir = args.output_dir - filename = args.bag_filepath - - - LOG.info("to-save map: %s" % to_save) - LOG.info("Output directory = %s" % save_dir) - LOG.info("Bag filename = %s" % filename) - - self.run_multiprocess() - - def run_multiprocess(self): - num_processors = mp.cpu_count() - pool = mp.Pool(num_processors) - - with rosbag.Bag(filename, 'r') as bag: - starttime = bag.get_start_time() - endtime = bag.get_end_time() - LOG.info("Bag start time and end time: %f -> %f" % (starttime, endtime)) - - step = (endtime - starttime) / float(num_processors) - times = [(starttime + (step*i), starttime + (step * (i + 1))) - for i in range(num_processors)] - LOG.info("Time slices:") - for T in times: - LOG.info(" %s" % (T,)) - assert times[-1][1] == endtime, \ - "Expected end time to be %f, got %f" % (endtime, times[-1][1]) - pool.map(save_times, times) - pool.close() - pool.join() - - ids_text = open(os.path.join(save_dir, "frame_ids.yaml"), "w+") - for _id in ids: +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('yaml_config', + help="YAML config file mapping topics to extract " + "with the output subdirectories to extract to.") + parser.add_argument('output_dir', + help="Directory to output image sub-directories to.") + parser.add_argument('bag_filepath', + help="Filesystem path to the bag file to explode.") + args, unknown = parser.parse_known_args() + + with open(args.yaml_config) as fp: + to_save = yaml.safe_load(fp) + save_dir = args.output_dir + filename = args.bag_filepath + + LOG.info("to-save map: %s" % to_save) + LOG.info("Output directory = %s" % save_dir) + LOG.info("Bag filename = %s" % filename) + + os.makedirs(save_dir, exist_ok=True) + ids = [] + wrote_nav = False + + from pathlib import Path + with AnyReader([Path(filename)]) as reader: + connections = [c for c in reader.connections if c.topic in to_save] + for connection, timestamp, rawdata in reader.messages(connections=connections): + msg = reader.deserialize(rawdata, connection.msgtype) + name = to_save[connection.topic] + if name == 'nav': + if not wrote_nav: + odom_to_yaml(msg, str(save_dir)) + wrote_nav = True + else: + save_image(msg, name, str(save_dir), ids) + + with open(os.path.join(save_dir, "frame_ids.yaml"), "w+") as ids_text: + for _id in ids: ids_text.write(_id) - ids_text.close() - if __name__ == '__main__': - # Go to class functions that do all the heavy lifting. Do error checking. - image_creator = ImageCreator() + main() diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/CATKIN_IGNORE b/src/kitware-ros-pkg/kwiver_ros_param_interface/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/CMakeLists.txt b/src/kitware-ros-pkg/kwiver_ros_param_interface/CMakeLists.txt deleted file mode 100644 index dcd55a89..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(kwiver_ros_param_interface) - -add_definitions(-std=c++11) - -## Find catkin and any catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs) - -## Declare a catkin package -#catkin_package() -catkin_package(CATKIN_DEPENDS roscpp std_msgs sensor_msgs kwiver - INCLUDE_DIRS include) - -find_package(KWIVER REQUIRED) -link_directories(${KWIVER_LIBRARY_DIR}) -#message("KWIVER_LIBRARIES: ${KWIVER_LIBRARIES}") -include(${KWIVER_CMAKE_DIR}/kwiver-utils.cmake) #kwiver_install_headers, etc - -include_directories(include - ${catkin_INCLUDE_DIRS} - ${KWIVER_INCLUDE_DIRS} - ${EIGEN_INCLUDE_DIR} - ${CMAKE_CURRENT_BINARY_DIR}) - -set(dynamic_config_ros_headers_public dynamic_config_ros.h) - -## Install KWIVER public header files to include/kwiver/... -kwiver_install_headers( - SUBDIR arrows/ros - ${dynamic_config_ros_headers_public} - ) - -kwiver_install_headers( - ${CMAKE_CURRENT_BINARY_DIR}/kwiver_algo_dynamic_config_ros_export.h - NOPATH SUBDIR arrows/ros - ) - -set(dynamic_config_ros_sources dynamic_config_ros.cxx) - -## Add a library to Kwiver -kwiver_add_library(kwiver_algo_ros ${dynamic_config_ros_headers_public} - ${dynamic_config_ros_sources}) - -#target_link_libraries( kwiver_algo_ros -# PUBLIC vital_algo -# kwiver_algo_core -# PRIVATE kwiversys -# kwiver_algo_ocv -# ) - -#target_link_libraries( kwiver_algo_ros -# ${catkin_LIBRARIES} -# ${KWIVER_LIBRARIES} -# ${OpenCV_LIBRARIES} -# ) - -## Generate and add a plug-in library to Kwiver based on kwiver_algo_ros -#algorithms_create_plugin(kwiver_algo_ros register_algorithms.cxx) - diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/README.md b/src/kitware-ros-pkg/kwiver_ros_param_interface/README.md deleted file mode 100644 index f59d3b78..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/README.md +++ /dev/null @@ -1 +0,0 @@ -Provides a C++ ROS parameter interface between kwiver and ROS. It allows Kwiver to read parameters from the ROS parameter server. diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.cxx b/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.cxx deleted file mode 100644 index 9eaf87e9..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.cxx +++ /dev/null @@ -1,50 +0,0 @@ - -/** - * \file - * \brief Header defining the implementation to dynamic_config_ros - */ - -#include "dynamic_config_ros.h" -#include "ros/ros.h" - -namespace kwiver { -namespace arrows { -namespace core { - - -// ------------------------------------------------------------------ -dynamic_config_ros:: -dynamic_config_ros() -{ } - - -// ------------------------------------------------------------------ -void -dynamic_config_ros:: -set_configuration( kwiver::vital::config_block_sptr config ) -{ } - - -// ------------------------------------------------------------------ -bool -dynamic_config_ros:: -check_configuration( kwiver::vital::config_block_sptr config ) const -{ - return true; -} - - -// ------------------------------------------------------------------ -kwiver::vital::config_block_sptr -dynamic_config_ros:: -get_dynamic_configuration() -{ - auto config_block = kwiver::vital::config_block::empty_config(); - int target_size; - ros::param::getCached("person_detect_target_size", target_size); - config_block->set_value("person_detect_target_size", target_size); - return config_block; -} - -} } } // end namespace - diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.h b/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.h deleted file mode 100644 index ca38393b..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/dynamic_config_ros.h +++ /dev/null @@ -1,40 +0,0 @@ - -/** - * \file - * \brief Header defining the interface to dynamic_config_ros - */ - -#ifndef ARROWS_CORE_DYNAMIC_CONFIG_NONE_H -#define ARROWS_CORE_DYNAMIC_CONFIG_NONE_H - -#include - -#include - -namespace kwiver { -namespace arrows { -namespace core { - -/// A class for bypassing image conversion -class KWIVER_ALGO_CORE_EXPORT dynamic_config_ros - : public vital::algorithm_impl -{ -public: - /// default constructor - dynamic_config_ros(); - - virtual void set_configuration( kwiver::vital::config_block_sptr config ); - virtual bool check_configuration( kwiver::vital::config_block_sptr config ) const; - - /// Return dynamic configuration values - /** - * This method returns dynamic configuration values. A valid config - * block is returned even if there are not values being returned. - */ - virtual kwiver::vital::config_block_sptr get_dynamic_configuration(); -}; - -} } } // end namespace - -#endif /* ARROWS_CORE_DYNAMIC_CONFIG_NONE_H */ - diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/include/kwiver_ros_param_interface/kwiver_ros_param_interface.h b/src/kitware-ros-pkg/kwiver_ros_param_interface/include/kwiver_ros_param_interface/kwiver_ros_param_interface.h deleted file mode 100644 index a578d1e4..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/include/kwiver_ros_param_interface/kwiver_ros_param_interface.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef __KWIVER_ROS_PARAM_INTERFACE__ -#define __KWIVER_ROS_PARAM_INTERFACE__ - -#include "ros/ros.h" -#include "std_msgs/Int32.h" -#include "sensor_msgs/JointState.h" - -#define WITH_ROS - - -template -class KwiverRosParamInterface{ -#ifdef WITH_ROS - ros::NodeHandle ros_node_handle_; - ros::Publisher pan_tilt_pub_; -#endif - std::string key_; - - public: - KwiverRosParamInterface(std::string key) : key_(key){} - ~KwiverRosParamInterface(); - - bool SetParam(DATA_T data){ -#ifdef WITH_ROS - return ros::param::set(key_, data); -#endif - } -}; - -#endif // __KWIVER_ROS_PARAM_INTERFACE__ - diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/package.xml b/src/kitware-ros-pkg/kwiver_ros_param_interface/package.xml deleted file mode 100644 index e88e32a4..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - kwiver_ros_param_interface - 0.0.0 - Kwiver ROS Parameter Interface - - Sam Petrocelli - - - - - TODO - - catkin - - roscpp - std_msgs - sensor_msgs - kwiver - - roscpp - std_msgs - sensor_msgs - kwiver - - - - - - - diff --git a/src/kitware-ros-pkg/kwiver_ros_param_interface/register_algorithms.cxx b/src/kitware-ros-pkg/kwiver_ros_param_interface/register_algorithms.cxx deleted file mode 100644 index eb2cc287..00000000 --- a/src/kitware-ros-pkg/kwiver_ros_param_interface/register_algorithms.cxx +++ /dev/null @@ -1,35 +0,0 @@ - -#include -#include - -#include - -namespace kwiver { -namespace arrows { -namespace darknet { - -extern "C" -KWIVER_ALGO_DARKNET_EXPORT -void -register_factories( kwiver::vital::plugin_loader& vpm ) -{ - static auto const module_name = std::string( "arrows.dynamic_config_ros" ); - if (vpm.is_module_loaded( module_name ) ) - { - return; - } - - // add factory implementation-name type-to-create - auto fact = vpm.ADD_ALGORITHM( "dynamic_config_ros", kwiver::arrows::darknet::dynamic_config_ros ); - fact->add_attribute( kwiver::vital::plugin_factory::PLUGIN_DESCRIPTION, - "Dynamic Configuration from ROS parameter server" ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_MODULE_NAME, module_name ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_VERSION, "1.0" ) - .add_attribute( kwiver::vital::plugin_factory::PLUGIN_ORGANIZATION, "Kitware Inc." ) - ; - - vpm.mark_module_as_loaded( module_name ); -} - -} } } // end namespace - diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/CATKIN_IGNORE b/src/kitware-ros-pkg/rqt_sprokit_adapter/CATKIN_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/CMakeLists.txt b/src/kitware-ros-pkg/rqt_sprokit_adapter/CMakeLists.txt deleted file mode 100644 index 03b5be12..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/CMakeLists.txt +++ /dev/null @@ -1,196 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_sprokit_adapter) - -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-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 - rqt_gui - rqt_gui_py -) - -## 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 you 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 rqt_sprokit_adapter -# CATKIN_DEPENDS rospy rqt_gui rqt_gui_py -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/rqt_sprokit_adapter.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/rqt_sprokit_adapter_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/rqt_sprokit_adapter - 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 resource - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_rqt_sprokit_adapter.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) diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/package.xml b/src/kitware-ros-pkg/rqt_sprokit_adapter/package.xml deleted file mode 100644 index 03e71213..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/package.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - rqt_sprokit_adapter - 0.0.0 - The rqt_sprokit_adapter package - - - - - Adam Romlein - - - - - - Apache 2.0 - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - rqt_gui - rqt_gui_py - rospy - rqt_gui - rqt_gui_py - - - - - - - - diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/plugin.xml b/src/kitware-ros-pkg/rqt_sprokit_adapter/plugin.xml deleted file mode 100644 index 76530b6c..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for Controlling the behavior of a Sprokit Adapter Node - - - - - folder - Plugins related to Configuration. - - - applications-other - A Python GUI plugin Controlling the behavior of a Sprokit Adapter Node. - - - diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/resource/rqt_sprokit_adapter.ui b/src/kitware-ros-pkg/rqt_sprokit_adapter/resource/rqt_sprokit_adapter.ui deleted file mode 100644 index 2822157a..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/resource/rqt_sprokit_adapter.ui +++ /dev/null @@ -1,84 +0,0 @@ - - - Form - - - - 0 - 0 - 400 - 300 - - - - Form - - - - - 20 - 60 - 136 - 32 - - - - Show Probablility Text - - - - - - 130 - 10 - 241 - 33 - - - - - - - 10 - 10 - 107 - 27 - - - - Topic - - - - - - 170 - 110 - 131 - 33 - - - - 1.000000000000000 - - - 0.050000000000000 - - - - - - 20 - 110 - 131 - 27 - - - - Threshold - - - - - - diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/scripts/rqt_sprokit_adapter b/src/kitware-ros-pkg/rqt_sprokit_adapter/scripts/rqt_sprokit_adapter deleted file mode 100644 index a78313ea..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/scripts/rqt_sprokit_adapter +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_sprokit_adapter.sprokit_adapter import SprokitAdapter -from rqt_gui.main import Main - -plugin = 'rqt_sprokit_adapter.sprokit_adapter.SprokitAdapter' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin,plugin_argument_provider=SprokitAdapter.add_arguments)) diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/setup.py b/src/kitware-ros-pkg/rqt_sprokit_adapter/setup.py deleted file mode 100644 index dac748ad..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/setup.py +++ /dev/null @@ -1,14 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['rqt_sprokit_adapter'], - package_dir={'': 'src'}, - requires=['rospy', 'std_msgs', 'std_srvs'], - scripts=['scripts/rqt_sprokit_adapter'] -) - -setup(**setup_args) diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/__init__.py b/src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/sprokit_adapter.py b/src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/sprokit_adapter.py deleted file mode 100644 index a73f4897..00000000 --- a/src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/sprokit_adapter.py +++ /dev/null @@ -1,130 +0,0 @@ -import os -import argparse -import rospy -import rospkg -import distutils.util - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -from diagnostic_msgs.msg import DiagnosticStatus, KeyValue - -class SprokitAdapterTopicHandler(object): - def __init__(self, topic): - self.topic = topic - self._publisher = rospy.Publisher(topic,DiagnosticStatus) - - def publish(self,showProbability=True,threshold=True): - msg = DiagnosticStatus() - msg.name = "SprokitAdapter" - msg.level = DiagnosticStatus.OK - msg.hardware_id = self.topic - msg.message = "No Message" - msg.values = [ - KeyValue(key='threshold',value=str(threshold)), - KeyValue(key='draw_text',value=str(showProbability)) - ] - if self._publisher is not None: - self._publisher.publish(msg) - - def close(self): - self._publisher.unregister() - del self._publisher - self._publisher = None - self._topic = None - -class SprokitAdapter(Plugin): - - def __init__(self, context): - super(SprokitAdapter, self).__init__(context) - self.initialized = False - self._topicHandler = None - # Give QObjects reasonable names - self.setObjectName('SprokitAdapter') - - args = self._parse_args(context.argv()) - - # Create QWidget - self._widget = QWidget() - # Get path to UI file which should be in the "resource" folder of this package - ui_file = os.path.join(rospkg.RosPack().get_path('rqt_sprokit_adapter'), 'resource', 'rqt_sprokit_adapter.ui') - # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) - - # Give QObjects reasonable names - self._widget.setObjectName('SprokitAdapterUi') - - # Show _widget.windowTitle on left-top of each plugin (when - # it's set in _widget). This is useful when you open multiple - # plugins at once. Also if you open multiple instances of your - # plugin at once, these lines add number to make it easy to - # tell from pane to pane. - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - # Add widget to the user interface - context.add_widget(self._widget) - - self._widget.topicLineEdit.editingFinished.connect(self._publish) - self._widget.showProbabilityCheckBox.clicked.connect(self._publish) - self._widget.thresholdSpinBox.valueChanged.connect(self._publish) - - def shutdown_plugin(self): - if self._topicHandler is not None: - self._topicHandler.close() - self._topicHandler = None - - def save_settings(self, plugin_settings, instance_settings): - # TODO save intrinsic configuration, usually using: - # instance_settings.set_value(k, v) - instance_settings.set_value('topic',self._widget.topicLineEdit.text()) - instance_settings.set_value('threshold',self._widget.thresholdSpinBox.value()) - instance_settings.set_value('draw_text',self._widget.showProbabilityCheckBox.isChecked()) - - def restore_settings(self, plugin_settings, instance_settings): - # TODO restore intrinsic configuration, usually using: - # v = instance_settings.value(k) - topic = instance_settings.value('topic') - print("Topic ",topic) - threshold = instance_settings.value('threshold') - print("Threshold ",threshold) - draw_text = instance_settings.value('draw_text') - print("draw text ",draw_text) - self._widget.topicLineEdit.setText(topic) - self._widget.thresholdSpinBox.setValue(float(threshold)) - self._widget.showProbabilityCheckBox.setCheckState(distutils.util.strtobool(draw_text)) - self.initialized = True - self._publish() - pass - - def _parse_args(self, argv): - parser = argparse.ArgumentParser(prog='rqt_bag', add_help=False) - SprokitAdapter.add_arguments(parser) - return parser.parse_args(argv) - - def _publish(self): - #print("Topic: ",self._widget.topicLineEdit.text()) - #print("ShowText: ",self._widget.showProbabilityCheckBox.isChecked()) - #print("Threshold ",self._widget.thresholdSpinBox.value()) - topic = self._widget.topicLineEdit.text() - if topic == "": - return - if not self._topicHandler or self._topicHandler.topic != topic: - if not self._topicHandler is None: - self._topicHandler.close() - self._topicHandler = SprokitAdapterTopicHandler(topic) - - self._topicHandler.publish(self._widget.showProbabilityCheckBox.isChecked(), - self._widget.thresholdSpinBox.value() - ) - - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_sprokit_adapter plugin') - group.add_argument('--topic', default='display_parameters', help='publish the clock time') - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure - # This will enable a setting button (gear icon) in each dock widget title bar - # Usually used to open a modal configuration dialog diff --git a/src/kitware-ros-pkg/sensor_simulator/CMakeLists.txt b/src/kitware-ros-pkg/sensor_simulator/CMakeLists.txt deleted file mode 100755 index 83f04d40..00000000 --- a/src/kitware-ros-pkg/sensor_simulator/CMakeLists.txt +++ /dev/null @@ -1,201 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sensor_simulator) - -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-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 - cv_bridge - rospy - sensor_msgs - std_msgs - camera_info_manager_py - image_view -) - -## 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 -# sensor_msgs# 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 you 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 focus_camera -# CATKIN_DEPENDS cv_bridge opencv2 rospy sensor_msgs std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/focus_camera.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/focus_camera_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 - 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_focus_camera.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) diff --git a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch deleted file mode 100644 index 1b5d4af1..00000000 --- a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch.xml b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch.xml new file mode 100644 index 00000000..ca2464b2 --- /dev/null +++ b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras_one_sys.launch b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras_one_sys.launch deleted file mode 100644 index 79151aef..00000000 --- a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_cameras_one_sys.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch deleted file mode 100644 index 7d61e90e..00000000 --- a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch.xml b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch.xml new file mode 100644 index 00000000..03f928b8 --- /dev/null +++ b/src/kitware-ros-pkg/sensor_simulator/launch/simulate_ins.launch.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/kitware-ros-pkg/sensor_simulator/package.xml b/src/kitware-ros-pkg/sensor_simulator/package.xml index aa933a07..da38b239 100755 --- a/src/kitware-ros-pkg/sensor_simulator/package.xml +++ b/src/kitware-ros-pkg/sensor_simulator/package.xml @@ -1,61 +1,22 @@ - - + + + sensor_simulator - 0.0.0 - Simulate the view that would be seen by a camera using a source panoramic image. The simulation assumes that the camera is at the same center of projection as the panoramic source. + 1.0.0 + Simulated camera / INS message sources for development - - - Adam Romlein Matt Brown - - - - - Apache 2.0 + rclpy + sensor_msgs + custom_msgs + cv_bridge + python3-numpy - - - - - - - - - - - - - - - - - - - - - - - - catkin - cv_bridge - rospy - sensor_msgs - std_msgs - image_view - - cv_bridge - rospy - sensor_msgs - std_msgs - image_view - - - - + + ament_python diff --git a/src/core/kamerahealth/nodes/__init__.py b/src/kitware-ros-pkg/sensor_simulator/resource/sensor_simulator old mode 100755 new mode 100644 similarity index 100% rename from src/core/kamerahealth/nodes/__init__.py rename to src/kitware-ros-pkg/sensor_simulator/resource/sensor_simulator diff --git a/src/kitware-ros-pkg/sensor_simulator/scripts/simulate_ins.py b/src/kitware-ros-pkg/sensor_simulator/scripts/simulate_ins.py deleted file mode 100755 index 6e860f92..00000000 --- a/src/kitware-ros-pkg/sensor_simulator/scripts/simulate_ins.py +++ /dev/null @@ -1,89 +0,0 @@ -#! /usr/bin/python -from __future__ import division, print_function -import numpy as np -import time - -# ROS imports -import rospy -import genpy -from custom_msgs.msg import GSOF_INS -from kamera.sensor_models import quaternion_from_euler -from kamera.sensor_models.nav_conversions import ned_quat_to_enu_quat - - -def main(): - # Launch the node. - node = 'simulate_ptz_camera' - rospy.init_node(node, anonymous=False) - node = rospy.get_name() - - lat = rospy.get_param('%s/lat' % node) - lon = rospy.get_param('%s/lon' % node) - height = rospy.get_param('%s/height' % node) - - yaw0 = rospy.get_param('%s/nominal_yaw' % node) - pitch0 = rospy.get_param('%s/nominal_pitch' % node) - roll0 = rospy.get_param('%s/nominal_roll' % node) - - yaw_range = rospy.get_param('%s/yaw_range' % node) - pitch_range = rospy.get_param('%s/pitch_range' % node) - roll_range = rospy.get_param('%s/roll_range' % node) - motion_rate = rospy.get_param('%s/motion_rate' % node) - pub_rate = rospy.get_param('%s/pub_rate' % node) - - topic = rospy.get_param('%s/topic' % node) - - rospy.loginfo('lat (deg): %s' % str(lat)) - rospy.loginfo('lon (deg): %s' % str(lon)) - rospy.loginfo('height (m): %s' % str(height)) - rospy.loginfo('nominal_yaw (deg): %s' % str(yaw0)) - rospy.loginfo('nominal_pitch (deg): %s' % str(pitch0)) - rospy.loginfo('nominal_roll (deg): %s' % str(roll0)) - rospy.loginfo('yaw_range (deg): %s' % str(yaw_range)) - rospy.loginfo('pitch_range (deg): %s' % str(pitch_range)) - rospy.loginfo('roll_range (deg): %s' % str(roll_range)) - rospy.loginfo('Motion rate (deg/s): %s' % str(motion_rate)) - rospy.loginfo('Publish rate: %s' % str(pub_rate)) - rospy.loginfo('Odometry topic: %s' % str(topic)) - # ------------------------------------------------------------------------ - - ins_state_pub = rospy.Publisher(topic, GSOF_INS, queue_size=1) - - rate = rospy.Rate(pub_rate) - t0 = rospy.get_time() - yaw = pitch = roll = 0 - while not rospy.is_shutdown(): - t = rospy.get_time() - t0 - yaw = yaw0 + yaw_range*np.sin(t*motion_rate/yaw_range*2*np.pi) - pitch = pitch0 + pitch_range*np.sin(t*motion_rate/pitch_range*2*np.pi) - roll = roll0 + roll_range*np.sin(t*motion_rate/roll_range*2*np.pi) - - print('yaw:', yaw, 'pitch:', pitch, 'roll:', roll) - msg = GSOF_INS() - msg.latitude = lat - msg.longitude = lon - msg.altitude = height - msg.align_status = 4 - msg.gnss_status = 1 - msg.north_velocity = 50 - msg.east_velocity = 20 - msg.down_velocity = 1 - msg.total_speed = np.sqrt(msg.north_velocity**2 + msg.east_velocity**2 + msg.down_velocity**2) - - - msg.heading = yaw - msg.pitch = pitch - msg.roll = roll - msg.track_angle = 5 - msg.time = rospy.get_time() - - msg.header.stamp = genpy.Time.from_sec(rospy.get_time()) - ins_state_pub.publish(msg) - rate.sleep() - - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass diff --git a/src/kitware-ros-pkg/sensor_simulator/setup.cfg b/src/kitware-ros-pkg/sensor_simulator/setup.cfg new file mode 100644 index 00000000..7d9aa4ea --- /dev/null +++ b/src/kitware-ros-pkg/sensor_simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sensor_simulator +[install] +install_scripts=$base/lib/sensor_simulator diff --git a/src/kitware-ros-pkg/sensor_simulator/setup.py b/src/kitware-ros-pkg/sensor_simulator/setup.py index 3ac8429f..ef03da31 100755 --- a/src/kitware-ros-pkg/sensor_simulator/setup.py +++ b/src/kitware-ros-pkg/sensor_simulator/setup.py @@ -1,9 +1,29 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['sensor_simulator'], - package_dir={'': 'src'}) +from setuptools import setup -setup(**d) +package_name = "sensor_simulator" + +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + package_dir={"": "src"}, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Simulated camera / INS message sources for development", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "simulate_cameras = sensor_simulator.simulate_cameras:main", + "simulate_ins = sensor_simulator.simulate_ins:main", + ], + }, +) diff --git a/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/camera_simulator.py b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/camera_simulator.py index 5cbcbda2..53ba4e29 100644 --- a/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/camera_simulator.py +++ b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/camera_simulator.py @@ -9,13 +9,15 @@ import time # ROS imports -import rospy -import rospkg +import rclpy.logging +from builtin_interfaces.msg import Time as MsgTime from sensor_msgs.msg import Image -import std_msgs.msg -from sensor_msgs.msg import JointState -from cv_bridge import CvBridge, CvBridgeError -import genpy +from cv_bridge import CvBridge + + +def _time_msg_from_sec(t): + sec = int(t) + return MsgTime(sec=sec, nanosec=int(round((t - sec) * 1e9))) # Instantiate CvBridge @@ -26,9 +28,11 @@ class CameraSimulator(): """Camera that outputs test imagery. """ - def __init__(self, res_x, res_y, encoding, image_topic): + def __init__(self, node, res_x, res_y, encoding, image_topic): """Initialization. + :param node: rclpy node owning the publisher. + :param res_x: Horizontal resolution (i.e., number of columns). :type res_x: int @@ -58,11 +62,11 @@ def __init__(self, res_x, res_y, encoding, image_topic): rand_img = np.random.rand(self.res_y, self.res_x, 3) rand_img = np.round(rand_img*65535).astype(np.uint16) + self._node = node self._rand_img = rand_img self._encoding = encoding - self._image_topic = rospy.resolve_name(image_topic) - self._image_publisher = rospy.Publisher(image_topic, Image, - queue_size=100) + self._image_topic = image_topic + self._image_publisher = node.create_publisher(Image, image_topic, 100) self._seq_ind = 0 @property @@ -98,8 +102,7 @@ def publish_image(self, image): image_message = bridge.cv2_to_imgmsg(image, encoding=self.encoding) image_message.header.frame_id = self.image_topic - image_message.header.stamp = genpy.Time.from_sec(t) - image_message.header.seq = self._seq_ind + image_message.header.stamp = _time_msg_from_sec(t) self._image_publisher.publish(image_message) self._seq_ind += 1 @@ -112,6 +115,7 @@ def publish_test_image(self): shift = int(np.random.randint(0, L, 1)) image = np.roll(image, shift) self.publish_image(image) - rospy.loginfo('Published %i x %i image with encoding \'%s\' on image ' - 'topic: %s' % (image.shape[1],image.shape[0], - self.encoding,self._image_topic)) + rclpy.logging.get_logger('camera_simulator').info( + 'Published %i x %i image with encoding \'%s\' on image ' + 'topic: %s' % (image.shape[1], image.shape[0], + self.encoding, self._image_topic)) diff --git a/src/kitware-ros-pkg/sensor_simulator/scripts/simulate_cameras.py b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_cameras.py similarity index 50% rename from src/kitware-ros-pkg/sensor_simulator/scripts/simulate_cameras.py rename to src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_cameras.py index 7488d846..b5e24521 100755 --- a/src/kitware-ros-pkg/sensor_simulator/scripts/simulate_cameras.py +++ b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_cameras.py @@ -1,51 +1,49 @@ #! /usr/bin/python from __future__ import division, print_function -import numpy as np # ROS imports -import rospy -import rospkg +import rclpy +from rclpy.node import Node # kitware-ros-pkg imports import sensor_simulator.camera_simulator as camera_simulator -rospack = rospkg.RosPack() +def main(args=None): + rclpy.init(args=args) + node = Node('simulate_cameras') -def main(): - # Launch the node. - node = 'simulate_cameras' - rospy.init_node(node, anonymous=False) - node = rospy.get_name() - - frame_rate = rospy.get_param('%s/frame_rate' % node, default=4) - # ------------------------------------------------------------------------ - + frame_rate = node.declare_parameter('frame_rate', 4.0).value # Define camera simulator - ir_cam = camera_simulator.CameraSimulator(res_x=540, res_y=512, + ir_cam = camera_simulator.CameraSimulator(node, res_x=540, res_y=512, encoding='mono8', image_topic='ir/image_raw') - eo_cam = camera_simulator.CameraSimulator(res_x=6576, res_y=4384, + eo_cam = camera_simulator.CameraSimulator(node, res_x=6576, res_y=4384, encoding='bayer_grbg8', image_topic='rgb/image_raw') - uv_cam = camera_simulator.CameraSimulator(res_x=6576, res_y=4384, + uv_cam = camera_simulator.CameraSimulator(node, res_x=6576, res_y=4384, encoding='mono8', image_topic='uv/image_raw') - rate = rospy.Rate(frame_rate) - rospy.loginfo('Publishing images at %0.1f Hz' % frame_rate) - while not rospy.is_shutdown(): + node.get_logger().info('Publishing images at %0.1f Hz' % frame_rate) + + def tick(): ir_cam.publish_test_image() eo_cam.publish_test_image() uv_cam.publish_test_image() - rate.sleep() - -if __name__ == '__main__': + node.create_timer(1.0 / frame_rate, tick) try: - main() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_ins.py b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_ins.py new file mode 100755 index 00000000..4f1f19b5 --- /dev/null +++ b/src/kitware-ros-pkg/sensor_simulator/src/sensor_simulator/simulate_ins.py @@ -0,0 +1,98 @@ +#! /usr/bin/python +from __future__ import division, print_function +import time + +import numpy as np + +# ROS imports +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Time as MsgTime +from custom_msgs.msg import GsofIns + + +def _time_msg_from_sec(t): + sec = int(t) + return MsgTime(sec=sec, nanosec=int(round((t - sec) * 1e9))) + + +def main(args=None): + rclpy.init(args=args) + node = Node('simulate_ptz_camera') + log = node.get_logger() + + lat = node.declare_parameter('lat', 0.0).value + lon = node.declare_parameter('lon', 0.0).value + height = node.declare_parameter('height', 0.0).value + + yaw0 = node.declare_parameter('nominal_yaw', 0.0).value + pitch0 = node.declare_parameter('nominal_pitch', 0.0).value + roll0 = node.declare_parameter('nominal_roll', 0.0).value + + yaw_range = node.declare_parameter('yaw_range', 1.0).value + pitch_range = node.declare_parameter('pitch_range', 1.0).value + roll_range = node.declare_parameter('roll_range', 1.0).value + motion_rate = node.declare_parameter('motion_rate', 1.0).value + pub_rate = node.declare_parameter('pub_rate', 10.0).value + + topic = node.declare_parameter('topic', '/ins').value + + log.info('lat (deg): %s' % str(lat)) + log.info('lon (deg): %s' % str(lon)) + log.info('height (m): %s' % str(height)) + log.info('nominal_yaw (deg): %s' % str(yaw0)) + log.info('nominal_pitch (deg): %s' % str(pitch0)) + log.info('nominal_roll (deg): %s' % str(roll0)) + log.info('yaw_range (deg): %s' % str(yaw_range)) + log.info('pitch_range (deg): %s' % str(pitch_range)) + log.info('roll_range (deg): %s' % str(roll_range)) + log.info('Motion rate (deg/s): %s' % str(motion_rate)) + log.info('Publish rate: %s' % str(pub_rate)) + log.info('Odometry topic: %s' % str(topic)) + # ------------------------------------------------------------------------ + + ins_state_pub = node.create_publisher(GsofIns, topic, 1) + + t0 = time.time() + + def tick(): + t = time.time() - t0 + yaw = yaw0 + yaw_range * np.sin(t * motion_rate / yaw_range * 2 * np.pi) + pitch = pitch0 + pitch_range * np.sin(t * motion_rate / pitch_range * 2 * np.pi) + roll = roll0 + roll_range * np.sin(t * motion_rate / roll_range * 2 * np.pi) + + print('yaw:', yaw, 'pitch:', pitch, 'roll:', roll) + msg = GsofIns() + msg.latitude = float(lat) + msg.longitude = float(lon) + msg.altitude = float(height) + msg.align_status = 4 + msg.gnss_status = 1 + msg.north_velocity = 50.0 + msg.east_velocity = 20.0 + msg.down_velocity = 1.0 + msg.total_speed = float(np.sqrt(msg.north_velocity**2 + + msg.east_velocity**2 + + msg.down_velocity**2)) + + msg.heading = float(yaw) + msg.pitch = float(pitch) + msg.roll = float(roll) + msg.track_angle = 5.0 + now = time.time() + msg.time = now + msg.header.stamp = _time_msg_from_sec(now) + ins_state_pub.publish(msg) + + node.create_timer(1.0 / pub_rate, tick) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt b/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt index 358fd041..ed111ee6 100644 --- a/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt +++ b/src/kitware-ros-pkg/sprokit_adapters/CMakeLists.txt @@ -49,6 +49,12 @@ target_link_libraries( kw_detector_fusion_adapter_node install(TARGETS kw_detector_fusion_adapter_node DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS + scripts/publish_sync_msgs.py + scripts/rebroadcast_infrequent_detections.py + scripts/save_detection_chips_to_disk.py + scripts/save_images_to_disk.py + DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} FILES_MATCHING PATTERN "*.launch.xml") diff --git a/src/kitware-ros-pkg/sprokit_adapters/scripts/publish_sync_msgs.py b/src/kitware-ros-pkg/sprokit_adapters/scripts/publish_sync_msgs.py index 942faa02..a0390b40 100755 --- a/src/kitware-ros-pkg/sprokit_adapters/scripts/publish_sync_msgs.py +++ b/src/kitware-ros-pkg/sprokit_adapters/scripts/publish_sync_msgs.py @@ -10,9 +10,19 @@ import cv2 import numpy as np -import rospy +import rclpy +import rclpy.logging +from rclpy.node import Node +from builtin_interfaces.msg import Time as MsgTime from cv_bridge import CvBridge, CvBridgeError +log = rclpy.logging.get_logger("publish_sync_msgs") + + +def _time_msg_from_sec(t): + sec = int(t) + return MsgTime(sec=sec, nanosec=int(round((t - sec) * 1e9))) + # KAMERA imports. from custom_msgs.msg import SynchronizedImages @@ -38,15 +48,15 @@ def get_fov_dirs(flight_dir): return actual_dirs class ROSPublishSyncMsgs(object): - def __init__(self, flight_dir, rate, out_sync_image_topic): + def __init__(self, node, flight_dir, rate, out_sync_image_topic): + self.node = node self.flight_dir = flight_dir self.rate = rate - self.pub = rospy.Publisher(out_sync_image_topic, - SynchronizedImages, - queue_size=1) + self.pub = node.create_publisher(SynchronizedImages, + out_sync_image_topic, 1) def start_publishing(self): - ros_rate = rospy.Rate(self.rate) + period = 1.0 / self.rate fov_dirs = get_fov_dirs(self.flight_dir) if len(fov_dirs) == 0: @@ -74,7 +84,7 @@ def start_publishing(self): fnames[dirname][t] = {} cam = f.split('_')[-1].split('.')[0] fnames[dirname][t][cam] = f - rospy.loginfo("Total time to organize: %s" % (time.time() - tic)) + log.info("Total time to organize: %s" % (time.time() - tic)) # Sort by t @@ -84,8 +94,7 @@ def start_publishing(self): seq = 0 for t in fnames[d]: sync_msg = SynchronizedImages() - sync_msg.header.stamp = rospy.Time(t) - sync_msg.header.seq = seq + sync_msg.header.stamp = _time_msg_from_sec(t) seq += 1 for cam in fnames[d][t]: fname = fnames[d][t][cam] @@ -101,7 +110,7 @@ def start_publishing(self): try: msg = bridge.cv2_to_imgmsg(im, encoding=encoding) except CvBridgeError as e: - rospy.logerr(e) + log.error(str(e)) break if cam == 'rgb': sync_msg.image_rgb = msg @@ -112,28 +121,27 @@ def start_publishing(self): elif cam == 'ir': sync_msg.image_ir = msg sync_msg.file_path_ir = fname - rospy.loginfo("Publishing sync image for time %s" % t) + log.info("Publishing sync image for time %s" % t) self.pub.publish(sync_msg) - if rospy.is_shutdown(): + if not rclpy.ok(): raise SystemExit - ros_rate.sleep() + time.sleep(period) -def main(): - rospy.init_node("publish_sync_msgs", anonymous=True) +def main(args=None): + rclpy.init(args=args) + node = Node("publish_sync_msgs") - flight_dir = rospy.get_param("~flight_dir") - rate = rospy.get_param("~publish_rate") - out_sync_image_topic = rospy.get_param("~out_topic") + flight_dir = node.declare_parameter("flight_dir", "").value + rate = node.declare_parameter("publish_rate", 1.0).value + out_sync_image_topic = node.declare_parameter("out_topic", "/synched").value - PSM = ROSPublishSyncMsgs(flight_dir, rate, out_sync_image_topic) + PSM = ROSPublishSyncMsgs(node, flight_dir, rate, out_sync_image_topic) PSM.start_publishing() - rospy.spin() + rclpy.spin(node) + if __name__ == "__main__": - try: - main() - except rospy.ROSInterruptException: - raise SystemExit + main() diff --git a/src/kitware-ros-pkg/sprokit_adapters/scripts/rebroadcast_infrequent_detections.py b/src/kitware-ros-pkg/sprokit_adapters/scripts/rebroadcast_infrequent_detections.py index db0ca428..0f957488 100755 --- a/src/kitware-ros-pkg/sprokit_adapters/scripts/rebroadcast_infrequent_detections.py +++ b/src/kitware-ros-pkg/sprokit_adapters/scripts/rebroadcast_infrequent_detections.py @@ -10,10 +10,13 @@ import string # ROS imports -import rospy -import rospy +import rclpy +import rclpy.logging +from rclpy.node import Node from cv_bridge import CvBridge, CvBridgeError +log = rclpy.logging.get_logger("rebroadcast_infrequent_detections") + # Custom Imports from sensor_msgs.msg import Image from custom_msgs.msg import ImageSpaceDetectionList @@ -32,8 +35,9 @@ def generate_uid(n=20): class DetectionRebroadcast(object): - def __init__(self, det_in_topic, det_out_topic, image_topics, + def __init__(self, node, det_in_topic, det_out_topic, image_topics, det_transform_service): + self.node = node # Set up locks. self.latest_dets_lock = threading.RLock() @@ -53,18 +57,16 @@ def __init__(self, det_in_topic, det_out_topic, image_topics, if det_transform_service is not None: self.set_det_tform_service(det_transform_service) - self.det_pub = rospy.Publisher(det_out_topic, - ImageSpaceDetectionList, - queue_size=10) + self.det_pub = node.create_publisher(ImageSpaceDetectionList, + det_out_topic, 10) - rospy.Subscriber(det_in_topic, ImageSpaceDetectionList, - callback=self.detection_list_callback, - queue_size=10) + node.create_subscription(ImageSpaceDetectionList, det_in_topic, + self.detection_list_callback, 10) - rospy.loginfo('Rebroadcasting detections from topic: \'%s\' on topic: ' - '\'%s\'' % (det_in_topic,det_out_topic)) + log.info('Rebroadcasting detections from topic: \'%s\' on topic: ' + '\'%s\'' % (det_in_topic,det_out_topic)) - rospy.loginfo("Starting image processing thread") + log.info("Starting image processing thread") self.thread = threading.Thread(target=self.process_images) # Entire Python program exits when only daemon threads are left and we # want this thread to shutdown as cleanly as possible. @@ -72,10 +74,10 @@ def __init__(self, det_in_topic, det_out_topic, image_topics, self.thread.start() for image_topic in image_topics: - rospy.loginfo('Receiving images on topic: %s' % image_topic) - rospy.Subscriber(image_topic, Image, - callback=self.ros_image_callback, - callback_args=image_topic, queue_size=1) + log.info('Receiving images on topic: %s' % image_topic) + node.create_subscription( + Image, image_topic, + lambda msg, t=image_topic: self.ros_image_callback(msg, t), 1) @property def lock(self): @@ -101,12 +103,13 @@ def set_det_tform_service(self, topic): :type topic: str """ - rospy.loginfo('Waiting for detection list transformation service ' - '\'%s\' to come alive' % topic) - rospy.wait_for_service(topic) + log.info('Waiting for detection list transformation service ' + '\'%s\' to come alive' % topic) + client = self.node.create_client(TransformDetectionList, topic) + while not client.wait_for_service(timeout_sec=1.0) and rclpy.ok(): + pass with self._det_tform_serv_lock: - self._det_tform_serv = rospy.ServiceProxy(topic, - TransformDetectionList) + self._det_tform_serv = client def detection_list_callback(self, msg): """Receive a detection list. @@ -115,7 +118,7 @@ def detection_list_callback(self, msg): :type msg: ImageSpaceDetectionList """ - rospy.loginfo('Received detection (seq: %i)' % msg.header.seq) + log.info('Received detection') with self.latest_dets_lock: self.latest_dets = msg self.tformed_versions_latest_dets = {msg.header.frame_id:msg} @@ -129,14 +132,12 @@ def ros_image_callback(self, msg, topic): """ # Lock so that only one message can initialize. if self.latest_dets is None: - rospy.loginfo('Received with image (seq: %i) from message ' - 'topic \'%s\', but have not received detections, ' - 'so skipping.' % - (msg.header.seq,topic)) + log.info('Received image from message ' + 'topic \'%s\', but have not received detections, ' + 'so skipping.' % topic) return else: - rospy.loginfo('Received with image (seq: %i) from message ' - 'topic \'%s\'' % (msg.header.seq,topic)) + log.info('Received image from message topic \'%s\'' % topic) with self.image_lock: self.image_deque.appendleft(msg) @@ -144,7 +145,7 @@ def ros_image_callback(self, msg, topic): self.image_deque.pop() def process_images(self): - while True and not rospy.is_shutdown(): + while rclpy.ok(): with self.image_lock: if len(self.image_deque) == 0: continue @@ -164,14 +165,20 @@ def process_images(self): if fid1 not in self.tformed_versions_latest_dets: try: with self._det_tform_serv_lock: - resp = self._det_tform_serv(self.latest_dets, fid1) + req = TransformDetectionList.Request() + req.src_detections = self.latest_dets + req.dst_frame_id = fid1 + future = self._det_tform_serv.call_async(req) + while not future.done() and rclpy.ok(): + time.sleep(0.01) + resp = future.result() msg1 = resp.dst_detections self.tformed_versions_latest_dets[fid1] = msg1 - except rospy.ServiceException as e: - rospy.logerr('Could not transform detections from ' - 'source frame_id \'%s\' to destination ' - '\'%s\' because %s' % - (self.latest_dets.frame_id,fid1, e)) + except Exception as e: + log.error('Could not transform detections from ' + 'source frame_id \'%s\' to destination ' + '\'%s\' because %s' % + (self.latest_dets.header.frame_id, fid1, e)) raise e msg_tformed = self.tformed_versions_latest_dets[fid1] @@ -212,50 +219,42 @@ def process_images(self): det.camera_of_origin = image_msg.header.frame_id msg.detections.append(det) - rospy.loginfo('Rebroadcasting detection list with %i detections' % - len(msg.detections)) + log.info('Rebroadcasting detection list with %i detections' % + len(msg.detections)) self.det_pub.publish(msg) -def main(): - # Launch the node. - node = 'rebroadcast_infrequent_detections' - rospy.init_node(node, anonymous=False) - - node_name = rospy.get_name() +def main(args=None): + rclpy.init(args=args) + node = Node('rebroadcast_infrequent_detections') # -------------------------- Read Parameters ----------------------------- - det_in_topic = rospy.get_param('%s/det_in_topic' % node_name) - det_out_topic = rospy.get_param('%s/det_out_topic' % node_name) + det_in_topic = node.declare_parameter('det_in_topic', '').value + det_out_topic = node.declare_parameter('det_out_topic', '').value image_topics = [] i = 1 while True: - try: - param_name = '%s/image_in%i_topic' % (node_name, i) - param = rospy.get_param(param_name) - if param != 'unused': - image_topics.append(param) - i += 1 - else: - break - except KeyError: + param = node.declare_parameter('image_in%i_topic' % i, 'unused').value + if param != 'unused': + image_topics.append(param) + i += 1 + else: break - param_name = '%s/detection_transform_service' % node_name - det_transform_service = rospy.get_param(param_name, None) + det_transform_service = node.declare_parameter( + 'detection_transform_service', 'none').value if det_transform_service == 'none': det_transform_service = None # ------------------------------------------------------------------------ - det_rebroadcast = DetectionRebroadcast(det_in_topic, det_out_topic, + det_rebroadcast = DetectionRebroadcast(node, det_in_topic, det_out_topic, image_topics, det_transform_service) + (void_ref,) = (det_rebroadcast,) + + rclpy.spin(node) - rospy.spin() if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/kitware-ros-pkg/sprokit_adapters/scripts/save_detection_chips_to_disk.py b/src/kitware-ros-pkg/sprokit_adapters/scripts/save_detection_chips_to_disk.py index 418f8979..7bc589be 100755 --- a/src/kitware-ros-pkg/sprokit_adapters/scripts/save_detection_chips_to_disk.py +++ b/src/kitware-ros-pkg/sprokit_adapters/scripts/save_detection_chips_to_disk.py @@ -6,23 +6,28 @@ import time # ROS imports -import rospy +import rclpy +import rclpy.logging +from rclpy.node import Node from custom_msgs.msg import ImageSpaceDetectionList from cv_bridge import CvBridge, CvBridgeError +def _stamp_to_sec(stamp): + return stamp.sec + stamp.nanosec * 1e-9 + + # Instantiate CvBridge bridge = CvBridge() class ChipSaver(object): - def __init__(self, det_topic, image_directory, ext='jpg'): - rospy.loginfo('Saving chips for detection topic det_topics: %s' % - det_topic) + def __init__(self, node, det_topic, image_directory, ext='jpg'): + rclpy.logging.get_logger('save_chips').info( + 'Saving chips for detection topic det_topics: %s' % det_topic) self.image_directory = image_directory - self.image_subscriber = rospy.Subscriber(det_topic, - ImageSpaceDetectionList, - self.callback_ros) + self.image_subscriber = node.create_subscription( + ImageSpaceDetectionList, det_topic, self.callback_ros, 10) self.ext = ext def callback_ros(self, msg): @@ -33,7 +38,7 @@ def callback_ros(self, msg): """ frame_id = msg.header.frame_id - frame_time = int(np.round(msg.header.stamp.to_sec()*100)) + frame_time = int(np.round(_stamp_to_sec(msg.header.stamp)*100)) frame_id = frame_id.replace('/','_') @@ -52,31 +57,24 @@ def callback_ros(self, msg): cv2.imwrite(fname, raw_image) -def main(): - # Launch the node. - node = 'save_images_to_disk' - rospy.init_node(node, anonymous=False) - - node_name = rospy.get_name() +def main(args=None): + rclpy.init(args=args) + node = Node('save_detection_chips_to_disk') # -------------------------- Read Parameters ----------------------------- - #print('rospy.get_param_names()', rospy.get_param_names()) - - # Load in cueing camera details. det_topics = [] i = 1 while True: - try: - param_name = '%s/detection_topic%i' % (node_name, i) - det_topics.append(rospy.get_param(param_name)) - i += 1 - except KeyError: + topic = node.declare_parameter('detection_topic%i' % i, '').value + if not topic: break + det_topics.append(topic) + i += 1 - image_directory = rospy.get_param('%s/image_directory' % node_name) - image_directory = '%s/%i' % (image_directory,int(time.time())) + image_directory = node.declare_parameter('image_directory', '.').value + image_directory = '%s/%i' % (image_directory, int(time.time())) - ext = rospy.get_param('%s/image_extension' % node_name) + ext = node.declare_parameter('image_extension', 'jpg').value try: os.makedirs(image_directory) @@ -84,13 +82,11 @@ def main(): pass # ------------------------------------------------------------------------ - for det_topic in det_topics: - ChipSaver(det_topic, image_directory, ext) + savers = [ChipSaver(node, t, image_directory, ext) for t in det_topics] + (void_ref,) = (savers,) + + rclpy.spin(node) - rospy.spin() if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/kitware-ros-pkg/sprokit_adapters/scripts/save_images_to_disk.py b/src/kitware-ros-pkg/sprokit_adapters/scripts/save_images_to_disk.py index 902e39b3..e95512fd 100755 --- a/src/kitware-ros-pkg/sprokit_adapters/scripts/save_images_to_disk.py +++ b/src/kitware-ros-pkg/sprokit_adapters/scripts/save_images_to_disk.py @@ -6,21 +6,26 @@ import time # ROS imports -import rospy +import rclpy +from rclpy.node import Node from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError +def _stamp_to_sec(stamp): + return stamp.sec + stamp.nanosec * 1e-9 + + # Instantiate CvBridge bridge = CvBridge() class ImageSaver(object): - def __init__(self, topic_name, image_directory, ext='jpg'): + def __init__(self, node, topic_name, image_directory, ext='jpg'): print('Saving images for topic:', topic_name) self.image_directory = image_directory - self.image_subscriber = rospy.Subscriber(topic_name, Image, - self.image_callback_ros) + self.image_subscriber = node.create_subscription( + Image, topic_name, self.image_callback_ros, 10) self.ext = ext def image_callback_ros(self, image_msg): @@ -41,7 +46,7 @@ def image_callback_ros(self, image_msg): raw_image = raw_image[...,::-1] frame_id = image_msg.header.frame_id - frame_time = int(np.round(image_msg.header.stamp.to_sec()*100)) + frame_time = int(np.round(_stamp_to_sec(image_msg.header.stamp)*100)) if raw_image.ndim == 3: raw_image = cv2.cvtColor(raw_image, cv2.COLOR_RGB2BGR) @@ -54,47 +59,37 @@ def image_callback_ros(self, image_msg): -def main(): - # Launch the node. - node = 'save_images_to_disk' - rospy.init_node(node, anonymous=False) - - node_name = rospy.get_name() - - # -------------------------- Read Parameters ----------------------------- - #print('rospy.get_param_names()', rospy.get_param_names()) - +def main(args=None): + rclpy.init(args=args) + node = Node('save_images_to_disk') + + # -------------------------- Read Parameters ----------------------------- # Load in cueing camera details. image_topics = [] i = 1 while True: - try: - param_name = ''.join([node_name,'/image_topic',str(i)]) - image_topics.append(rospy.get_param(param_name)) - i += 1 - except: + topic = node.declare_parameter('image_topic%i' % i, '').value + if not topic: break - - param_name = ''.join([node_name,'/image_directory']) - image_directory = rospy.get_param(param_name) - image_directory = ''.join([image_directory,'/',str(int(time.time()))]) - - param_name = ''.join([node_name,'/image_extension']) - ext = rospy.get_param(param_name) - + image_topics.append(topic) + i += 1 + + image_directory = node.declare_parameter('image_directory', '.').value + image_directory = ''.join([image_directory, '/', str(int(time.time()))]) + + ext = node.declare_parameter('image_extension', 'jpg').value + try: os.makedirs(image_directory) except OSError: pass # ------------------------------------------------------------------------ - - for image_topic in image_topics: - ImageSaver(image_topic, image_directory, ext) - - rospy.spin() - + + savers = [ImageSaver(node, t, image_directory, ext) for t in image_topics] + (void_ref,) = (savers,) + + rclpy.spin(node) + + if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/process/sysinfo/CMakeLists.txt b/src/process/sysinfo/CMakeLists.txt deleted file mode 100644 index 8a3f4f44..00000000 --- a/src/process/sysinfo/CMakeLists.txt +++ /dev/null @@ -1,199 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sysinfo) - -## 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 - custom_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 exec_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 exec_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 exec_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 nexus -# CATKIN_DEPENDS roscpp 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}/nexus.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/nexus_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_nexus.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) diff --git a/src/process/sysinfo/launch/syscall.launch b/src/process/sysinfo/launch/syscall.launch deleted file mode 100644 index eb69e2b1..00000000 --- a/src/process/sysinfo/launch/syscall.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/process/sysinfo/launch/syscall.launch.xml b/src/process/sysinfo/launch/syscall.launch.xml new file mode 100644 index 00000000..1676de62 --- /dev/null +++ b/src/process/sysinfo/launch/syscall.launch.xml @@ -0,0 +1,8 @@ + + + + + + + diff --git a/src/process/sysinfo/nodes/__init__.py b/src/process/sysinfo/nodes/__init__.py deleted file mode 100755 index e69de29b..00000000 diff --git a/src/process/sysinfo/nodes/syscall_node.py b/src/process/sysinfo/nodes/syscall_node.py deleted file mode 100755 index 16c60b93..00000000 --- a/src/process/sysinfo/nodes/syscall_node.py +++ /dev/null @@ -1,43 +0,0 @@ -#! /usr/bin/python -# -*- coding: utf-8 -*- - -import sys -import subprocess -import shlex -import rospy -from custom_msgs.srv import SysCall - -USE_SHELL = False - - -def syscall_cb(msg): - cmdlist = shlex.split(msg.cmd) - rospy.loginfo(cmdlist) - try: - proc = subprocess.Popen( - cmdlist, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=USE_SHELL - ) - except Exception as exc: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("subprocess failed: {}: {}".format(exc_type, value)) - return ('', '{}: {}'.format(exc_type, value)) - try: - outs, errs = proc.communicate() - except Exception as exc: - exc_type, value, traceback = sys.exc_info() - rospy.logerr("subprocess failed: {}: {}".format(exc_type, value)) - proc.kill() - outs, errs = proc.communicate() - stdout = outs.decode() if outs else "" - stderr = errs.decode() if errs else "" - return (stdout, stderr) - - -def main(): - rospy.init_node("syscall") - syscall_service = rospy.Service("syscall", SysCall, syscall_cb) - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/src/process/sysinfo/package.xml b/src/process/sysinfo/package.xml index 47fa2ae1..00dabd83 100644 --- a/src/process/sysinfo/package.xml +++ b/src/process/sysinfo/package.xml @@ -1,67 +1,19 @@ - + + sysinfo - 0.0.0 + 1.0.0 Provides system info - - - - Michael McDermott - - - - - + Adam Romlein Apache 2.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - custom_msgs - rospy - std_msgs - rospy + rclpy std_msgs custom_msgs - - - - + + ament_python diff --git a/src/core/kamerahealth/scripts/__init__.py b/src/process/sysinfo/resource/sysinfo similarity index 100% rename from src/core/kamerahealth/scripts/__init__.py rename to src/process/sysinfo/resource/sysinfo diff --git a/src/process/sysinfo/scripts/__init__.py b/src/process/sysinfo/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/process/sysinfo/setup.cfg b/src/process/sysinfo/setup.cfg new file mode 100644 index 00000000..ca329dbf --- /dev/null +++ b/src/process/sysinfo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sysinfo +[install] +install_scripts=$base/lib/sysinfo diff --git a/src/process/sysinfo/setup.py b/src/process/sysinfo/setup.py index 3288102d..aa6b1cc2 100755 --- a/src/process/sysinfo/setup.py +++ b/src/process/sysinfo/setup.py @@ -1,9 +1,27 @@ -#!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from glob import glob -# this function uses information from package.xml to populate dict -d = generate_distutils_setup(packages=['sysinfo'], - package_dir={'': 'src'}) +from setuptools import setup -setup(**d) +package_name = "sysinfo" + +setup( + name=package_name, + version="1.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Adam Romlein", + maintainer_email="adam.romlein@kitware.com", + description="Provides system info", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "syscall_node = sysinfo.syscall_node:main", + ], + }, +) diff --git a/src/core/kamerahealth/src/__init__.py b/src/process/sysinfo/sysinfo/__init__.py similarity index 100% rename from src/core/kamerahealth/src/__init__.py rename to src/process/sysinfo/sysinfo/__init__.py diff --git a/src/process/sysinfo/sysinfo/syscall_node.py b/src/process/sysinfo/sysinfo/syscall_node.py new file mode 100755 index 00000000..39d97b18 --- /dev/null +++ b/src/process/sysinfo/sysinfo/syscall_node.py @@ -0,0 +1,59 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- + +import sys +import subprocess +import shlex + +import rclpy +from rclpy.node import Node + +from custom_msgs.srv import SysCall + +USE_SHELL = False + + +class SysCallNode(Node): + def __init__(self): + super().__init__("syscall") + self.srv = self.create_service(SysCall, "syscall", self.syscall_cb) + + def syscall_cb(self, msg, resp): + cmdlist = shlex.split(msg.cmd) + self.get_logger().info(str(cmdlist)) + try: + proc = subprocess.Popen( + cmdlist, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=USE_SHELL + ) + except Exception: + exc_type, value, traceback = sys.exc_info() + self.get_logger().error("subprocess failed: {}: {}".format(exc_type, value)) + resp.stdout = "" + resp.stderr = "{}: {}".format(exc_type, value) + return resp + try: + outs, errs = proc.communicate() + except Exception: + exc_type, value, traceback = sys.exc_info() + self.get_logger().error("subprocess failed: {}: {}".format(exc_type, value)) + proc.kill() + outs, errs = proc.communicate() + resp.stdout = outs.decode() if outs else "" + resp.stderr = errs.decode() if errs else "" + return resp + + +def main(args=None): + rclpy.init(args=args) + node = SysCallNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/run_scripts/EXPORT_ROS_MASTER.sh b/src/run_scripts/EXPORT_ROS_MASTER.sh deleted file mode 100755 index 8f740ce8..00000000 --- a/src/run_scripts/EXPORT_ROS_MASTER.sh +++ /dev/null @@ -1,4 +0,0 @@ -. "$(catkin locate)/activate_ros.bash" - -SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -export ROS_MASTER_URI="$(cat "${SCRIPT_DIR}/ROS_MASTER.txt")" diff --git a/src/run_scripts/aliases.sh b/src/run_scripts/aliases.sh index 63830ef2..c64fa65b 100755 --- a/src/run_scripts/aliases.sh +++ b/src/run_scripts/aliases.sh @@ -6,9 +6,9 @@ if [[ -z "${KAM_REPO_DIR}" ]]; then exit 1 fi -alias cb="catkin build" -alias nch="roslaunch" -alias pub="rostopic pub --once" +alias cb="colcon build --packages-select" +alias nch="ros2 launch" +alias pub="ros2 topic pub --once" alias arc-off="set-is-archiving 0" alias arc-on="set-is-archiving 1" alias send-pulse="pub /daq/pulse std_msgs/UInt32 $1" @@ -18,8 +18,8 @@ alias set-trig-freq="redis-cli -h $REDIS_HOST set /sys/arch/trigger_freq $1" alias ips="ip -br addr" alias scan="arp-scan 192.168.88.1/24" alias tko="tmux kill-session" -alias rtls="rostopic list" -alias rnls="rosnode list" +alias rtls="ros2 topic list" +alias rnls="ros2 node list" alias kamwat="docker compose -f ${KAM_REPO_DIR}/compose/nodelist.yml run nodelist /entry/wat.sh" @@ -33,7 +33,7 @@ whoros() { set-is-archiving() { NODENAME=${2:-nuvo0} -rosservice call "/$NODENAME/set_archiving" "archiving: $1 +ros2 service call "/$NODENAME/set_archiving" custom_msgs/srv/SetArchiving "archiving: $1 project: 'bench' flight: '7' effort: 'test-effort' @@ -41,13 +41,13 @@ notes: ''" } -# catkin build shortcuts -alias cb-daq="catkin build custom_msgs mcc_daq" -alias cb-ins="catkin build custom_msgs ins_driver" -alias cb-cam="catkin build custom_msgs nexus kw_genicam_driver prosilica_camera" +# colcon build shortcuts +alias cb-daq="colcon build --packages-up-to mcc_daq" +alias cb-ins="colcon build --packages-up-to ins_driver" +alias cb-cam="colcon build --packages-up-to nexus kw_genicam_driver prosilica_camera" -alias cb-backend="catkin build backend" -alias cb-gui="catkin build wxpython_gui" +alias cb-backend="colcon build --packages-up-to backend" +alias cb-gui="colcon build --packages-up-to wxpython_gui" # runtime shortcuts @@ -81,22 +81,22 @@ alias run5-ir="run-ir" # bring up nexus run-nexus() { - roslaunch --wait nexus nexus.launch system_name:=${NODE_HOSTNAME} + ros2 launch view_server image_view_server.launch.xml } alias run6-nexus="run-nexus" run-debay() { - roslaunch --wait color_processing debayer.launch system_name:=${NODE_HOSTNAME} + echo "debayer moved into the phase_one driver; no separate node" } alias run7-debay="run-debay" run-imageview() { - roslaunch --wait wxpython_gui image_view_server.launch system_name:=${NODE_HOSTNAME} + ros2 launch view_server image_view_server.launch.xml } alias run8-imageview="run-imageview" -alias run-gui="roslaunch --wait wxpython_gui system_control_panel.launch" +alias run-gui="ros2 launch wxpython_gui system_control_panel.launch.xml" @@ -107,8 +107,8 @@ check-nuvos() { } kill-rgb() { - echo "rosnode kill /subsys${1}/rgb_driver" - rosnode kill /subsys${1}/rgb/rgb_driver + echo "ROS2 has no rosnode kill; stopping the container/process instead" + pkill -INT -f rgb_driver || true } kill-rgb-all() { @@ -119,12 +119,10 @@ kill-rgb-all() { kill-nodes() { - nodes=$(rosnode list /subsys${1}/) - rosnode kill ${nodes} + echo "ROS2 has no rosnode kill; stop the supervisor programs instead" } kill-nodes-all() { - nodes=$(rosnode list /) - rosnode kill ${nodes} + echo "ROS2 has no rosnode kill; stop the supervisor programs instead" } diff --git a/src/run_scripts/diag/rosnet.sh b/src/run_scripts/diag/rosnet.sh index dfeccf5d..ecf4f64b 100755 --- a/src/run_scripts/diag/rosnet.sh +++ b/src/run_scripts/diag/rosnet.sh @@ -3,9 +3,8 @@ ## Diagnostics for ros networking ## From satellite -# bare minimum - if this fails, you can't see the rosmaster at all -rostopic list +# bare minimum - if this fails, DDS discovery is broken +ros2 topic list # Should be able to get basic info -rosnode info ${node} - +ros2 node info ${node} diff --git a/src/run_scripts/entry/debay.sh b/src/run_scripts/entry/debay.sh deleted file mode 100755 index eedd6f1e..00000000 --- a/src/run_scripts/entry/debay.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash - -# Nexus node startup script - -echo "? @ ? DeBAYERING ? @ ? " -source /entry/project.sh -source /aliases.sh -NODE_HOSTNAME=${NODE_HOSTNAME:-undefined} -roslaunch --wait color_processing debayer.launch system_name:=${NODE_HOSTNAME} diff --git a/src/run_scripts/entry/health_cam.sh b/src/run_scripts/entry/health_cam.sh index e1ef89c7..9236f378 100755 --- a/src/run_scripts/entry/health_cam.sh +++ b/src/run_scripts/entry/health_cam.sh @@ -12,4 +12,4 @@ if [[ -z "${HOSTNAME}" ]]; then exit 1 fi -rosservice call "/${HOSTNAME}/${CAM_MODE}/${CAM_MODE}_driver/health" "{}" \ No newline at end of file +ros2 service call "/${HOSTNAME}/${CAM_MODE}/${CAM_MODE}_driver/health" std_srvs/srv/Trigger "{}" \ No newline at end of file diff --git a/src/run_scripts/entry/healthcheck.sh b/src/run_scripts/entry/healthcheck.sh index 91609b83..dc623baa 100755 --- a/src/run_scripts/entry/healthcheck.sh +++ b/src/run_scripts/entry/healthcheck.sh @@ -4,4 +4,4 @@ set -e # setup ros environment source "/opt/ros/$ROS_DISTRO/setup.bash" HOST=${REDIS_HOST:-nuvo0} -rosservice call /${HOST}/rgb/rgb_driver/health "{}" +ros2 service call /${HOST}/rgb/rgb_driver/health std_srvs/srv/Trigger "{}" diff --git a/src/run_scripts/entry/publish_sync_msgs.sh b/src/run_scripts/entry/publish_sync_msgs.sh index d1e10e04..7c71f2d6 100755 --- a/src/run_scripts/entry/publish_sync_msgs.sh +++ b/src/run_scripts/entry/publish_sync_msgs.sh @@ -7,7 +7,7 @@ source /entry/project.sh source /aliases.sh # Launch image directory publisher from specified dir -exec roslaunch --wait sprokit_adapters publish_sync_msgs.launch \ - publish_rate:=1 \ - out_topic:="/${NODE_HOSTNAME}/synched" \ - flight_dir:="/mnt/data/testset" +exec ros2 run sprokit_adapters publish_sync_msgs.py --ros-args \ + -p publish_rate:=1.0 \ + -p out_topic:="/${NODE_HOSTNAME}/synched" \ + -p flight_dir:="/mnt/data/testset" diff --git a/src/run_scripts/entry/ros2jaeger.sh b/src/run_scripts/entry/ros2jaeger.sh deleted file mode 100755 index 524f56e4..00000000 --- a/src/run_scripts/entry/ros2jaeger.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash - -echo "<> <> <> ROS2JAEGER <> <> <> " -source /entry/project.sh - -ROSWAIT="--wait" - -catkin build ros2jaeger - -exec roslaunch "${ROSWAIT}" ros2jaeger ros2jaeger.launch \ - system_name:=${NODE_HOSTNAME} \ diff --git a/src/run_scripts/entry/spoofins.sh b/src/run_scripts/entry/spoofins.sh index e16e8916..3890d9b1 100755 --- a/src/run_scripts/entry/spoofins.sh +++ b/src/run_scripts/entry/spoofins.sh @@ -5,4 +5,4 @@ echo "( ) ( ) ( ) SPOOOOOOF INS ( ) ( ) ( ) " source /entry/project.sh source /aliases.sh -roslaunch --wait ins_driver ins.launch spoof:=${SPOOF_INS} \ No newline at end of file +SPOOF_RATE=${SPOOF_INS} exec ros2 launch ins_driver ins.launch.xml \ No newline at end of file diff --git a/src/run_scripts/entry/syscall.sh b/src/run_scripts/entry/syscall.sh index f2f5445e..f53d2355 100755 --- a/src/run_scripts/entry/syscall.sh +++ b/src/run_scripts/entry/syscall.sh @@ -5,8 +5,5 @@ echo "<> <> <> SysCall <> <> <> " source /entry/project.sh -ROSWAIT="--wait" - -exec roslaunch "${ROSWAIT}" sysinfo syscall.launch \ - system_name:=`hostname` \ - verbosity:=$(/cfg/get ".verbosity") +exec ros2 launch sysinfo syscall.launch.xml \ + system_name:=`hostname` diff --git a/src/run_scripts/entry/wat.sh b/src/run_scripts/entry/wat.sh index 1aebbb4e..c8030a50 100755 --- a/src/run_scripts/entry/wat.sh +++ b/src/run_scripts/entry/wat.sh @@ -1,6 +1,6 @@ #!/bin/bash -# Diagnostics +# Diagnostics errcho() { (>&2 echo -e "\e[31m$1\e[0m") @@ -10,39 +10,26 @@ echo "[?] [?] [?] WAT. [?] [?] [?] " source /entry/project_env.sh -MASTER_HOST=$(echo $ROS_MASTER_URI| grep -Po -e '(?<=http:\/\/)([\w\.]+)(?=:)') -echo "MASTER_HOST: " - # Expected exit code from a Ctrl-C when in explicit docker run mode. trap "errcho 'Caught SIGINT'; cleanup" SIGINT # Expected exit code from docker stop command. trap "errcho 'Caught SIGTERM'; cleanup" SIGTERM -echo "=== === === === Looking for ROS_MASTER === === === === " +echo "=== === === === ROS2 environment === === === === " +echo "ROS_DISTRO : ${ROS_DISTRO}" +echo "ROS_DOMAIN_ID : ${ROS_DOMAIN_ID}" +echo "RMW_IMPLEMENTATION: ${RMW_IMPLEMENTATION:-default}" + echo "=== === === === /etc/hosts: === === === === " cat /etc/hosts echo "=== === === === /etc/resolv.conf: === === === === " cat /etc/resolv.conf -echo "=== === === === ping ${MASTER_HOST} === === === === " -ping -c1 -W1 "${MASTER_HOST}" - -echo "=== === === === nslookup ${MASTER_HOST} === === === === " -nslookup "${MASTER_HOST}" - -echo "=== === === === dig ${MASTER_HOST} === === === === " -dig ${MASTER_HOST} -MASTER_IP=$(dig +short ${MASTER_HOST}) -echo $MASTER_IP - -if [[ -z $MASTER_IP ]]; then - errcho "FATAL. Cannot resolve to master IP. This is a nonstarter \n :( :( :(" - exit 1 -fi - -echo "=== === === === dig MASTER_IP (${MASTER_IP}) === === === === " -nslookup $MASTER_IP +echo "=== === === === visible nodes === === === === " +ros2 node list || true -exec roswtf +echo "=== === === === visible topics === === === === " +ros2 topic list || true +exec ros2 doctor --report diff --git a/src/run_scripts/run_detector.sh b/src/run_scripts/run_detector.sh index 583376b1..827953ac 100755 --- a/src/run_scripts/run_detector.sh +++ b/src/run_scripts/run_detector.sh @@ -18,8 +18,7 @@ IMAGE_LIST_DIR=$DETECTION_CSV_DIR mkdir -p ${DETECTION_CSV_DIR} mkdir -p ${IMAGE_LIST_DIR} -roslaunch sprokit_adapters sprokit_detector_fusion_adapter.launch \ - kwiver:=${WS_DEVEL} \ +ros2 launch sprokit_adapters sprokit_detector_fusion_adapter.launch.xml \ detector_node:=detector \ detection_pipefile:="${PIPEFILE}" \ embed_det_chips:=true \ From 41a9312748537f0194ec3662c489464e326b87a5 Mon Sep 17 00:00:00 2001 From: romleiaj Date: Fri, 3 Jul 2026 21:58:02 -0400 Subject: [PATCH 16/20] Sweep last ROS_MASTER_URI / catkin references from dev+setup scripts check_system/setup_kamera_env/basic-aliases/aliases setmaster helper, prosilica test_homography, genicam run_simple_driver (now points at the installed a6750 node via ros2 pkg prefix), and the uas tmux env all move to ROS_DOMAIN_ID / ros2 CLI equivalents. Remaining grep hits for rospy/catkin in the tree are comments and docstrings only. --- src/cams/kw_genicam_driver/scripts/run_simple_driver.sh | 9 +++------ src/cams/prosilica_camera/test_homography.sh | 4 ++-- src/run_scripts/alias/basic-aliases.sh | 2 +- src/run_scripts/aliases.sh | 4 ++-- src/run_scripts/entry/setup_kamera_env.sh | 3 ++- src/run_scripts/setup/check_system.sh | 2 +- tmux/uas/env.sh | 3 ++- 7 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/cams/kw_genicam_driver/scripts/run_simple_driver.sh b/src/cams/kw_genicam_driver/scripts/run_simple_driver.sh index b8c5df55..035f2f40 100755 --- a/src/cams/kw_genicam_driver/scripts/run_simple_driver.sh +++ b/src/cams/kw_genicam_driver/scripts/run_simple_driver.sh @@ -1,10 +1,8 @@ #!/bin/bash set -e -CATKIN_DEVEL="$(catkin locate -d)" - # Make sure driver executable has setcap run on it -NODE_EXE="${CATKIN_DEVEL}/.private/kw_genicam_driver/lib/kw_genicam_driver/simple_driver_node" +NODE_EXE="$(ros2 pkg prefix kw_genicam_driver)/lib/kw_genicam_driver/a6750_driver_node" if [ ! -f "${NODE_EXE}" ] then echo "ERROR: Expected location of driver node was not found: ${NODE_EXE}" @@ -17,8 +15,7 @@ sudo setcap cap_net_raw+ep "${NODE_EXE}" NET_IFACE=enp4s0 sudo /usr/dalsa/GigeV/bin/gev_nettweak ${NET_IFACE} -roslaunch kw_genicam_driver genicam_simple.launch \ - debug:=true \ +ros2 launch kw_genicam_driver flir_a6750.launch.xml \ namespace:=/test \ firmware_mode:=bayer \ camera_serial:=S1125704 \ @@ -27,4 +24,4 @@ roslaunch kw_genicam_driver genicam_simple.launch \ frame_id:=/test/camera/cueing/0 \ output_topic_raw:=camera/cueing/0/bayer_image_raw \ output_topic_debayer:=camera/cueing/0/image_raw \ - output_frame_rate:=3 + output_frame_rate:=3.0 diff --git a/src/cams/prosilica_camera/test_homography.sh b/src/cams/prosilica_camera/test_homography.sh index 7cba7d3c..bf1acf89 100755 --- a/src/cams/prosilica_camera/test_homography.sh +++ b/src/cams/prosilica_camera/test_homography.sh @@ -1,8 +1,8 @@ #!/bin/bash -export ROS_MASTER_URI=http://nuvo0:11311/ +# ROS2: uses DDS discovery; ensure ROS_DOMAIN_ID matches the system -rosservice call /nuvo2/uv/uv_view_service/get_image_view "homography: [1,0,0,0,1,0,0,0,1] +ros2 service call /nuvo2/uv/uv_view_service/get_image_view custom_msgs/srv/RequestImageView "homography: [1,0,0,0,1,0,0,0,1] output_height: 2 output_width: 2 interpolation: 0 diff --git a/src/run_scripts/alias/basic-aliases.sh b/src/run_scripts/alias/basic-aliases.sh index 27b30fe6..3047a212 100755 --- a/src/run_scripts/alias/basic-aliases.sh +++ b/src/run_scripts/alias/basic-aliases.sh @@ -13,7 +13,7 @@ alias h="history -i | grep -Pv '^ *[0-9]+ [[:digit:]\:\- ]{16} h '| grep " alias gp="git pull --ff-only" alias plsub="git pull --recurse-submodules && git submodule update --init --recursive" alias p="ping -c 1" -alias pros="ping -c 1 $ROS_MASTER_URI" +alias pros="ros2 doctor" alias sapt="sudo apt-get install" alias ..="cd .." alias ll="ls -lh --color=tty" diff --git a/src/run_scripts/aliases.sh b/src/run_scripts/aliases.sh index c64fa65b..35cadd77 100755 --- a/src/run_scripts/aliases.sh +++ b/src/run_scripts/aliases.sh @@ -24,11 +24,11 @@ alias rnls="ros2 node list" alias kamwat="docker compose -f ${KAM_REPO_DIR}/compose/nodelist.yml run nodelist /entry/wat.sh" setru() { - export ROS_MASTER_URI=http://${1}:11311/ + export ROS_DOMAIN_ID=${1} } whoros() { - echo $ROS_MASTER_URI + echo $ROS_DOMAIN_ID } set-is-archiving() { diff --git a/src/run_scripts/entry/setup_kamera_env.sh b/src/run_scripts/entry/setup_kamera_env.sh index 2fdcbb75..1fd0a212 100755 --- a/src/run_scripts/entry/setup_kamera_env.sh +++ b/src/run_scripts/entry/setup_kamera_env.sh @@ -60,4 +60,5 @@ fi ## This is some jankitude to set the pod. I really wanna fix this export NODE_HOSTNAME -export ROS_MASTER_URI="http://${MASTER_HOST}:${_ROS_PORT}/" +# ROS2: DDS discovery replaces the ROS master; share ROS_DOMAIN_ID instead +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" diff --git a/src/run_scripts/setup/check_system.sh b/src/run_scripts/setup/check_system.sh index 29a8f31d..68a4da8f 100755 --- a/src/run_scripts/setup/check_system.sh +++ b/src/run_scripts/setup/check_system.sh @@ -106,7 +106,7 @@ check_command W jq logbold "checking other conditions:" -for VNAME in REDIS_HOST ROS_MASTER_URI +for VNAME in REDIS_HOST ROS_DOMAIN_ID do if [[ -z "${!VNAME}" ]]; then logyel "UNSET :\e[1m ${VNAME}=" diff --git a/tmux/uas/env.sh b/tmux/uas/env.sh index cfbf7a6d..db8bcc77 100644 --- a/tmux/uas/env.sh +++ b/tmux/uas/env.sh @@ -21,7 +21,8 @@ unset _redis_elapsed echo "Redis successfully connected at ${REDIS_HOST}, starting." export NODE_HOSTNAME=$(hostname) -export ROS_MASTER_URI="http://${REDIS_HOST}:11311" +# ROS2: peer discovery via DDS; all hosts must share a domain id +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" export KAMERA_DIR="/home/user/kw/kamera" export DATA_MOUNT_POINT=$(redis-cli --raw -h ${REDIS_HOST} get /sys/arch/base) export CAM_FOV=$(redis-cli --raw -h ${REDIS_HOST} get /sys/${NODE_HOSTNAME}/cam_fov) From 59ecf917734c4d3f02ea8f2f0faf0a2574c7cdcd Mon Sep 17 00:00:00 2001 From: romleiaj Date: Fri, 3 Jul 2026 22:02:23 -0400 Subject: [PATCH 17/20] Refresh gui-deps base image for Jazzy / Ubuntu 24.04 - libgl1-mesa-glx no longer exists on noble; use libgl1 + libglx-mesa0 - pip installs use --break-system-packages (PEP 668) and drop the pip self-upgrade; Pillow folded into the single install layer - Drop the legacy 'PyGeodesy<19.12' pin (py2-era, untested on py3): the GUI uses pygeodesy.geoids.GeoidPGM, which the unpinned core-deps install provides and shapefile_monitor already runs against - Drop the pip install of roskv: its setup.py was removed in the ROS2 port, and gui.dockerfile's colcon --packages-up-to wxpython_gui builds roskv into the workspace overlay instead --- docker-compose.yml | 2 +- docker/base/gui-deps.dockerfile | 26 +++++++++++++------------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/docker-compose.yml b/docker-compose.yml index b0424b26..d3b7e645 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -125,7 +125,7 @@ services: ## ========================== GUI chain ======================================== - # GUI deps layered on the Noetic core-deps image. + # GUI deps layered on the Jazzy core-deps image. gui-deps: build: context: . diff --git a/docker/base/gui-deps.dockerfile b/docker/base/gui-deps.dockerfile index fbf55331..33e1daf7 100644 --- a/docker/base/gui-deps.dockerfile +++ b/docker/base/gui-deps.dockerfile @@ -1,12 +1,14 @@ -## GUI deps layered on the Noetic core-deps chain. +## GUI deps layered on the Jazzy core-deps chain. FROM kamera/base/core-deps:latest +# libgl1-mesa-glx was dropped in Ubuntu 24.04; libgl1 + libglx-mesa0 replace it RUN apt-get update && apt-get install -y --no-install-recommends \ gdal-bin \ python3-gdal \ python3-tk \ python3-wxgtk4.0 \ - libgl1-mesa-glx \ + libgl1 \ + libglx-mesa0 \ libqt5x11extras5 \ locales \ && rm -rf /var/lib/apt/lists/* @@ -18,19 +20,17 @@ ENV LANG=en_US.UTF-8 \ LANGUAGE=en_US:en \ LC_ALL=en_US.UTF-8 -RUN pip install --upgrade \ - pip \ - Pillow - -# numpy/scipy/shapely/pyshp come from core-deps; only GUI-unique deps here. -# TODO: validate the unpinned pygeodesy already installed by core-deps and drop -# this <19.12 downgrade (legacy Py2-era pin, untested on Py3). -RUN pip install --no-cache-dir \ - 'PyGeodesy<19.12' \ +# numpy/scipy/shapely/pyshp/pygeodesy come from core-deps; only GUI-unique deps +# here. The legacy 'PyGeodesy<19.12' pin is gone: the GUI uses +# pygeodesy.geoids.GeoidPGM, which the unpinned core-deps install provides +# (shapefile_monitor already runs against it). +RUN pip install --break-system-packages --no-cache-dir \ + Pillow \ exifread \ ipython \ psutil \ simplekml -COPY src/core/roskv /src/roskv -RUN pip install --no-cache-dir '/src/roskv[redis]' +# roskv is no longer pip-installable (its setup.py was removed in the ROS2 +# port); it is built into the colcon workspace by gui.dockerfile via +# --packages-up-to wxpython_gui, which pulls in roskv as a dependency. From 242b763e94be3942fb58de4c44a46ebd2071d201 Mon Sep 17 00:00:00 2001 From: "cameron.johnson" Date: Fri, 14 Aug 2026 10:20:09 -0400 Subject: [PATCH 18/20] merged tmux/nayak and tmux/taiga --- provision/ansible/hosts.yml | 16 +-- provision/ansible/playbooks/cas/configure.yml | 12 ++ src/cfg/nayak/supervisor_group_follower.conf | 9 ++ src/cfg/nayak/supervisor_group_leader.conf | 9 ++ src/cfg/taiga/supervisor_group_follower.conf | 9 ++ src/cfg/taiga/supervisor_group_leader.conf | 9 ++ tmux/{taiga => crewed}/env.sh | 0 .../follower/start_tmux_session.sh | 0 tmux/crewed/follower/supervisor.conf | 78 ++++++++++++ tmux/{nayak => crewed}/leader/ir.sh | 0 .../{nayak => crewed}/leader/restart_redis.sh | 0 .../leader/start_tmux_session.sh | 0 tmux/crewed/leader/supervisor.conf | 114 +++++++++++++++++ tmux/{nayak => crewed}/mount_nas.sh | 0 tmux/{nayak => crewed}/start_gui.sh | 0 tmux/{taiga => crewed}/startup.sh | 0 tmux/nayak/env.sh | 60 --------- tmux/nayak/follower/supervisor.conf | 81 ------------ tmux/nayak/leader/supervisor.conf | 117 ------------------ tmux/nayak/startup.sh | 7 -- tmux/taiga/follower/start_tmux_session.sh | 24 ---- tmux/taiga/follower/supervisor.conf | 81 ------------ tmux/taiga/leader/ir.sh | 10 -- tmux/taiga/leader/restart_redis.sh | 5 - tmux/taiga/leader/start_tmux_session.sh | 24 ---- tmux/taiga/leader/supervisor.conf | 117 ------------------ tmux/taiga/mount_nas.sh | 17 --- tmux/taiga/start_gui.sh | 13 -- 28 files changed, 249 insertions(+), 563 deletions(-) create mode 100644 src/cfg/nayak/supervisor_group_follower.conf create mode 100644 src/cfg/nayak/supervisor_group_leader.conf create mode 100644 src/cfg/taiga/supervisor_group_follower.conf create mode 100644 src/cfg/taiga/supervisor_group_leader.conf rename tmux/{taiga => crewed}/env.sh (100%) rename tmux/{nayak => crewed}/follower/start_tmux_session.sh (100%) create mode 100644 tmux/crewed/follower/supervisor.conf rename tmux/{nayak => crewed}/leader/ir.sh (100%) rename tmux/{nayak => crewed}/leader/restart_redis.sh (100%) rename tmux/{nayak => crewed}/leader/start_tmux_session.sh (100%) create mode 100644 tmux/crewed/leader/supervisor.conf rename tmux/{nayak => crewed}/mount_nas.sh (100%) rename tmux/{nayak => crewed}/start_gui.sh (100%) rename tmux/{taiga => crewed}/startup.sh (100%) delete mode 100644 tmux/nayak/env.sh delete mode 100644 tmux/nayak/follower/supervisor.conf delete mode 100644 tmux/nayak/leader/supervisor.conf delete mode 100755 tmux/nayak/startup.sh delete mode 100755 tmux/taiga/follower/start_tmux_session.sh delete mode 100644 tmux/taiga/follower/supervisor.conf delete mode 100755 tmux/taiga/leader/ir.sh delete mode 100755 tmux/taiga/leader/restart_redis.sh delete mode 100755 tmux/taiga/leader/start_tmux_session.sh delete mode 100644 tmux/taiga/leader/supervisor.conf delete mode 100755 tmux/taiga/mount_nas.sh delete mode 100755 tmux/taiga/start_gui.sh diff --git a/provision/ansible/hosts.yml b/provision/ansible/hosts.yml index da28beda..818edd28 100644 --- a/provision/ansible/hosts.yml +++ b/provision/ansible/hosts.yml @@ -22,7 +22,7 @@ all: hosts: center0taiga: redis_host: true - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/leader/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/leader/supervisor.conf" leader: True follower: False gui: True @@ -30,19 +30,20 @@ all: ssd_id: "ca0a9985-84ac-4019-95a6-242d3c81c86d" left1taiga: redis_host: False - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/follower/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/follower/supervisor.conf" leader: False follower: True ssd_id: "d31ab8b6-b273-49c3-bd32-56c4fb76219c" right2taiga: redis_host: False - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/follower/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/follower/supervisor.conf" leader: False follower: True ssd_id: "883fec5d-e6a2-454a-8239-438e2fbc56c3" vars: gui: False config_dir: "taiga" + tmux_dir: "crewed" nvidia_cuda: True uav: hosts: @@ -75,29 +76,30 @@ all: hosts: center0nayak: redis_host: true - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/leader/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/leader/supervisor.conf" leader: True follower: False ssd_id: "3ca48dde-3b0f-440a-bbc2-69e6bd559325" left1nayak: redis_host: False - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/follower/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/follower/supervisor.conf" leader: False follower: True ssd_id: "40d83577-292f-4795-b275-9b3c38504c22" right2nayak: redis_host: False - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/follower/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/follower/supervisor.conf" leader: False follower: True ssd_id: "1b5b6c41-48f2-43fe-82db-fdc598387ea3" center-bak-nayak: redis_host: True - supervisor_file: "{{ kamera_dir }}/tmux/{{ config_dir }}/leader/supervisor.conf" + supervisor_file: "{{ kamera_dir }}/tmux/{{ tmux_dir }}/leader/supervisor.conf" leader: True follower: False ssd_id: "TODO" vars: gui: False config_dir: "nayak" + tmux_dir: "crewed" nvidia_cuda: True diff --git a/provision/ansible/playbooks/cas/configure.yml b/provision/ansible/playbooks/cas/configure.yml index 7a6ab331..31c9c1cc 100644 --- a/provision/ansible/playbooks/cas/configure.yml +++ b/provision/ansible/playbooks/cas/configure.yml @@ -62,6 +62,18 @@ state: link force: True + # The [program:*] definitions are shared across crewed systems + # (tmux/crewed); this links in the per-system [group:] that names + # what operators address as `supervisorctl restart :*`. + - name: Link per-system supervisor group + become: True + file: + src: "{{ kamera_dir }}/src/cfg/{{ config_dir }}/supervisor_group_{{ 'leader' if leader else 'follower' }}.conf" + dest: /etc/supervisor/conf.d/{{ config_dir }}-group.conf + state: link + force: True + owner: root + - name: Copy over supervisor config for sudoless and RPC become: True copy: diff --git a/src/cfg/nayak/supervisor_group_follower.conf b/src/cfg/nayak/supervisor_group_follower.conf new file mode 100644 index 00000000..b8a1a389 --- /dev/null +++ b/src/cfg/nayak/supervisor_group_follower.conf @@ -0,0 +1,9 @@ +; Per-system supervisor group. The [program:*] definitions live in the +; shared tmux/crewed/follower/supervisor.conf; this file names the group the +; operators address (supervisorctl restart nayak:*) and lists which of the +; shared programs this system actually runs. supervisord merges every +; file matched by [include] in supervisord.conf, so a group here can +; reference programs defined there. + +[group:nayak] +programs=cam_ir,cam_uv,cam_rgb,imageview,detector,fps_monitor diff --git a/src/cfg/nayak/supervisor_group_leader.conf b/src/cfg/nayak/supervisor_group_leader.conf new file mode 100644 index 00000000..8d83779e --- /dev/null +++ b/src/cfg/nayak/supervisor_group_leader.conf @@ -0,0 +1,9 @@ +; Per-system supervisor group. The [program:*] definitions live in the +; shared tmux/crewed/leader/supervisor.conf; this file names the group the +; operators address (supervisorctl restart nayak:*) and lists which of the +; shared programs this system actually runs. supervisord merges every +; file matched by [include] in supervisord.conf, so a group here can +; reference programs defined there. + +[group:nayak] +programs=ins,daq,cam_param_monitor,fps_monitor,shapefile_monitor,cam_ir,cam_uv,cam_rgb,imageview,detector diff --git a/src/cfg/taiga/supervisor_group_follower.conf b/src/cfg/taiga/supervisor_group_follower.conf new file mode 100644 index 00000000..aae80b30 --- /dev/null +++ b/src/cfg/taiga/supervisor_group_follower.conf @@ -0,0 +1,9 @@ +; Per-system supervisor group. The [program:*] definitions live in the +; shared tmux/crewed/follower/supervisor.conf; this file names the group the +; operators address (supervisorctl restart taiga:*) and lists which of the +; shared programs this system actually runs. supervisord merges every +; file matched by [include] in supervisord.conf, so a group here can +; reference programs defined there. + +[group:taiga] +programs=cam_ir,cam_uv,cam_rgb,imageview,detector,fps_monitor diff --git a/src/cfg/taiga/supervisor_group_leader.conf b/src/cfg/taiga/supervisor_group_leader.conf new file mode 100644 index 00000000..464194ef --- /dev/null +++ b/src/cfg/taiga/supervisor_group_leader.conf @@ -0,0 +1,9 @@ +; Per-system supervisor group. The [program:*] definitions live in the +; shared tmux/crewed/leader/supervisor.conf; this file names the group the +; operators address (supervisorctl restart taiga:*) and lists which of the +; shared programs this system actually runs. supervisord merges every +; file matched by [include] in supervisord.conf, so a group here can +; reference programs defined there. + +[group:taiga] +programs=ins,daq,cam_param_monitor,fps_monitor,shapefile_monitor,cam_ir,cam_uv,cam_rgb,imageview,detector diff --git a/tmux/taiga/env.sh b/tmux/crewed/env.sh similarity index 100% rename from tmux/taiga/env.sh rename to tmux/crewed/env.sh diff --git a/tmux/nayak/follower/start_tmux_session.sh b/tmux/crewed/follower/start_tmux_session.sh similarity index 100% rename from tmux/nayak/follower/start_tmux_session.sh rename to tmux/crewed/follower/start_tmux_session.sh diff --git a/tmux/crewed/follower/supervisor.conf b/tmux/crewed/follower/supervisor.conf new file mode 100644 index 00000000..ff88d16c --- /dev/null +++ b/tmux/crewed/follower/supervisor.conf @@ -0,0 +1,78 @@ +[program:kamerad] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh kamerad +user=user +autostart=true +autorestart=true + +[program:image_manager] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh image_manager +user=user +autostart=false + +[program:mount_nas] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/mount_nas.sh +startsecs=0 +user=root +autostart=true + +[program:host_shutdown] +command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh +startsecs=0 +user=root +autostart=false +autorestart=false + +[program:host_reboot] +command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh +startsecs=0 +user=root +autostart=false +autorestart=false + +[program:cam_ir] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh cam_ir +user=user +autostart=false + +[program:cam_uv] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh cam_uv +user=user +autostart=false + +[program:cam_rgb] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh cam_rgb +user=user +autostart=false + +[program:imageview] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh imageview +user=user +autostart=false + +[program:fps_monitor] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh fps_monitor +user=user +autostart=false + +[program:detector] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh detector +user=user +autostart=false + +[program:flight_summary] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh flight_summary +user=user +autostart=false + +[program:homography] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh homography +user=user +autostart=false + +[program:detections] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/follower/start_tmux_session.sh detections +user=user +autostart=false + +[group:postproc] +programs=flight_summary,detections,homography diff --git a/tmux/nayak/leader/ir.sh b/tmux/crewed/leader/ir.sh similarity index 100% rename from tmux/nayak/leader/ir.sh rename to tmux/crewed/leader/ir.sh diff --git a/tmux/nayak/leader/restart_redis.sh b/tmux/crewed/leader/restart_redis.sh similarity index 100% rename from tmux/nayak/leader/restart_redis.sh rename to tmux/crewed/leader/restart_redis.sh diff --git a/tmux/nayak/leader/start_tmux_session.sh b/tmux/crewed/leader/start_tmux_session.sh similarity index 100% rename from tmux/nayak/leader/start_tmux_session.sh rename to tmux/crewed/leader/start_tmux_session.sh diff --git a/tmux/crewed/leader/supervisor.conf b/tmux/crewed/leader/supervisor.conf new file mode 100644 index 00000000..c158f125 --- /dev/null +++ b/tmux/crewed/leader/supervisor.conf @@ -0,0 +1,114 @@ +[program:kamerad] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh kamerad +user=user +autostart=true +autorestart=true + +[program:image_manager] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh image_manager +user=user +autostart=false + +[program:restart_redis] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/restart_redis.sh +startsecs=0 +user=root +autostart=true + +[program:mount_nas] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/mount_nas.sh +startsecs=0 +user=root +autostart=true + +[program:host_shutdown] +command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh +startsecs=0 +user=root +autostart=false +autorestart=false + +[program:host_reboot] +command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh +startsecs=0 +user=root +autostart=false +autorestart=false + +[program:docker_registry] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh registry +user=user +autostart=true + +[program:core_init] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh core_init +user=user +autostart=false + +[program:cam_param_monitor] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh cam_param_monitor +user=user +autostart=false + +[program:fps_monitor] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh fps_monitor +user=user +autostart=false + +[program:shapefile_monitor] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh shapefile_monitor +user=user +autostart=false + +[program:cam_ir] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh cam_ir +user=user +autostart=false + +[program:cam_uv] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh cam_uv +user=user +autostart=false + +[program:cam_rgb] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh cam_rgb +user=user +autostart=false + +[program:ins] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh ins +user=user +autostart=false + +[program:daq] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh daq +user=user +autostart=false + +[program:imageview] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh imageview +user=user +autostart=false + +[program:detector] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh detector +user=user +autostart=false + +[program:flight_summary] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh flight_summary +user=user +autostart=false + +[program:homography] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh homography +user=user +autostart=false + +[program:detections] +command=/bin/bash /home/user/kw/kamera/tmux/crewed/leader/start_tmux_session.sh detections +user=user +autostart=false + +[group:postproc] +programs=flight_summary,detections,homography diff --git a/tmux/nayak/mount_nas.sh b/tmux/crewed/mount_nas.sh similarity index 100% rename from tmux/nayak/mount_nas.sh rename to tmux/crewed/mount_nas.sh diff --git a/tmux/nayak/start_gui.sh b/tmux/crewed/start_gui.sh similarity index 100% rename from tmux/nayak/start_gui.sh rename to tmux/crewed/start_gui.sh diff --git a/tmux/taiga/startup.sh b/tmux/crewed/startup.sh similarity index 100% rename from tmux/taiga/startup.sh rename to tmux/crewed/startup.sh diff --git a/tmux/nayak/env.sh b/tmux/nayak/env.sh deleted file mode 100644 index 14dd7ada..00000000 --- a/tmux/nayak/env.sh +++ /dev/null @@ -1,60 +0,0 @@ -#!/bin/bash - -SYSTEM_NAME="$(cat ${HOME}/kw/SYSTEM_NAME)" -export SYSTEM_NAME - -export KAMERA_DIR=$(${HOME}/.config/kamera/repo_dir.bash) -CFG_FILE="${KAMERA_DIR}/src/cfg/${SYSTEM_NAME}/config.yaml" -cq () { - CFG_FILE=${CFG_FILE} ${KAMERA_DIR}/src/cfg/get "$@" -} - -export REDIS_HOST=$(cq ".redis_host") - -# Uncomment this line if you wish to run the GUI in "offline" mode -# (without nuvo0, 1, etc. hooked up) -# export REDIS_HOST="localhost" - -_redis_elapsed=0 -RESP=$(redis-cli -h "${REDIS_HOST}" ping 2>/dev/null) -while [ "$RESP" != "PONG" ]; do - if [ $((_redis_elapsed % 30)) -eq 0 ]; then - echo "Waiting for Redis at ${REDIS_HOST} (${_redis_elapsed}s elapsed, got '${RESP}')..." - fi - sleep 1 - _redis_elapsed=$((_redis_elapsed + 1)) - RESP=$(redis-cli -h "${REDIS_HOST}" ping 2>/dev/null) -done -unset _redis_elapsed - -echo "Redis successfully connected at ${REDIS_HOST}, starting." - -export NODE_HOSTNAME=$(hostname) -# ROS2: peer discovery via DDS; all hosts must share a domain id -export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" -export DOCKER_KAMERA_DIR="/root/kamera" -export DATA_MOUNT_POINT=$(cq .local_ssd_mnt) -export CAM_FOV=$(cq ".arch.hosts[\"${NODE_HOSTNAME}\"].fov") - -export ROS_DISTRO="jazzy" -export KAMERA_DNS_IP="192.168.88.1" -export PROJ_DIR="/root/kamera" -export PULSE_TTY=/dev/ttyS0 -export MCC_DAQ="/dev/$(readlink /dev/mcc_daq)" - -# Toggles reading detector images from NAS vs. reading from ROS msg -# INCREASED I/O -export READ_FROM_NAS=0 - -# Toggles the option to compress / decompress images within the nexus -# To test the detector's performance on compressed imagery -# INCREASED LATENCY (~0.7s) -export COMPRESS_IMAGERY=0 - -# Sets the compression used on the phase on imagery -# Best to keep within 80-100 for optimal quality -export JPEG_QUALITY=85 - -# This is now overwritten by the INS, depending if it has a lock or not, -# but leave here to initialize the state -export SPOOF_EVENTS=0 diff --git a/tmux/nayak/follower/supervisor.conf b/tmux/nayak/follower/supervisor.conf deleted file mode 100644 index fc8d80cd..00000000 --- a/tmux/nayak/follower/supervisor.conf +++ /dev/null @@ -1,81 +0,0 @@ -[program:kamerad] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh kamerad -user=user -autostart=true -autorestart=true - -[program:image_manager] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh image_manager -user=user -autostart=false - -[program:mount_nas] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/mount_nas.sh -startsecs=0 -user=root -autostart=true - -[program:host_shutdown] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:host_reboot] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:cam_ir] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh cam_ir -user=user -autostart=false - -[program:cam_uv] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh cam_uv -user=user -autostart=false - -[program:cam_rgb] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh cam_rgb -user=user -autostart=false - -[program:imageview] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh imageview -user=user -autostart=false - -[program:fps_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh fps_monitor -user=user -autostart=false - -[program:detector] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh detector -user=user -autostart=false - -[program:flight_summary] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh flight_summary -user=user -autostart=false - -[program:homography] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh homography -user=user -autostart=false - -[program:detections] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/follower/start_tmux_session.sh detections -user=user -autostart=false - -[group:nayak] -programs=cam_ir,cam_uv,cam_rgb,imageview,detector,fps_monitor - -[group:postproc] -programs=flight_summary,detections,homography diff --git a/tmux/nayak/leader/supervisor.conf b/tmux/nayak/leader/supervisor.conf deleted file mode 100644 index 71511265..00000000 --- a/tmux/nayak/leader/supervisor.conf +++ /dev/null @@ -1,117 +0,0 @@ -[program:kamerad] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh kamerad -user=user -autostart=true -autorestart=true - -[program:image_manager] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh image_manager -user=user -autostart=false - -[program:restart_redis] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/restart_redis.sh -startsecs=0 -user=root -autostart=true - -[program:mount_nas] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/mount_nas.sh -startsecs=0 -user=root -autostart=true - -[program:host_shutdown] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:host_reboot] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:docker_registry] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh registry -user=user -autostart=true - -[program:core_init] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh core_init -user=user -autostart=false - -[program:cam_param_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh cam_param_monitor -user=user -autostart=false - -[program:fps_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh fps_monitor -user=user -autostart=false - -[program:shapefile_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh shapefile_monitor -user=user -autostart=false - -[program:cam_ir] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh cam_ir -user=user -autostart=false - -[program:cam_uv] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh cam_uv -user=user -autostart=false - -[program:cam_rgb] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh cam_rgb -user=user -autostart=false - -[program:ins] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh ins -user=user -autostart=false - -[program:daq] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh daq -user=user -autostart=false - -[program:imageview] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh imageview -user=user -autostart=false - -[program:detector] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh detector -user=user -autostart=false - -[program:flight_summary] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh flight_summary -user=user -autostart=false - -[program:homography] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh homography -user=user -autostart=false - -[program:detections] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh detections -user=user -autostart=false - -[group:nayak] -programs=ins,daq,fps_monitor,cam_param_monitor,shapefile_monitor,cam_ir,cam_uv,cam_rgb,imageview,detector - -[group:postproc] -programs=flight_summary,detections,homography diff --git a/tmux/nayak/startup.sh b/tmux/nayak/startup.sh deleted file mode 100755 index 66b9d9bc..00000000 --- a/tmux/nayak/startup.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh - -#DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" -# . /opt/ros/noetic/setup.sh - -# rosclean purge -y -mkdir -p ~/.config/kamera/gui diff --git a/tmux/taiga/follower/start_tmux_session.sh b/tmux/taiga/follower/start_tmux_session.sh deleted file mode 100755 index 36f9da0a..00000000 --- a/tmux/taiga/follower/start_tmux_session.sh +++ /dev/null @@ -1,24 +0,0 @@ -#!/bin/bash - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" -if [ $# != 1 ]; then - echo "Must enter session name." >&2 - exit 1 -fi -export SESSION="$1" - -cleanup() { - echo "Stopping session ${SESSION}" - docker compose -f "${KAMERA_DIR}/compose/${SESSION}.yml" down - tmux kill-session -t "${SESSION}" 2>/dev/null -} -trap cleanup EXIT - -. "${DIR}/../startup.sh" -export KAMERA_DIR=$(${HOME}/.config/kamera/repo_dir.bash) - -echo "Starting session '${SESSION}'." -tmux new-session -d -s "${SESSION}" -c "${KAMERA_DIR}" \ - "bash -c 'source ${DIR}/../env.sh && docker compose -f compose/${SESSION}.yml up ${SESSION}'" - -sleep infinity diff --git a/tmux/taiga/follower/supervisor.conf b/tmux/taiga/follower/supervisor.conf deleted file mode 100644 index bfb77ad1..00000000 --- a/tmux/taiga/follower/supervisor.conf +++ /dev/null @@ -1,81 +0,0 @@ -[program:kamerad] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh kamerad -user=user -autostart=true -autorestart=true - -[program:image_manager] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh image_manager -user=user -autostart=false - -[program:mount_nas] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/mount_nas.sh -startsecs=0 -user=root -autostart=true - -[program:host_shutdown] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:host_reboot] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:cam_ir] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh cam_ir -user=user -autostart=false - -[program:cam_uv] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh cam_uv -user=user -autostart=false - -[program:cam_rgb] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh cam_rgb -user=user -autostart=false - -[program:imageview] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh imageview -user=user -autostart=false - -[program:fps_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh fps_monitor -user=user -autostart=false - -[program:detector] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh detector -user=user -autostart=false - -[program:flight_summary] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh flight_summary -user=user -autostart=false - -[program:homography] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh homography -user=user -autostart=false - -[program:detections] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/follower/start_tmux_session.sh detections -user=user -autostart=false - -[group:taiga] -programs=cam_ir,cam_uv,cam_rgb,imageview,detector,fps_monitor - -[group:postproc] -programs=flight_summary,detections,homography diff --git a/tmux/taiga/leader/ir.sh b/tmux/taiga/leader/ir.sh deleted file mode 100755 index b6f5fcfd..00000000 --- a/tmux/taiga/leader/ir.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash -set -e - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" -cam_id=ircam0 # IR cam - -#gc_config "${cam_id}" "PtpMode=Auto" - -export ROS_NAMESPACE="/ir" -rosrun rc_genicam_camera rc_genicam_camera _device:=$cam_id diff --git a/tmux/taiga/leader/restart_redis.sh b/tmux/taiga/leader/restart_redis.sh deleted file mode 100755 index 7bdecb27..00000000 --- a/tmux/taiga/leader/restart_redis.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -# Needed on Ubuntu 18 -sleep 10 -service redis restart diff --git a/tmux/taiga/leader/start_tmux_session.sh b/tmux/taiga/leader/start_tmux_session.sh deleted file mode 100755 index 36f9da0a..00000000 --- a/tmux/taiga/leader/start_tmux_session.sh +++ /dev/null @@ -1,24 +0,0 @@ -#!/bin/bash - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" -if [ $# != 1 ]; then - echo "Must enter session name." >&2 - exit 1 -fi -export SESSION="$1" - -cleanup() { - echo "Stopping session ${SESSION}" - docker compose -f "${KAMERA_DIR}/compose/${SESSION}.yml" down - tmux kill-session -t "${SESSION}" 2>/dev/null -} -trap cleanup EXIT - -. "${DIR}/../startup.sh" -export KAMERA_DIR=$(${HOME}/.config/kamera/repo_dir.bash) - -echo "Starting session '${SESSION}'." -tmux new-session -d -s "${SESSION}" -c "${KAMERA_DIR}" \ - "bash -c 'source ${DIR}/../env.sh && docker compose -f compose/${SESSION}.yml up ${SESSION}'" - -sleep infinity diff --git a/tmux/taiga/leader/supervisor.conf b/tmux/taiga/leader/supervisor.conf deleted file mode 100644 index 6157e801..00000000 --- a/tmux/taiga/leader/supervisor.conf +++ /dev/null @@ -1,117 +0,0 @@ -[program:kamerad] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh kamerad -user=user -autostart=true -autorestart=true - -[program:image_manager] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh image_manager -user=user -autostart=false - -[program:restart_redis] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/restart_redis.sh -startsecs=0 -user=root -autostart=true - -[program:mount_nas] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/mount_nas.sh -startsecs=0 -user=root -autostart=true - -[program:host_shutdown] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_shutdown.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:host_reboot] -command=/bin/bash /home/user/kw/kamera/provision/start_stop/host_reboot.sh -startsecs=0 -user=root -autostart=false -autorestart=false - -[program:docker_registry] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh registry -user=user -autostart=true - -[program:core_init] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh core_init -user=user -autostart=false - -[program:cam_param_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh cam_param_monitor -user=user -autostart=false - -[program:fps_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh fps_monitor -user=user -autostart=false - -[program:shapefile_monitor] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh shapefile_monitor -user=user -autostart=false - -[program:cam_ir] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh cam_ir -user=user -autostart=false - -[program:cam_uv] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh cam_uv -user=user -autostart=false - -[program:cam_rgb] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh cam_rgb -user=user -autostart=false - -[program:ins] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh ins -user=user -autostart=false - -[program:daq] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh daq -user=user -autostart=false - -[program:imageview] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh imageview -user=user -autostart=false - -[program:detector] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh detector -user=user -autostart=false - -[program:flight_summary] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh flight_summary -user=user -autostart=false - -[program:homography] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh homography -user=user -autostart=false - -[program:detections] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh detections -user=user -autostart=false - -[group:taiga] -programs=ins,daq,cam_param_monitor,fps_monitor,shapefile_monitor,cam_ir,cam_uv,cam_rgb,imageview,detector - -[group:postproc] -programs=flight_summary,detections,homography diff --git a/tmux/taiga/mount_nas.sh b/tmux/taiga/mount_nas.sh deleted file mode 100755 index e0e270d6..00000000 --- a/tmux/taiga/mount_nas.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env bash - -# check if the nas is available, and try to automount if it is -NAS_HOST=kamera_nas -if ping -c1 -W1 ${NAS_HOST}; then - mount -a - NAS_POINT=$(grep "^${NAS_HOST}" /proc/mounts | awk '{print $2}') - echo "${NAS_POINT}" - notify-send -t 5000 -i folder-open \ - "NAS mounted" "Mounted NAS to ${NAS_POINT}" 2>/dev/null || true - true -else - notify-send -t 5000 --urgency=critical -i dialog-warning \ - "NAS mount failed" "Failed to ping NAS host ${NAS_HOST}" || true - echo "Failed to ping NAS at ${NAS_HOST}" - false -fi diff --git a/tmux/taiga/start_gui.sh b/tmux/taiga/start_gui.sh deleted file mode 100755 index 2cf47ea2..00000000 --- a/tmux/taiga/start_gui.sh +++ /dev/null @@ -1,13 +0,0 @@ -#!/bin/bash - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" - -xhost + - -source $DIR/startup.sh -source $DIR/env.sh - -echo "Start gui." -cd $DIR/../.. - -docker compose -f $KAMERA_DIR/compose/gui.yml up From bf16386daab983a0eea60bcbdc37c0fb5eb37a7d Mon Sep 17 00:00:00 2001 From: "cameron.johnson" Date: Fri, 14 Aug 2026 16:54:12 -0400 Subject: [PATCH 19/20] Make fstab provisioning entries idempotent The two lineinfile tasks matched on exact line text, so any change to mount options or disk UUID appended a new fstab variant instead of replacing the old one; deployed systems accumulated stale entries (cas3 carries three generations of /mnt/data lines, including a literal by-uuid/TODO from hosts.yml's placeholder ssd_id). Key each entry on its mount point with a regexp so changes replace in place. Verified against a container seeded with cas3's exact fstab: first pass rewrites stale variants in situ, repeat passes are no-ops. lineinfile only replaces the last match, so pre-existing duplicates on deployed boxes still need a one-time manual cleanup. --- provision/ansible/playbooks/cas/configure.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/provision/ansible/playbooks/cas/configure.yml b/provision/ansible/playbooks/cas/configure.yml index 31c9c1cc..a1414a39 100644 --- a/provision/ansible/playbooks/cas/configure.yml +++ b/provision/ansible/playbooks/cas/configure.yml @@ -148,6 +148,12 @@ become: True lineinfile: path: /etc/fstab + # regexp keys the entry on its mount point so option/source changes + # REPLACE the line; without it, lineinfile appends a new variant on + # every change and stale entries accumulate (observed on cas3). + # Note lineinfile replaces only the last match: pre-existing + # duplicates from before this fix need a one-time manual cleanup. + regexp: '^\S+\s+{{ data_dir | regex_escape() }}\s+nfs\s' line: kamera_nas:/volume1/kamera/flight_data {{ data_dir }} nfs vers=3,auto,nofail,noatime,nolock,intr,tcp,actimeo=1800 0 0 state: present insertbefore: EOF @@ -163,6 +169,8 @@ become: True lineinfile: path: /etc/fstab + # keyed on the mount point for the same reason as the NAS entry + regexp: '^\S+\s+/mnt/data\s+ext4\s' line: /dev/disk/by-uuid/{{ ssd_id }} /mnt/data ext4 nofail,user,noatime 0 0 state: present insertbefore: EOF From 65ae14d993714baad1e07cf2c935dcf104cbdd3a Mon Sep 17 00:00:00 2001 From: "cameron.johnson" Date: Fri, 14 Aug 2026 16:54:33 -0400 Subject: [PATCH 20/20] Bump core base image CUDA 12.6.2 -> 13.0.3 Ubuntu 24.04 stays; CUDA moves to the newest major release. 13.0 is the ceiling for native driver support across the machines we can verify (dev box runs the r580 branch; 13.1+ would lean on CUDA minor-version compatibility there). The Phase One ImageSDKCuda ships no hard-linked CUDA sonames (runtime probe via IsCudaSupported), so the major bump is safe for it. Deployment prerequisite: every node needs NVIDIA driver >= 580 before this image lands; aircraft systems should be checked before rollout. The VIAME detector image is unaffected (separate CUDA 12.6 base from kitware/viame). --- docker/base/core-ros.dockerfile | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docker/base/core-ros.dockerfile b/docker/base/core-ros.dockerfile index 7876315c..592d267b 100644 --- a/docker/base/core-ros.dockerfile +++ b/docker/base/core-ros.dockerfile @@ -1,6 +1,9 @@ # This image contains the base of the ROS/CUDA for the system, plus # a bunch of utility packages -FROM nvidia/cuda:12.6.2-devel-ubuntu24.04 AS base_cuda_ubuntu +# CUDA 13.0 = newest major with native support on the fleet's r580 drivers +# (13.1+ would rely on minor-version compatibility on r580 hosts; every node +# needs driver >= 580 before this image deploys). +FROM nvidia/cuda:13.0.3-devel-ubuntu24.04 AS base_cuda_ubuntu WORKDIR /root # setup environment