Handle missing or disconnected camera

.docker/Dockerfile

@@ -1,4 +1,5 @@
 ARG ROS_VERSION=noetic
+ARG BUILD_TYPE=git
 
 #====================================================================
 # Preliminary image with dependencies
@@ -70,14 +71,14 @@ RUN groupadd --gid $USER_GID $USERNAME \
 USER $USERNAME
 
 # Install the default ROS entrypoint
-COPY --chmod=0755 ros_entrypoint.sh /home/$USERNAME/ros_entrypoint.sh
+COPY --chmod=0755 .docker/ros_entrypoint.sh /home/$USERNAME/ros_entrypoint.sh
 ENTRYPOINT [ "/home/ros/ros_entrypoint.sh" ]
 CMD ["/bin/bash"]
 
 #====================================================================
-# Intermediate image with cloned Git repos
+# Load cloned Git repos into image
 #====================================================================
-FROM ci as clone
+FROM ci as clone_git
 
 ARG WS_DIR=/home/$USERNAME/ros_ws
 ONBUILD WORKDIR ${WS_DIR}/src
@@ -90,16 +91,33 @@ ONBUILD RUN echo "Cloning from ${ARENA_CAMERA_ROS_BRANCH} branch ${ARENA_CAMERA_
 ONBUILD ADD https://api.github.com/repos/apl-ocean-engineering/arena_camera_ros/git/refs/heads/${ARENA_CAMERA_ROS_BRANCH} /tmp/version.json
 ONBUILD RUN git clone --depth 1 -b ${ARENA_CAMERA_ROS_BRANCH} ${ARENA_CAMERA_ROS_REPO}
 
+ONBUILD RUN vcs import --shallow --skip-existing < arena_camera_ros/arena_camera_ros.repos
+
+
 #====================================================================
-# Bring in ROS dependencies but don't buil
+# Load copy of local source into image
+#====================================================================
+FROM ci as clone_local
+
+ARG WS_DIR=/home/$USERNAME/ros_ws
+ONBUILD WORKDIR ${WS_DIR}/src
+
+# Manually install dependencies to prevent hitting the network if only the
+# local source has changed
+ONBUILD RUN git clone --depth 1 -b ros1 https://github.com/apl-ocean-engineering/imaging_msgs.git
+
+ONBUILD RUN echo "Adding local copy of arena_camera_ros to Docker image"
+ONBUILD ADD arena_camera ${WS_DIR}/src/arena_camera_ros/arena_camera
+ONBUILD ADD camera_control_msgs ${WS_DIR}/src/arena_camera_ros/camera_control_msgs
+ONBUILD ADD arena_camera_ros.repos ${WS_DIR}/src/arena_camera_ros/arena_camera_ros.repos
+
+#====================================================================
+# Bring in ROS dependencies but don't build
 #====================================================================
 #
-FROM clone as prebuild
+FROM clone_${BUILD_TYPE} as prebuild
 
 ARG WS_DIR
-WORKDIR ${WS_DIR}/src
-
-RUN vcs import --shallow --skip-existing < arena_camera_ros/arena_camera_ros.repos
 
 WORKDIR ${WS_DIR}
 

.docker/docker-compose.yml

@@ -3,23 +3,34 @@
 services:
   arena_camera:
     build:
-      dockerfile: Dockerfile
-    image: ghcr.io/apl-ocean-engineering/arena_camera_ros:latest
+      context: ..
+      dockerfile: .docker/Dockerfile
+    image: ghcr.io/apl-ocean-engineering/arena_camera_ros:${IMAGE_TAG:-latest}
     stdin_open: true
     tty: true
     network_mode: host
 
-  # Run with the local checkout bind-mounted into the workspace
-  build_local:
+  # Prebuild includes rosdep and vcs import
+  # (basically, everything before "catkin build")
+  prebuild:
+    extends: arena_camera
+    # Local-only image
+    image: arena_camera_ros:prebuild
     build:
-      dockerfile: Dockerfile
       target: prebuild
-    image: ghcr.io/apl-ocean-engineering/arena_camera_ros/build:latest
-    stdin_open: true
-    tty: true
-    network_mode: host
     command: /bin/bash
+
+  # Prebuild with _this_ copy of the repo mounted
+  #
+  mount_local:
+    extends: prebuild
     volumes:
       - type: bind
         source: ..
         target: /home/ros/ros_ws/src/arena_camera_ros
+
+  build_local:
+    extends: arena_camera
+    build:
+      args:
+        BUILD_TYPE: local

arena_camera/cfg/ArenaCamera.cfg

@@ -19,6 +19,6 @@ gen.add("exposure_ms",       double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposu
 gen.add("auto_gain",         bool_t,   SensorLevels.RECONFIGURE_RUNNING, "Use camera auto-gain.", True)
 gen.add("gain",              double_t, SensorLevels.RECONFIGURE_RUNNING, "Camera gain as a pct [0.0, 1.0]", 0.5, 0.0, 1.0)
 
-gen.add("gamma",             double_t, SensorLevels.RECONFIGURE_RUNNING, "Camera gamma [0.0, 5.0]", 2.2, 0.0, 5.0)
+gen.add("gamma",             double_t, SensorLevels.RECONFIGURE_STOP, "Camera gamma [0.0, 5.0]", 2.2, 0.0, 5.0)
 
 exit(gen.generate(PACKAGE, "arena_camera", "ArenaCamera"))

arena_camera/include/arena_camera/arena_camera_nodelet.h

@@ -99,6 +99,8 @@ class ArenaCameraNodeletBase : public nodelet::Nodelet {
   void stopStreaming();
 
  protected:
+  bool tryConnect();
+
   /**
    * Creates the camera instance either by UserDeviceId, SerialNumber,
    * or taking the first auto-detected camera.
@@ -116,7 +118,8 @@ class ArenaCameraNodeletBase : public nodelet::Nodelet {
 
   /// Virtual callback for node initialization _after_ Node::onInit()
   /// Only called if that initialization/configuration is successful.
-  virtual void onSuccessfulInit(){};
+  virtual void onCameraConnect(){};
+  virtual void onCameraDisconnect(){};
 
   // === Functions to set/get ImageEncoding ===
 
@@ -248,13 +251,11 @@ class ArenaCameraNodeletBase : public nodelet::Nodelet {
 
   ArenaCameraParameter arena_camera_parameter_set_;
 
-  std::unique_ptr<image_transport::ImageTransport> it_;
+  std::unique_ptr<image_transport::ImageTransport> image_transport_;
   image_transport::CameraPublisher img_raw_pub_;
 
   std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
 
-  std::vector<std::size_t> sampling_indices_;
-
   boost::recursive_mutex device_mutex_;
 
   // Internal cache for exposure and gain, updated by callback
@@ -290,7 +291,7 @@ class ArenaCameraStreamingNodelet : public ArenaCameraNodeletBase {
   ArenaCameraStreamingNodelet();
   virtual ~ArenaCameraStreamingNodelet();
 
-  void onSuccessfulInit() override;
+  void onCameraConnect() override;
 
  protected:
   typedef std::function<void(Arena::IImage *pImage)> ImageCallback_t;
@@ -320,7 +321,7 @@ class ArenaCameraPolledNodelet : public ArenaCameraNodeletBase {
   ArenaCameraPolledNodelet();
   virtual ~ArenaCameraPolledNodelet();
 
-  void onSuccessfulInit() override;
+  void onCameraConnect() override;
 
   /// Callback for the grab images action
   /// @param goal the goal

arena_camera/launch/arena_camera_nodelet.launch

@@ -14,6 +14,9 @@
     <arg name="run_web_server" default="false" />
     <arg name="web_server_port" default="8080" />
 
+    <arg name="serial_number" type="string" default="" />
+    <arg name="retry" default="true" doc="If true, driver will retry camera connection;  if false, driver will fail if it can't find camera"/>
+
     <arg unless="$(arg debug)" name="launch_prefix" value="" />
     <arg     if="$(arg debug)" name="launch_prefix" value="gdb -ex run --args" />
 
@@ -30,6 +33,9 @@
     <!-- The main camera driver nodelet -->
     <node name="$(arg node_name)" pkg="nodelet" type="nodelet" output="screen"
             args="load arena_camera/ArenaCameraStreamingNodelet $(arg manager)" >
+        <param name="serial_number" type="string" value="$(arg serial_number)" />
+        <param name="retry" value="$(arg retry)"/>
+
         <rosparam command="load" file="$(arg config_file)" />
     </node>
 

arena_camera/launch/default_params.yaml

@@ -4,6 +4,12 @@
 #  The tf frame under which the images were published
 camera_frame: arena_camera
 
+# Period for connection health check (in seconds)
+check_period_sec: 1.0
+
+# Period to retry connecting to camera, if connection fails (in seconds)
+retry_period_sec: 5.0
+
 #  The DeviceUserID of the camera. If empty, the first camera found in the
 #  device list will be used.  If specified, device_user_id takes precedence
 #  over serial_number.   If neither is specified, the system users the
@@ -13,7 +19,8 @@ camera_frame: arena_camera
 
 #  Serial number of camera to look for.
 #
-#serial_number: "12345"
+# If left empty, driver will user device_user_id or autodetect a camera
+#serial_number: "234200113"
 
 #  The CameraInfo URL (Uniform Resource Locator) where the optional intrinsic
 #  camera calibration parameters are stored. This URL string will be parsed