diff --git a/compose/cam_ir.yml b/compose/cam_ir.yml index 6620c3e8..5dbf20f0 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: "jazzy" 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..4657f55b 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: "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 e93da621..2b60ef41 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: "jazzy" 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..7d202324 100644 --- a/compose/gui.yml +++ b/compose/gui.yml @@ -9,9 +9,7 @@ services: tty: true environment: REDIS_HOST: "${REDIS_HOST}" - ROS_MASTER_URI: "${ROS_MASTER_URI}" - ROS_IP: "${ROS_IP:-}" - ROS_HOSTNAME: "${ROS_HOSTNAME:-}" + ROS_DOMAIN_ID: "${ROS_DOMAIN_ID:-0}" DATA_MOUNT_POINT: "${DATA_MOUNT_POINT}" GUI_CFG_DIR: "${GUI_CFG_DIR}" DISPLAY: "${DISPLAY}" 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/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/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..592d267b 100644 --- a/docker/base/core-ros.dockerfile +++ b/docker/base/core-ros.dockerfile @@ -1,12 +1,15 @@ # 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 +# 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 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 +23,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 +61,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 +78,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..c3723408 100644 --- a/docker/base/detector-viame-deps.dockerfile +++ b/docker/base/detector-viame-deps.dockerfile @@ -1,11 +1,13 @@ -# Build off the public VIAME docker build (with ITK support) -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 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 +22,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/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. 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/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/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..a1414a39 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: @@ -136,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 @@ -151,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 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/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..9573ccff 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,22 @@ 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 + phase_one + kw_genicam_driver - - - - + ament_cmake 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) 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/phase_one/include/phase_one/phase_one_utils.h b/src/cams/cam_utils/include/cam_utils/event_cache.hpp similarity index 60% rename from src/cams/phase_one/include/phase_one/phase_one_utils.h rename to src/cams/cam_utils/include/cam_utils/event_cache.hpp index 42474d53..d28c24b9 100644 --- a/src/cams/phase_one/include/phase_one/phase_one_utils.h +++ b/src/cams/cam_utils/include/cam_utils/event_cache.hpp @@ -1,41 +1,42 @@ #pragma once -#ifndef PHASE_ONE_UTILS_H -#define PHASE_ONE_UTILS_H +#ifndef CAM_UTILS_EVENT_CACHE_HPP +#define CAM_UTILS_EVENT_CACHE_HPP + +#include +#include +#include #include -#include -#include -// ROS -#include -// Custom -#include +#include +#include +#include -const ros::Duration ZERO_DURATION(0); +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 ros::Duration ALMOST_INSTANT{0, 10}; -const ros::Duration MICROSECOND{0, 1000}; +const rclcpp::Duration ALMOST_INSTANT{0, 10}; +const rclcpp::Duration MICROSECOND{0, 1000}; // Get the absolute value of a ROS duration -ros::Duration rosabs(ros::Duration dur); +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 + 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); + 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(ros::Duration const &tolerance); + // 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.). @@ -43,10 +44,10 @@ class EventCache { // exposure large imagery (e.g. Phase One) void set_delay(double delay); - void set_delay(ros::Duration const &delay); + void set_delay(rclcpp::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); + void push_back(rclcpp::Time const &t, const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg); // Return size of this cache int size(); @@ -56,41 +57,45 @@ class EventCache { // 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); + // 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(ros::Time image_time, std_msgs::Header &head) { - return search(image_time, head, true); + 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 - ros::Duration tol; + rclcpp::Duration tol{ZERO_DURATION}; // Expected delay from event to image - ros::Duration delay; + rclcpp::Duration delay{ZERO_DURATION}; // Set time to remove messages older than when `purge` is called - ros::Duration stale_time{5}; + 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; + 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 +#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/phase_one/src/phase_one_utils.cpp b/src/cams/cam_utils/src/event_cache.cpp similarity index 52% rename from src/cams/phase_one/src/phase_one_utils.cpp rename to src/cams/cam_utils/src/event_cache.cpp index a888274b..6c641ea3 100644 --- a/src/cams/phase_one/src/phase_one_utils.cpp +++ b/src/cams/cam_utils/src/event_cache.cpp @@ -1,58 +1,61 @@ +#include +#include #include -#include -#include -// ROS -#include -// Custom -#include -#include - - -ros::Duration rosabs(ros::Duration dur) { - if (dur < ZERO_DURATION) - return -dur; - return dur; + +#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() { - ros::Duration tol(0.499); - ros::Duration delay(0); - this->set_tolerance(tol); - this->set_delay(delay); + this->set_tolerance(rclcpp::Duration::from_seconds(0.499)); + this->set_delay(rclcpp::Duration(0, 0)); } -EventCache::EventCache(ros::Duration tol, ros::Duration delay) { +EventCache::EventCache(rclcpp::Duration tol, rclcpp::Duration delay) { this->set_tolerance(tol); this->set_delay(delay); } -void EventCache::set_tolerance(ros::Duration const &tolerance) { +void EventCache::set_tolerance(rclcpp::Duration const &tolerance) { std::lock_guard guard(mutex_); this->tol = tolerance; } -void EventCache::set_delay(ros::Duration const &delay) { +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 = ros::Duration{delay}; + this->delay = rclcpp::Duration::from_seconds(delay); } -void EventCache::push_back(ros::Time const &t, const boost::shared_ptr& msg) { +void EventCache::push_back(rclcpp::Time const &t, const custom_msgs::msg::GsofEvt::ConstSharedPtr& msg) { std::lock_guard guard(mutex_); - if (!msg->sys_time.isValid()) { - ROS_ERROR("Zero/invalid sys_time encountered in EventCache::push_back()"); + if (!stamp_is_valid(msg->sys_time)) { + RCLCPP_ERROR(LOG, "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()"); + 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::GSOF_EVT(*msg)); + event_map.emplace(t, custom_msgs::msg::GsofEvt(*msg)); } int EventCache::size() { @@ -63,45 +66,48 @@ int EventCache::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 << it->second.header.stamp.sec << "." << it->second.header.stamp.nanosec + << " " << it->second.header.frame_id << "\n"; } std::cout << "\n---" << std::endl; } -bool EventCache::search(ros::Time image_time, std_msgs::Header &head, bool remove_when_found) { +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.isValid()) { - ROS_ERROR("zero/invalid image_time encountered in EventCache::search()"); + 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 - 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); + 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 << " sys: " << pair.second.sys_time << " cdt: " << corrected_delay << " ad: " << actual_delay; + 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; - head.seq = pair.second.header.seq; - best_sys_time = pair.second.sys_time; + event_num = pair.second.event_num; + best_sys_time = sys_time; + have_best = true; 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); + RCLCPP_INFO_STREAM(LOG, "Matched " << count << "/" << event_map_copy.size() << " time headers"); if (count >= 1) { - if (remove_when_found && best_sys_time.isValid()) { + if (remove_when_found && have_best) { event_map.erase(best_sys_time); } return true; @@ -111,10 +117,10 @@ bool EventCache::search(ros::Time image_time, std_msgs::Header &head, bool remov 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 ; + 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); } @@ -148,8 +154,8 @@ std::map parseParams(std::string parameters) { param_to_value[name] = value; return param_to_value; }; - - + + std::vector loadFile(std::string filename) { std::vector lines; std::ifstream inputFile(filename); @@ -164,4 +170,4 @@ std::vector loadFile(std::string filename) { } } return lines; -}; \ No newline at end of file +}; 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/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/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/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..33f85325 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 "std_msgs/msg/u_int8.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_msgs/msg/header.hpp" -#include - -#include -#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/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/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); 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/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/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/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..72571bb3 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 @@ - - - - - - - - - - - - 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/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/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/backend/scripts/__init__.py b/src/core/ins_driver/resource/ins_driver similarity index 100% rename from src/backend/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() 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/ins_driver/scripts/__init__.py b/src/core/kamcore/resource/kamcore similarity index 100% rename from src/core/ins_driver/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/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/kamcore/scripts/__init__.py b/src/core/kamerahealth/resource/kamerahealth similarity index 100% rename from src/core/kamcore/scripts/__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/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/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/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 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/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/kamerahealth/__init__.py b/src/core/ser_daq/resource/ser_daq similarity index 100% rename from src/core/kamerahealth/__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/kamerahealth/nodes/__init__.py b/src/core/ser_daq/ser_daq/__init__.py old mode 100755 new mode 100644 similarity index 100% rename from src/core/kamerahealth/nodes/__init__.py rename to src/core/ser_daq/ser_daq/__init__.py 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", + ], + }, +) 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/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 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/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/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/scripts/__init__.py b/src/kitware-ros-pkg/sensor_simulator/resource/sensor_simulator similarity index 100% rename from src/core/kamerahealth/scripts/__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 dc7b673a..ed111ee6 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,42 @@ 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} - ) - -# 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} + 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_dynamic_config - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${KWIVER_MODULE_DIR} ) +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") +install(DIRECTORY pipelines + DESTINATION share/${PROJECT_NAME}) + +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/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/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..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 @@ -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 #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; -}; 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/core/kamerahealth/src/__init__.py b/src/kitware-ros-pkg/wxpython_gui/resource/wxpython_gui similarity index 100% rename from src/core/kamerahealth/src/__init__.py rename to src/kitware-ros-pkg/wxpython_gui/resource/wxpython_gui 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/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/__init__.py b/src/process/nexus/nodes/__init__.py deleted file mode 100755 index e69de29b..00000000 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/core/roskv/src/roskv/nodes/__init__.py b/src/process/nexus/resource/nexus old mode 100755 new mode 100644 similarity index 100% rename from src/core/roskv/src/roskv/nodes/__init__.py rename to src/process/nexus/resource/nexus diff --git a/src/process/nexus/scripts/__init__.py b/src/process/nexus/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 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/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/ser_daq/scripts/__init__.py b/src/process/sysinfo/resource/sysinfo similarity index 100% rename from src/core/ser_daq/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/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/__init__.py b/src/process/sysinfo/sysinfo/__init__.py similarity index 100% rename from src/kitware-ros-pkg/rqt_sprokit_adapter/src/rqt_sprokit_adapter/__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/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/kitware-ros-pkg/kwiver_ros_param_interface/CATKIN_IGNORE b/src/process/view_server/resource/view_server similarity index 100% rename from src/kitware-ros-pkg/kwiver_ros_param_interface/CATKIN_IGNORE 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/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/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 ebdb3e17..35cadd77 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,22 +18,22 @@ 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" 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() { 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,24 +41,24 @@ 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 # 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 } @@ -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/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_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 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/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/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/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/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/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/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") 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/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/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/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/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/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/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 \ 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/taiga/env.sh b/tmux/crewed/env.sh similarity index 92% rename from tmux/taiga/env.sh rename to tmux/crewed/env.sh index 5420de8c..cbcfffeb 100644 --- a/tmux/taiga/env.sh +++ b/tmux/crewed/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="jazzy" 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/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/taiga/start_gui.sh b/tmux/crewed/start_gui.sh similarity index 79% rename from tmux/taiga/start_gui.sh rename to tmux/crewed/start_gui.sh index 34917088..2cf47ea2 100755 --- a/tmux/taiga/start_gui.sh +++ b/tmux/crewed/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/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 a6b993f6..00000000 --- a/tmux/nayak/env.sh +++ /dev/null @@ -1,61 +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 ROS_MASTER="$(cq .master_host)" -export NODE_HOSTNAME=$(hostname) -export ROS_HOSTNAME=${NODE_HOSTNAME} -export ROS_MASTER_URI="http://${ROS_MASTER}:11311" -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 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 932f660e..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:roscore] -command=/bin/bash /home/user/kw/kamera/tmux/nayak/leader/start_tmux_session.sh roscore -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/start_gui.sh b/tmux/nayak/start_gui.sh deleted file mode 100755 index 34917088..00000000 --- a/tmux/nayak/start_gui.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" - -xhost + - -source $DIR/startup.sh -source $DIR/env.sh - -if [ ${REDIS_HOST} = "localhost" ]; then - roscore& -fi - -echo "Start gui." -cd $DIR/../.. - -docker compose -f $KAMERA_DIR/compose/gui.yml up 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 a52f86bd..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:roscore] -command=/bin/bash /home/user/kw/kamera/tmux/taiga/leader/start_tmux_session.sh roscore -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/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)