Program Listing for File stereo_view_node.hpp

↰ Return to documentation for file (include/image_view/stereo_view_node.hpp)

/*********************************************************************
* 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
*  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.
*********************************************************************/

// Copyright 2019, Joshua Whitley
//
// 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.

#ifndef IMAGE_VIEW__STEREO_VIEW_NODE_HPP_
#define IMAGE_VIEW__STEREO_VIEW_NODE_HPP_

#include <memory>
#include <mutex>
#include <string>

#include "message_filters/subscriber.h"
#include "message_filters/sync_policies/approximate_time.h"
#include "message_filters/sync_policies/exact_time.h"

#include <opencv2/highgui/highgui.hpp>

#include <rclcpp/rclcpp.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <stereo_msgs/msg/disparity_image.hpp>

namespace image_view
{

using sensor_msgs::msg::Image;
using stereo_msgs::msg::DisparityImage;
using message_filters::sync_policies::ExactTime;
using message_filters::sync_policies::ApproximateTime;

class StereoViewNode
  : public rclcpp::Node
{
public:
  explicit StereoViewNode(const rclcpp::NodeOptions & options);
  explicit StereoViewNode(const StereoViewNode &) = default;
  explicit StereoViewNode(StereoViewNode &&) = default;
  StereoViewNode & operator=(const StereoViewNode &) = default;
  StereoViewNode & operator=(StereoViewNode &&) = default;
  ~StereoViewNode();

private:
  using ExactPolicy = ExactTime<Image, Image, DisparityImage>;
  using ApproximatePolicy = ApproximateTime<Image, Image, DisparityImage>;
  using ExactSync = message_filters::Synchronizer<ExactPolicy>;
  using ApproximateSync = message_filters::Synchronizer<ApproximatePolicy>;

  image_transport::SubscriberFilter left_sub_, right_sub_;
  message_filters::Subscriber<DisparityImage> disparity_sub_;
  std::shared_ptr<ExactSync> exact_sync_;
  std::shared_ptr<ApproximateSync> approximate_sync_;
  int queue_size_;

  Image::ConstSharedPtr last_left_msg_, last_right_msg_;
  cv::Mat last_left_image_, last_right_image_;
  cv::Mat_<cv::Vec3b> disparity_color_;
  std::mutex image_mutex_;

  std::string filename_format_;
  int save_count_;

  rclcpp::TimerBase::SharedPtr check_synced_timer_;
  int left_received_, right_received_, disp_received_, all_received_;

  // colormap for disparities, RGB
  static constexpr unsigned char colormap[768] =
  {
    150, 150, 150, 107, 0, 12, 106, 0, 18, 105, 0, 24, 103, 0, 30,
    102, 0, 36, 101, 0, 42, 99, 0, 48, 98, 0, 54, 97, 0, 60,
    96, 0, 66, 94, 0, 72, 93, 0, 78, 92, 0, 84, 91, 0, 90,
    89, 0, 96, 88, 0, 102, 87, 0, 108, 85, 0, 114, 84, 0, 120,
    83, 0, 126, 82, 0, 131, 80, 0, 137, 79, 0, 143, 78, 0, 149,
    77, 0, 155, 75, 0, 161, 74, 0, 167, 73, 0, 173, 71, 0, 179,
    70, 0, 185, 69, 0, 191, 68, 0, 197, 66, 0, 203, 65, 0, 209,
    64, 0, 215, 62, 0, 221, 61, 0, 227, 60, 0, 233, 59, 0, 239,
    57, 0, 245, 56, 0, 251, 55, 0, 255, 54, 0, 255, 52, 0, 255,
    51, 0, 255, 50, 0, 255, 48, 0, 255, 47, 0, 255, 46, 0, 255,
    45, 0, 255, 43, 0, 255, 42, 0, 255, 41, 0, 255, 40, 0, 255,
    38, 0, 255, 37, 0, 255, 36, 0, 255, 34, 0, 255, 33, 0, 255,
    32, 0, 255, 31, 0, 255, 29, 0, 255, 28, 0, 255, 27, 0, 255,
    26, 0, 255, 24, 0, 255, 23, 0, 255, 22, 0, 255, 20, 0, 255,
    19, 0, 255, 18, 0, 255, 17, 0, 255, 15, 0, 255, 14, 0, 255,
    13, 0, 255, 11, 0, 255, 10, 0, 255, 9, 0, 255, 8, 0, 255,
    6, 0, 255, 5, 0, 255, 4, 0, 255, 3, 0, 255, 1, 0, 255,
    0, 4, 255, 0, 10, 255, 0, 16, 255, 0, 22, 255, 0, 28, 255,
    0, 34, 255, 0, 40, 255, 0, 46, 255, 0, 52, 255, 0, 58, 255,
    0, 64, 255, 0, 70, 255, 0, 76, 255, 0, 82, 255, 0, 88, 255,
    0, 94, 255, 0, 100, 255, 0, 106, 255, 0, 112, 255, 0, 118, 255,
    0, 124, 255, 0, 129, 255, 0, 135, 255, 0, 141, 255, 0, 147, 255,
    0, 153, 255, 0, 159, 255, 0, 165, 255, 0, 171, 255, 0, 177, 255,
    0, 183, 255, 0, 189, 255, 0, 195, 255, 0, 201, 255, 0, 207, 255,
    0, 213, 255, 0, 219, 255, 0, 225, 255, 0, 231, 255, 0, 237, 255,
    0, 243, 255, 0, 249, 255, 0, 255, 255, 0, 255, 249, 0, 255, 243,
    0, 255, 237, 0, 255, 231, 0, 255, 225, 0, 255, 219, 0, 255, 213,
    0, 255, 207, 0, 255, 201, 0, 255, 195, 0, 255, 189, 0, 255, 183,
    0, 255, 177, 0, 255, 171, 0, 255, 165, 0, 255, 159, 0, 255, 153,
    0, 255, 147, 0, 255, 141, 0, 255, 135, 0, 255, 129, 0, 255, 124,
    0, 255, 118, 0, 255, 112, 0, 255, 106, 0, 255, 100, 0, 255, 94,
    0, 255, 88, 0, 255, 82, 0, 255, 76, 0, 255, 70, 0, 255, 64,
    0, 255, 58, 0, 255, 52, 0, 255, 46, 0, 255, 40, 0, 255, 34,
    0, 255, 28, 0, 255, 22, 0, 255, 16, 0, 255, 10, 0, 255, 4,
    2, 255, 0, 8, 255, 0, 14, 255, 0, 20, 255, 0, 26, 255, 0,
    32, 255, 0, 38, 255, 0, 44, 255, 0, 50, 255, 0, 56, 255, 0,
    62, 255, 0, 68, 255, 0, 74, 255, 0, 80, 255, 0, 86, 255, 0,
    92, 255, 0, 98, 255, 0, 104, 255, 0, 110, 255, 0, 116, 255, 0,
    122, 255, 0, 128, 255, 0, 133, 255, 0, 139, 255, 0, 145, 255, 0,
    151, 255, 0, 157, 255, 0, 163, 255, 0, 169, 255, 0, 175, 255, 0,
    181, 255, 0, 187, 255, 0, 193, 255, 0, 199, 255, 0, 205, 255, 0,
    211, 255, 0, 217, 255, 0, 223, 255, 0, 229, 255, 0, 235, 255, 0,
    241, 255, 0, 247, 255, 0, 253, 255, 0, 255, 251, 0, 255, 245, 0,
    255, 239, 0, 255, 233, 0, 255, 227, 0, 255, 221, 0, 255, 215, 0,
    255, 209, 0, 255, 203, 0, 255, 197, 0, 255, 191, 0, 255, 185, 0,
    255, 179, 0, 255, 173, 0, 255, 167, 0, 255, 161, 0, 255, 155, 0,
    255, 149, 0, 255, 143, 0, 255, 137, 0, 255, 131, 0, 255, 126, 0,
    255, 120, 0, 255, 114, 0, 255, 108, 0, 255, 102, 0, 255, 96, 0,
    255, 90, 0, 255, 84, 0, 255, 78, 0, 255, 72, 0, 255, 66, 0,
    255, 60, 0, 255, 54, 0, 255, 48, 0, 255, 42, 0, 255, 36, 0,
    255, 30, 0, 255, 24, 0, 255, 18, 0, 255, 12, 0, 255, 6, 0,
    255, 0, 0,
  };

  void imageCb(
    const Image::ConstSharedPtr & left, const Image::ConstSharedPtr & right,
    const DisparityImage::ConstSharedPtr & disparity_msg);
  void saveImage(const char * prefix, const cv::Mat & image);
  static void mouseCb(int event, int x, int y, int flags, void * param);
  void checkInputsSynchronized();

  static inline void increment(int * value)
  {
    ++(*value);
  }
};

}  // namespace image_view

#endif  // IMAGE_VIEW__STEREO_VIEW_NODE_HPP_