diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 00000000..f1bf1f65 --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,100 @@ +version: 2 + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. +# + +# Configuration file for Circle CI +# CI will report failure if any executed command returns and error status +# Operations performed are as follows +# Build source code +# Run unit tests for C++ and Java +# Run static code analyzer for SourceCloud +# Upload test results +# Every run command should start with source ${INIT_ENV} to ensure all default dependancies are available +# TODO (CAR-6025) move to github action to take advantage of dynamic image tag and docker org to remove branch/version coupling +jobs: + build: + # Pull docker image from docker hub + # XTERM used for better catkin_make output + docker: + - image: usdotfhwastoldev/carma-base:develop + user: carma + environment: + TERM: xterm + INIT_ENV: /home/carma/.base-image/init-env.sh + resource_class: large + # Set working directory + working_directory: "/opt/carma/" + # Pull code and execute tests + # Name of each run command defines purpose use + steps: + - run: + name: Create src folder + command: | + source ${INIT_ENV} + mkdir src + cd src + mkdir CARMAAvtVimbaDriver + # Checkout PR branch + - checkout: + path: src/CARMAAvtVimbaDriver + - run: + name: Pull Deps + command: | + source ${INIT_ENV} + ./src/CARMAAvtVimbaDriver/docker/checkout.bash -r ${PWD} + - run: + name: Build Driver + command: | + source ${INIT_ENV} + export ROS_PARALLEL_JOBS='-j3 -l3' # Try to reduce memory consumption on build + build-wrapper-linux-x86-64 --out-dir /opt/carma/bw-output bash make_with_coverage.bash -m -e /opt/carma/ -o ./coverage_reports/gcov + - run: + name: Run C++ Tests + command: | + source ${INIT_ENV} + export ROS_PARALLEL_JOBS='-j3 -l3' # Try to reduce memory consumption on build + bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov + # Run SonarCloud analysis + # PR Branchs and number extracted from Circle variables and github api + # Circle CI seems to make a change to the base branch, so we must fetch --force to ensure correct git file change stats + # SONAR_SCANNER_TOKEN MUST be secured as an environment variable in Circle CI NOT in this file. + # The following sonar settings MUST be set in SonarCloud UI NOT in this file + # sonar.pullrequest.provider + # sonar.pullrequest.github.endpoint + # sonar.pullrequest.github.token.secured + # sonar.pullrequest.github.repository + # Use -X on sonar-scanner to enable debug output + - run: + name: Run Sonar Scanner + command: | + source ${INIT_ENV} + if [ -z "${CIRCLE_PULL_REQUEST}" ]; then + echo "Non-PR Build Detected. Running analysis on ${CIRCLE_BRANCH}" + cd src/CARMAAvtVimbaDriver + sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN} + exit 0; + fi + echo "PR branch ${CIRCLE_BRANCH}" + echo "Repo name ${CIRCLE_PROJECT_USERNAME}/${CIRCLE_PROJECT_REPONAME}" + echo "URL ${CIRCLE_PULL_REQUEST}" + export PR_NUM=`echo ${CIRCLE_PULL_REQUEST} | cut -d'/' -f7` + echo "PR number ${PR_NUM}" + export BASE_BRANCH_URL="https://api.github.com/repos/${CIRCLE_PROJECT_USERNAME}/${CIRCLE_PROJECT_REPONAME}/pulls/${PR_NUM}" + export TARGET_BRANCH=$(curl "$BASE_BRANCH_URL" | jq '.base.ref' | tr -d '"') + echo "Target Branch = ${TARGET_BRANCH}" + cd src/CARMAAvtVimbaDriver + git fetch --force origin ${TARGET_BRANCH}:${TARGET_BRANCH} + sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN} -Dsonar.pullrequest.base=${TARGET_BRANCH} -Dsonar.pullrequest.branch=${CIRCLE_BRANCH} -Dsonar.pullrequest.key=${PR_NUM} diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml new file mode 100644 index 00000000..c88db096 --- /dev/null +++ b/.github/workflows/docker.yml @@ -0,0 +1,8 @@ +name: Docker build +on: + pull_request: + types: [opened, synchronize, reopened] + +jobs: + docker: + uses: usdot-fhwa-stol/actions/.github/workflows/docker.yml@main \ No newline at end of file diff --git a/.github/workflows/dockerhub.yml b/.github/workflows/dockerhub.yml new file mode 100644 index 00000000..fd70cc8d --- /dev/null +++ b/.github/workflows/dockerhub.yml @@ -0,0 +1,16 @@ +name: Docker Hub build +on: + push: + branches: + - develop + - master + - "release/*" + tags: + - "carma-system-*" + +jobs: + dockerhub: + uses: usdot-fhwa-stol/actions/.github/workflows/dockerhub.yml@main + secrets: + DOCKERHUB_USERNAME: ${{ secrets.DOCKERHUB_USERNAME }} + DOCKERHUB_TOKEN: ${{ secrets.DOCKERHUB_TOKEN }} \ No newline at end of file diff --git a/.gitignore b/.gitignore index 335fe364..c078e122 100644 --- a/.gitignore +++ b/.gitignore @@ -5,7 +5,6 @@ src/VimbaCPP/ lib/ bin/ build/ -docs/ # Auto generated code cfg/cpp/ diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties new file mode 100644 index 00000000..20a7350f --- /dev/null +++ b/.sonarqube/sonar-scanner.properties @@ -0,0 +1,30 @@ +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +# Configuration file for Sonar Scanner used for CI + +sonar.projectKey=usdot-fhwa-stol_CARMAAvtVimbaDriver +sonar.organization=usdot-fhwa-stol +sonar.cfamily.build-wrapper-output=/opt/carma/bw-output +sonar.host.url=https://sonarcloud.io +sonar.sources=src/ +sonar.cfamily.gcov.reportsPath=/opt/carma/coverage_reports/gcov +sonar.coverageReportPaths=/opt/carma/coverage_reports/gcov/coverage.xml +sonar.cpp.file.suffixes=.cpp,.h,.tpp +sonar.c.file.suffixes=- +sonar.tests=test/ +# Set Git as SCM sensor +sonar.scm.disabled=false +sonar.scm.enabled=true +sonar.scm.provider=git diff --git a/CMakeLists.txt b/CMakeLists.txt index ffc947ba..15573f28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(avt_vimba_camera) +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(1) + find_package(catkin REQUIRED COMPONENTS #libvimba + cav_msgs camera_info_manager diagnostic_updater dynamic_reconfigure @@ -28,8 +32,9 @@ generate_dynamic_reconfigure_options( ) catkin_package( + #LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include - CATKIN_DEPENDS camera_info_manager diagnostic_updater dynamic_reconfigure image_geometry image_transport roscpp sensor_msgs std_msgs polled_camera + CATKIN_DEPENDS camera_info_manager diagnostic_updater dynamic_reconfigure image_geometry image_transport roscpp sensor_msgs std_msgs polled_camera cav_msgs ) ########### @@ -42,22 +47,26 @@ include_directories( include ) -# C++11 support +# C++14 support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +if(COMPILER_SUPPORTS_CXX14) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") elseif(COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") +set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") function(add_dependencies_and_linkings arg) add_dependencies(${arg} ${PROJECT_NAME}_gencfg + ${${PROJECT_NAME}_EXPORTED_TARGETS} #${libvimba_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} ) if("${ARCH}" STREQUAL i386) @@ -94,7 +103,8 @@ add_executable(mono_camera_node src/mono_camera.cpp src/avt_vimba_camera.cpp src/frame_observer.cpp -) + src/status_camera.cpp + ) add_dependencies_and_linkings(mono_camera_node) @@ -103,23 +113,31 @@ add_executable(stereo_camera_node src/stereo_camera.cpp src/avt_vimba_camera.cpp src/frame_observer.cpp -) + ) add_dependencies_and_linkings(stereo_camera_node) add_executable(sync_node src/nodes/sync_node.cpp src/sync.cpp -) + ) add_dependencies_and_linkings(sync_node) +add_library(avt_vimba_camera_lib + src/status_camera.cpp +) + +add_dependencies_and_linkings(avt_vimba_camera_lib) + + add_library(avt_camera_nodelets src/nodes/mono_camera_nodelet.cpp src/nodes/stereo_camera_nodelet.cpp src/stereo_camera.cpp src/avt_vimba_camera.cpp src/frame_observer.cpp) + add_dependencies_and_linkings(avt_camera_nodelets) ############# @@ -143,7 +161,7 @@ install(DIRECTORY include ## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES plugins.xml - launch/mono_camera.launch + #launch/mono_camera.launch launch/mono_camera_nodelet.launch launch/stereo_camera_one_node.launch launch/stereo_camera_two_nodes.launch @@ -179,3 +197,8 @@ elseif("${ARCH}" STREQUAL armv8) DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) endif() +#Test +## gtest C++ Framework +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +catkin_add_gtest(${PROJECT_NAME}-test test/status_mako_camera_test.cpp) +target_link_libraries(${PROJECT_NAME}-test avt_vimba_camera_lib ${catkin_LIBRARIES}) diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..8ee174cb --- /dev/null +++ b/Dockerfile @@ -0,0 +1,50 @@ +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +ARG DOCKER_ORG="usdotfhwastoldev" +ARG DOCKER_TAG="develop" +FROM ${DOCKER_ORG}/carma-base:${DOCKER_TAG} as base +FROM base as setup +ARG GIT_BRANCH="develop" + +ARG ROS1_PACKAGES="" +ENV ROS1_PACKAGES=${ROS1_PACKAGES} +ARG ROS2_PACKAGES="" +ENV ROS2_PACKAGES=${ROS2_PACKAGES} + +RUN mkdir ~/src +COPY --chown=carma . /home/carma/src/CARMAAvtVimbaDriver +RUN ~/src/CARMAAvtVimbaDriver/docker/checkout.bash -b ${GIT_BRANCH} +RUN ~/src/CARMAAvtVimbaDriver/docker/install.sh + +FROM base + +ARG BUILD_DATE="NULL" +ARG VERSION="NULL" +ARG VCS_REF="NULL" + +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="carma-avt-vimba-driver" +LABEL org.label-schema.description="AVT VIMBA vision driver + driver wrapper for the CARMA Platform" +LABEL org.label-schema.vendor="Leidos" +LABEL org.label-schema.version=${VERSION} +LABEL org.label-schema.url="https://highways.dot.gov/research/research-programs/operations/CARMA" +LABEL org.label-schema.vcs-url="https://github.com/usdot-fhwa-stol/avt_vimba_camera/" +LABEL org.label-schema.vcs-ref=${VCS_REF} +LABEL org.label-schema.build-date=${BUILD_DATE} + +COPY --from=setup /home/carma/install /opt/carma/install +RUN sudo chmod -R +x /opt/carma/install + +CMD [ "wait-for-it.sh", "localhost:11311", "--", "roslaunch", "avt_vimba_camera", "mono_camera.launch"] diff --git a/README.md b/README.md new file mode 100644 index 00000000..fa9b53bb --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +| CicleCI Build Status | Sonar Code Quality | DockerHub Release | DockerHub Release Candidate | DockerHub Develop | +|------|-----|-----|-----|-----| +[![CircleCI](https://img.shields.io/circleci/build/gh/usdot-fhwa-stol/avt_vimba_camera/develop?label=CircleCI)](https://app.circleci.com/pipelines/github/usdot-fhwa-stol/avt_vimba_camera?branch=develop) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-stol_CARMAAvtVimbaDriver&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-stol_CARMAAvtVimbaDriver) | [![DockerHub build](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml/badge.svg?branch=carma-system-4.5.0)](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml) | [![DockerHub build](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml/badge.svg?branch=release/lavida)](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml)| [![DockerHub build](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml/badge.svg?branch=develop)](https://github.com/usdot-fhwa-stol/avt_vimba_camera/actions/workflows/dockerhub.yml) + +# CARMAAvtVimbaDriver +This is a fork of a third party driver developed for video camera operation. It is modified for CARMA-specific use. The master repo, located [here](https://github.com/srv/avt_vimba_camera), had no readme for its home page, so this readme is added to the fork to describe how this repo fits into the rest of the CARMA environment. + +# CARMAPlatform +The primary CARMAPlatform repository can be found [here](https://github.com/usdot-fhwa-stol/carma-platform) and is part of the [USDOT FHWA STOL](https://github.com/usdot-fhwa-stol/) +github organization. Documentation on how the CARMAPlatform functions, how it will evolve over time, and how you can contribute can be found at the above links as well + +## Contribution +Welcome to the CARMA contributing guide. Please read this guide to learn about our development process, how to propose pull requests and improvements, and how to build and test your changes to this project. [CARMA Contributing Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) + +## Code of Conduct +Please read our [CARMA Code of Conduct](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Code_of_Conduct.md) which outlines our expectations for participants within the CARMA community, as well as steps to reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all and expect our code of conduct to be honored. Anyone who violates this code of conduct may be banned from the community. + +## Attribution +The development team would like to acknowledge the people who have made direct contributions to the design and code in this repository. [CARMA Attribution](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/ATTRIBUTION.txt) + +## License +By contributing to the Federal Highway Administration (FHWA) Connected Automated Research Mobility Applications (CARMA), you agree that your contributions will be licensed under its Apache License 2.0 license. [CARMA License](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/docs/License.md) + +## Contact +Please click on the CARMA logo below to visit the Federal Highway Adminstration(FHWA) CARMA website. For technical support from the CARMA team, please contact the CARMA help desk at CAVSupportServices@dot.gov. +[![CARMA Image](https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/docs/image/CARMA_icon.png)](https://highways.dot.gov/research/research-programs/operations/CARMA) diff --git a/Vimba_installation_under_Linux.pdf b/Vimba_installation_under_Linux.pdf new file mode 100755 index 00000000..7dc0e625 Binary files /dev/null and b/Vimba_installation_under_Linux.pdf differ diff --git a/Vimba_v5.0_Linux.tgz b/Vimba_v5.0_Linux.tgz new file mode 100755 index 00000000..e753fef1 Binary files /dev/null and b/Vimba_v5.0_Linux.tgz differ diff --git a/calibrations/calibration_50-0536911040.yaml b/calibrations/calibration_50-0536911040.yaml new file mode 100644 index 00000000..a9176b98 --- /dev/null +++ b/calibrations/calibration_50-0536911040.yaml @@ -0,0 +1,21 @@ +# Calibration file for the spare mobileye camera at STOL +image_width: 2064 +image_height: 1544 +camera_name: left_optical +camera_matrix: + rows: 3 + cols: 3 + data: [1477.37610504879, 0, 966.3836593155506, 0, 1470.959845844984, 721.682892292772, 0, 0, 1] +distortion_model: rational_polynomial +distortion_coefficients: + rows: 1 + cols: 8 + data: [0.1424740048592687, 2.048769427985746, 0.0008456231435827007, -0.0004816724796356829, -2.834810741450366, -0.1613802969459177, 1.425388415204544, -2.584546591478115] +rectification_matrix: + rows: 3 + cols: 3 + data: [0.9905132663703519, -0.003683047506279885, 0.1373677702570837, 0.002297487443287912, 0.9999448926725898, 0.01024368925000131, -0.1373979282805399, -0.009830909371423568, 0.9904671435868766] +projection_matrix: + rows: 3 + cols: 4 + data: [2046.340398990372, 0, 720.5770225524902, 0, 0, 2046.340398990372, 710.3249855041504, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/calibrations/camera_fl_intrinsics.yaml b/calibrations/camera_fl_intrinsics.yaml new file mode 100755 index 00000000..00709c9c --- /dev/null +++ b/calibrations/camera_fl_intrinsics.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 +CameraExtrinsicMat: !!opencv-matrix + cols: 4 + data: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 1.0] + dt: d + rows: 4 +CameraMat: !!opencv-matrix + cols: 3 + data: [4721.906852, 0.000000, 1203.133268, 0.000000, 4699.428088, 668.673413, 0.000000, 0.000000, 1.000000] + dt: d + rows: 3 +DistCoeff: !!opencv-matrix + cols: 1 + data: [0.017352, -2.130108, 0.008863, 0.014784, 0.000000] + dt: d + rows: 5 +ImageSize: [1920, 1440] +Reprojection Error: 0.0 +DistModel: plumb_bob diff --git a/cfg/AvtVimbaCamera.cfg b/cfg/AvtVimbaCamera.cfg index 5115a16c..b2399564 100755 --- a/cfg/AvtVimbaCamera.cfg +++ b/cfg/AvtVimbaCamera.cfg @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 PACKAGE='avt_vimba_camera' NODE='avt_vimba_camera' diff --git a/cfg/AvtVimbaCameraStereo.cfg b/cfg/AvtVimbaCameraStereo.cfg index 3be23b47..32a9c50d 100755 --- a/cfg/AvtVimbaCameraStereo.cfg +++ b/cfg/AvtVimbaCameraStereo.cfg @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 PACKAGE='avt_vimba_camera' NODE='avt_vimba_camera' diff --git a/docker/build-image.sh b/docker/build-image.sh new file mode 100755 index 00000000..5a95e8a5 --- /dev/null +++ b/docker/build-image.sh @@ -0,0 +1,174 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +USERNAME=usdotfhwastol + +cd "$(dirname "$0")" +IMAGE=$(basename `git rev-parse --show-toplevel`) + +echo "" +echo "##### $IMAGE Docker Image Build Script #####" +echo "" + +ROS1_PACKAGES="" +ROS2_PACKAGES="" +ROS1_PACKAGES_COLLECT=false +ROS2_PACKAGES_COLLECT=false + +while [[ $# -gt 0 ]]; do + arg="$1" + case $arg in + -v|--version) + ROS1_PACKAGES_COLLECT=false + ROS2_PACKAGES_COLLECT=false + + COMPONENT_VERSION_STRING="$2" + shift + shift + ;; + --system-release) + ROS1_PACKAGES_COLLECT=false + ROS2_PACKAGES_COLLECT=false + + SYSTEM_RELEASE=true + shift + ;; + -p|--push) + ROS1_PACKAGES_COLLECT=false + ROS2_PACKAGES_COLLECT=false + + PUSH=true + shift + ;; + -d|--develop) + ROS1_PACKAGES_COLLECT=false + ROS2_PACKAGES_COLLECT=false + + USERNAME=usdotfhwastoldev + COMPONENT_VERSION_STRING=develop + shift + ;; + --ros-1-packages|--ros1) + ROS1_PACKAGES_COLLECT=true + ROS2_PACKAGES_COLLECT=false + + shift + ;; + --ros-2-packages|--ros2) + ROS1_PACKAGES_COLLECT=false + ROS2_PACKAGES_COLLECT=true + + shift + ;; + *) + # Var test based on Stack Overflow question: https://stackoverflow.com/questions/5406858/difference-between-unset-and-empty-variables-in-bash + # Asker: green69 + # Answerer: geekosaur + if $ROS1_PACKAGES_COLLECT; then + ROS1_PACKAGES="$ROS1_PACKAGES $arg" + elif $ROS2_PACKAGES_COLLECT; then + ROS2_PACKAGES="$ROS2_PACKAGES $arg" + else + echo "Unknown argument $arg..." + exit -1 + fi + shift + ;; + esac +done + +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Performing incremental build of image to rebuild packages: ROS1>> $ROS1_PACKAGES ROS2>> $ROS2_PACKAGES..." + + echo "Updating Dockerfile references to use most recent image as base image" + # Trim of docker image LS command sourced from + # https://stackoverflow.com/questions/50625619/why-doesnt-the-cut-command-work-for-a-docker-image-ls-command + # Question Asker: Chris F + # Question Answerer: Arount + MOST_RECENT_IMAGE_DATA=$(docker image ls | grep $IMAGE | tr -s ' ') + + if [[ -z "$MOST_RECENT_IMAGE_DATA" ]]; then + echo No prior image exists to use as base, an initial image must be built first before attempting incremental build. + exit -1 + fi + + MOST_RECENT_IMAGE_HASH=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 3) + MOST_RECENT_IMAGE_ORG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 1 | cut -d "/" -f 1) + MOST_RECENT_IMAGE_TAG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 2) + MOST_RECENT_IMAGE_DATE=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 4,5,6) + + echo Using $MOST_RECENT_IMAGE_TAG $MOST_RECENT_IMAGE_HASH $MOST_RECENT_IMAGE_DATE as base for partial build... + + sed -i "s|^FROM[[:space:]]*[^[:space:]]*|FROM $MOST_RECENT_IMAGE_HASH|I" ../Dockerfile + + COMPONENT_VERSION_STRING="SNAPSHOT" + USERNAME="local" +fi + +if [[ -z "$COMPONENT_VERSION_STRING" ]]; then + COMPONENT_VERSION_STRING=$("./get-component-version.sh") +fi + +echo "Building docker image for $IMAGE version: $COMPONENT_VERSION_STRING" +echo "Final image name: $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING" + +cd .. +if [[ $COMPONENT_VERSION_STRING = "develop" ]]; then + sed "s|usdotfhwastoldev/|$USERNAME/|g; s|usdotfhwastolcandidate/|$USERNAME/|g; s|usdotfhwastol/|$USERNAME/|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:$COMPONENT_VERSION_STRING|g; s|checkout.bash|checkout.bash -d|g" \ + Dockerfile | docker build -f - --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ + --build-arg VERSION="$COMPONENT_VERSION_STRING" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . +elif [[ $COMPONENT_VERSION_STRING = "SNAPSHOT" ]]; then + docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ + --build-arg ROS1_PACKAGES="$ROS1_PACKAGES" \ + --build-arg ROS2_PACKAGES="$ROS2_PACKAGES" \ + --build-arg VERSION="$COMPONENT_VERSION_STRING" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . +else + #The addition of --network=host was a fix for a DNS resolution error that occured + #when running the platform inside an Ubuntu 20.04 virtual machine. The error and possible soliutions are + # discussed here: https://github.com/moby/moby/issues/41003 + docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ + --build-arg VERSION="$COMPONENT_VERSION_STRING" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . +fi + +TAGS=() +TAGS+=("$USERNAME/$IMAGE:$COMPONENT_VERSION_STRING") + +docker tag $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING $USERNAME/$IMAGE:latest +TAGS+=("$USERNAME/$IMAGE:latest") + +echo "Tagged $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING as $USERNAME/$IMAGE:latest" + +if [ "$SYSTEM_RELEASE" = true ]; then + SYSTEM_VERSION_STRING=$("./get-system-version.sh") + docker tag $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING $USERNAME/$IMAGE:$SYSTEM_VERSION_STRING + echo "Tagged $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING as $USERNAME/$IMAGE:$SYSTEM_VERSION_STRING" + TAGS+=("$USERNAME/$IMAGE:$SYSTEM_VERSION_STRING") +fi + +if [ "$PUSH" = true ]; then + for tag in $TAGS; do + docker push "${tag}" + done +fi + +echo "" +echo "##### $IMAGE Docker Image Build Done! #####" diff --git a/docker/checkout.bash b/docker/checkout.bash new file mode 100755 index 00000000..e4542b86 --- /dev/null +++ b/docker/checkout.bash @@ -0,0 +1,41 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +# CARMA packages checkout script +# Optional argument to set the root checkout directory with no ending '/' default is '~' + +set -exo pipefail + +dir=~ +BRANCH=develop # The script will use this unless the -b flag updates it +while [[ $# -gt 0 ]]; do + arg="$1" + case $arg in + -b|--branch) + BRANCH=$2 + shift + shift + ;; + -r|--root) + dir=$2 + shift + shift + ;; + esac +done +git clone https://github.com/usdot-fhwa-stol/carma-msgs.git "${dir}"/src/carma-msgs --branch "$BRANCH" --depth 1 +git clone https://github.com/usdot-fhwa-stol/carma-utils.git "${dir}"/src/carma-utils --branch "$BRANCH" --depth 1 + diff --git a/docker/get-component-version.sh b/docker/get-component-version.sh new file mode 100755 index 00000000..e5979fe1 --- /dev/null +++ b/docker/get-component-version.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cd "$(dirname "$0")" +cd .. +COMPONENT_TAG_PREFIX="${PWD##*/}" +git describe --all --match="$COMPONENT_TAG_PREFIX*" --always --dirty="-SNAPSHOT" | awk -F "/" '{print $NF}' | sed "s/$COMPONENT_TAG_PREFIX\_//" + diff --git a/docker/get-image-name.sh b/docker/get-image-name.sh new file mode 100755 index 00000000..3e873770 --- /dev/null +++ b/docker/get-image-name.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cd "$(dirname "$0")" +REPO_NAME="$(./get-repo-name.sh)" + +# This sed command based on Stack Overflow answer: https://stackoverflow.com/questions/28795479/awk-sed-script-to-convert-a-file-from-camelcase-to-underscores +# Asked by Corentin Peuvrel: https://stackoverflow.com/users/4608146/corentin-peuvrel +# Answered by pachopepe: https://stackoverflow.com/users/641896/pachopepe +# Credited in accordance with Stack Overflow's CC-BY license +echo $REPO_NAME | sed -r 's/CARMA/carma/' | sed -r 's/([A-Z])/-\L\1/g' | sed 's/^_//' + diff --git a/docker/get-package-name.sh b/docker/get-package-name.sh new file mode 100755 index 00000000..59a994e0 --- /dev/null +++ b/docker/get-package-name.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +# Copyright (C) 2019-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + + +cd "$(dirname "$0")" +cd .. +echo "${PWD##*/}" + diff --git a/docker/get-repo-name.sh b/docker/get-repo-name.sh new file mode 100755 index 00000000..a4ebe202 --- /dev/null +++ b/docker/get-repo-name.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +basename -s .git `git config --get remote.origin.url` diff --git a/docker/get-system-version.sh b/docker/get-system-version.sh new file mode 100755 index 00000000..e652c49c --- /dev/null +++ b/docker/get-system-version.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +SYSTEM_TAG_PREFIX="carma-system" + +git describe --all --match="$SYSTEM_TAG_PREFIX*" --always --dirty="-SNAPSHOT" | awk -F "/" '{print $NF}' + diff --git a/docker/get-tmp.sh b/docker/get-tmp.sh new file mode 100755 index 00000000..703b1fab --- /dev/null +++ b/docker/get-tmp.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Copyright (C) 2019-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cd "$(dirname "$0")" +cd .. + +DIR_NAME="${PWD##*/}" + +if [ "$DIR_NAME" = "CARMAPlatform" ]; then + echo "carma" +else + echo "${PWD##*/}" | sed -r 's/CARMA/carma/' | sed -r 's/([A-Z])/-\L\1/g' | sed 's/^_//' +fi diff --git a/docker/install.sh b/docker/install.sh new file mode 100755 index 00000000..5ed908e9 --- /dev/null +++ b/docker/install.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +if [[ ! -z "$ROS1_PACKAGES" ]]; then + echo "Sourcing previous build for incremental build start point..." + source /opt/carma/install/setup.bash +else + echo "Sourcing base image for full build..." + source /opt/ros/noetic/setup.bash +fi + +cd ~/ +if [[ ! -z "$ROS1_PACKAGES" ]]; then + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS1_PACKAGES +else + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release +fi diff --git a/docker/system_release.sh b/docker/system_release.sh new file mode 100755 index 00000000..edf1990a --- /dev/null +++ b/docker/system_release.sh @@ -0,0 +1,69 @@ +#!/bin/bash + +# Copyright (C) 2018-2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +# This script takes a system release name and version number as arguments, and +# updates version dependencies in Dockerfile and /docker/checkout.bash accordingly. + +# The -u | --unprompted option can be used to skip the interactive prompts, and +# provide arguments directly from the commandline. + +if [[ $# -eq 0 ]]; then + echo "Enter the system release name:" + read RELEASE_NAME + echo "Enter the system release version number:" + read RELEASE_VERSION +else + while [[ $# -gt 0 ]]; do + arg="$1" + case $arg in + -u|--unprompted) + RELEASE_NAME=$2 + RELEASE_VERSION=$3 + shift + shift + shift + ;; + esac +done +fi + +SYSTEM_RELEASE=carma-system-$RELEASE_VERSION +RELEASE_BRANCH=release/$RELEASE_NAME + +if git ls-remote -q | grep $RELEASE_BRANCH; then + echo "Checking out $RELEASE_BRANCH branch." + git checkout $RELEASE_BRANCH + + echo "Updating .circleci/config.yml base image." + sed -i "s|carma-base:.*|carma-base:$SYSTEM_RELEASE|g" .circleci/config.yml + + echo "Updating checkout.bash to point to system release version." + sed -i "s|CARMA[a-zA-Z]*_[0-9]*\.[0-9]*\.[0-9]*|$SYSTEM_RELEASE|g; s|carma-[a-zA-Z]*-[0-9]*\.[0-9]*\.[0-9]*|$SYSTEM_RELEASE|g" docker/checkout.bash + + echo "Updating Dockerfile to point to system release version." + sed -i "s|:CARMASystem_[0-9]*\.[0-9]*\.[0-9]*|:$SYSTEM_RELEASE|g; s|:carma-system-[0-9]*\.[0-9]*\.[0-9]*|:$SYSTEM_RELEASE|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:$SYSTEM_RELEASE|g" Dockerfile + + git add docker/checkout.bash Dockerfile + + git commit -m "Updated dependencies for $SYSTEM_RELEASE" + + git tag -a $SYSTEM_RELEASE -m "$SYSTEM_RELEASE version tag." + + echo "Dockerfile and checkout.bash updated, committed, and tagged." +else + echo "$RELEASE_BRANCH does not exist. Exiting script." + exit 0 +fi diff --git a/hooks/build b/hooks/build new file mode 100644 index 00000000..d851c7eb --- /dev/null +++ b/hooks/build @@ -0,0 +1,9 @@ +#!/bin/bash + +echo "Building $IMAGE_NAME" + +docker build --no-cache -t $IMAGE_NAME \ + --build-arg VERSION="$DOCKER_TAG" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . + \ No newline at end of file diff --git a/hooks/pre_build b/hooks/pre_build new file mode 100644 index 00000000..1b199017 --- /dev/null +++ b/hooks/pre_build @@ -0,0 +1,15 @@ +#!/bin/bash + +if [[ "$SOURCE_BRANCH" = "develop" ]]; then + # add -d flag to checkout.bash and update image dependencies + sed -i "s|checkout.bash|checkout.bash -d|g; s|usdotfhwastol/|usdotfhwastoldev/|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:develop|g s|:CARMA[a-zA-Z]*_[0-9]*\.[0-9]*\.[0-9]*|:develop|g; s|:carma-[a-zA-Z]*-[0-9]*\.[0-9]*\.[0-9]*|:develop|g" \ + Dockerfile +elif [[ "$SOURCE_BRANCH" =~ ^release/.*$ ]]; then + # swap checkout branch in checkout.bash to release branch + RELEASE_NAME=$(echo $SOURCE_BRANCH | cut -d "/" -f 2) + sed -i "s|usdotfhwastol/|usdotfhwastolcandidate/|g; s|usdotfhwastoldev/|usdotfhwastolcandidate/|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:$RELEASE_NAME|g; s|:CARMA[a-zA-Z]*_[0-9]*\.[0-9]*\.[0-9]*|:$RELEASE_NAME|g; s|:carma-[a-zA-Z]*-[0-9]*\.[0-9]*\.[0-9]*|:$RELEASE_NAME|g; s|:develop|:$RELEASE_NAME|g" \ + Dockerfile + # swap checkout branch in checkout.bash to release branch + sed -i "s|--branch .*|--branch $SOURCE_BRANCH|g" \ + docker/checkout.bash +fi diff --git a/include/avt_vimba_camera/mono_camera.h b/include/avt_vimba_camera/mono_camera.h index 97fb6109..8bf230eb 100644 --- a/include/avt_vimba_camera/mono_camera.h +++ b/include/avt_vimba_camera/mono_camera.h @@ -43,7 +43,8 @@ #include #include #include - +#include +#include #include namespace avt_vimba_camera { @@ -51,11 +52,13 @@ class MonoCamera { public: MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp); ~MonoCamera(void); + void updateCameraStatus(); + uint8_t cam_status; + private: - private: AvtVimbaApi api_; AvtVimbaCamera cam_; - + ros::Time last_time_; // diagnostic_updater::Updater updater_; // diagnostic_updater::TopicDiagnostic* pub_freq_; @@ -71,8 +74,6 @@ class MonoCamera { // ROS Camera publisher image_transport::CameraPublisher pub_; - - // sensor_msgs::CameraInfo left_info_; boost::shared_ptr info_man_; diff --git a/include/avt_vimba_camera/status_camera.h b/include/avt_vimba_camera/status_camera.h new file mode 100644 index 00000000..5e61c465 --- /dev/null +++ b/include/avt_vimba_camera/status_camera.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2019-2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include + +namespace avt_vimba_camera { +class StatusCamera +{ + ros::Publisher status_pub_ ; + boost::thread* cam_thread_; + +public: + uint8_t status_cam; + cav_msgs::DriverStatus status_; + //Destructor to interrupt the cam_thread + ~StatusCamera(); + void alertCallback(const cav_msgs::SystemAlertConstPtr &msg); + void publish_status(); + void publish_off_status(); + void pre_camera(ros::Publisher status_pub); + void post_camera(); +}; +} + diff --git a/launch/mobileye_eval.launch b/launch/mobileye_eval.launch new file mode 100644 index 00000000..e3e4c06d --- /dev/null +++ b/launch/mobileye_eval.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mono_camera.launch b/launch/mono_camera.launch index 529ab358..a2287b1a 100644 --- a/launch/mono_camera.launch +++ b/launch/mono_camera.launch @@ -1,74 +1,83 @@ + + + + + + + + + - - - - - - - - - - - + + + + + + + - - + + - - + + - - + + - - - - - - + + + + + + - - - - + + + + - - - - - - - - + + + + + + - - + + + + + + diff --git a/package.xml b/package.xml index d286fd91..6f2afe69 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ - + avt_vimba_camera - 0.0.10 + 1.0.0 Wrapper of the Allied Vision Technologies (AVT) VIMBA Ethernet and Firewire SDK. Miquel Massot @@ -14,32 +14,25 @@ Allied Vision Technologies catkin - camera_info_manager - diagnostic_updater - dynamic_reconfigure - image_geometry - image_transport - message_filters - roscpp - sensor_msgs - std_msgs - polled_camera - nodelet - - camera_info_manager - diagnostic_updater - dynamic_reconfigure - image_geometry - image_transport - message_filters - roscpp - sensor_msgs - std_msgs - polled_camera - nodelet - - image_proc - stereo_image_proc + camera_info_manager + diagnostic_updater + dynamic_reconfigure + image_geometry + image_transport + message_filters + roscpp + sensor_msgs + std_msgs + polled_camera + nodelet + cav_msgs + + + + image_proc + stereo_image_proc + + carma_cmake_common diff --git a/src/mono_camera.cpp b/src/mono_camera.cpp index 73d1dc2f..11a02212 100644 --- a/src/mono_camera.cpp +++ b/src/mono_camera.cpp @@ -31,7 +31,6 @@ /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - #define DEBUG_PRINTS 1 namespace avt_vimba_camera { @@ -39,7 +38,6 @@ namespace avt_vimba_camera { MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) { // Prepare node handle for the camera // TODO use nodelets with getMTNodeHandle() - // Start Vimba & list all available cameras api_.start(); @@ -47,6 +45,7 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp pub_ = it_.advertiseCamera("image_raw", 1); // Set the frame callback + cam_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::frameCallback, this, _1)); // Set the params @@ -67,9 +66,11 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp MonoCamera::~MonoCamera(void) { cam_.stop(); pub_.shutdown(); -} + } void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr) { + cam_status=cav_msgs::DriverStatus::OPERATIONAL; + last_time_=ros::Time::now(); ros::Time ros_time = ros::Time::now(); if (pub_.getNumSubscribers() > 0) { sensor_msgs::Image img; @@ -78,8 +79,10 @@ void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr) { ci.header.stamp = img.header.stamp = ros_time; img.header.frame_id = ci.header.frame_id; pub_.publish(img, ci); + } else { ROS_WARN_STREAM("Function frameToImage returned 0. No image published."); + cam_status=cav_msgs::DriverStatus::FAULT; } } // updater_.update(); @@ -162,4 +165,18 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig& info_man_->setCameraInfo(ci); } +//time_compare compares the system time with time stamp and if its greater than 3 sec it assigns OFF status +void MonoCamera::updateCameraStatus() +{ + ros::Duration img_time_difference=ros::Time::now()-last_time_; + //Greater than 2 seconds of non-response is the minimum time required to classify as not connected + if ((img_time_difference>ros::Duration(2.0)) && cam_status==cav_msgs::DriverStatus::OPERATIONAL) + { + cam_status=cav_msgs::DriverStatus::OFF; + } +} }; + + + + diff --git a/src/nodes/mono_camera_node.cpp b/src/nodes/mono_camera_node.cpp index 0292da92..c3803f62 100644 --- a/src/nodes/mono_camera_node.cpp +++ b/src/nodes/mono_camera_node.cpp @@ -1,15 +1,30 @@ #include #include +#include int main(int argc, char** argv) { - ros::init(argc, argv, "mono_camera_node"); - + ros::init(argc, argv, "mono_camera_node"); ros::NodeHandle nh; ros::NodeHandle nhp("~"); - + ros::Rate loop_rate(19); //Camera max frame rate is 19Hz + avt_vimba_camera::StatusCamera hc; + ros::Publisher status_pub = nh.advertise("driver_discovery", 1); + ros::Subscriber alert_sub = nh.subscribe("system_alert",10,&avt_vimba_camera::StatusCamera::alertCallback, &hc); + hc.pre_camera(status_pub); avt_vimba_camera::MonoCamera mc(nh,nhp); + hc.post_camera(); - ros::spin(); +while(ros::ok()) +{ +mc.updateCameraStatus(); +ros::spinOnce(); +loop_rate.sleep(); +hc.status_cam=mc.cam_status; +hc.publish_status(); +status_pub.publish(hc.status_); +} return 0; -} \ No newline at end of file +} + + diff --git a/src/status_camera.cpp b/src/status_camera.cpp new file mode 100644 index 00000000..c5620985 --- /dev/null +++ b/src/status_camera.cpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2019-2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include + +namespace avt_vimba_camera { + +//Destructor to interrupt the cam_thread +StatusCamera::~StatusCamera() +{ + cam_thread_->interrupt(); +} + +//System alert function definition +void StatusCamera::alertCallback(const cav_msgs::SystemAlertConstPtr &msg) +{ + if( msg->type==cav_msgs::SystemAlert::FATAL || msg->type==cav_msgs::SystemAlert::SHUTDOWN) + { + ros::shutdown(); + } +} + +void StatusCamera::publish_status() +{ //Various driver status conditions + boost::this_thread::interruption_point(); + if (status_cam==cav_msgs::DriverStatus::OFF) + { + status_.status=cav_msgs::DriverStatus::OFF; + } + else if (status_cam==cav_msgs::DriverStatus::OPERATIONAL) + { + status_.status=cav_msgs::DriverStatus::OPERATIONAL; + } + else if (status_cam==cav_msgs::DriverStatus::FAULT) + { + status_.status=cav_msgs::DriverStatus::FAULT; + } +} + +void StatusCamera::publish_off_status() +{ + while (true) { + boost::this_thread::interruption_point(); + status_.status = cav_msgs::DriverStatus::OFF; + status_pub_.publish(status_); + ros::Duration(0.1).sleep(); + } +} + +//Start the thread and subscriber declaration +void StatusCamera::pre_camera(ros::Publisher status_pub) +{ + status_.name=ros::this_node::getName(); + status_.camera=true; + status_pub_ = status_pub; + cam_thread_ = new boost::thread(boost::bind(&StatusCamera::publish_off_status,this)); +} +//Interrupt the cam_thread +void StatusCamera::post_camera() +{ + cam_thread_->interrupt(); +} +} diff --git a/test/status_mako_camera_test.cpp b/test/status_mako_camera_test.cpp new file mode 100644 index 00000000..fd7fb7b4 --- /dev/null +++ b/test/status_mako_camera_test.cpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2019-2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include + +TEST(MakoCameraTest, DriverStatus) +{ + avt_vimba_camera::StatusCamera hc; + hc.status_cam = cav_msgs::DriverStatus::OFF; + hc.publish_status(); + EXPECT_EQ(cav_msgs::DriverStatus::OFF, hc.status_.status); + hc.status_cam = cav_msgs::DriverStatus::OPERATIONAL; + hc.publish_status(); + EXPECT_EQ(cav_msgs::DriverStatus::OPERATIONAL, hc.status_.status); + hc.status_cam = cav_msgs::DriverStatus::FAULT; + hc.publish_status(); + EXPECT_EQ(cav_msgs::DriverStatus::FAULT, hc.status_.status); +} + +// Run all the tests +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}