stcamera_interface.h

Go to the documentation of this file.

/******************************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright (C) 2018, OMRON SENTECH. 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 OMRON SENTECH 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.
 *****************************************************************************/

#ifndef STCAMERA_STCAMERA_INTERFACE_H
#define STCAMERA_STCAMERA_INTERFACE_H

#include <mutex>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>

#include <omronsentech_camera/Event.h>
#include <omronsentech_camera/Chunk.h>

#include <omronsentech_camera/ReadNode.h>
#include <omronsentech_camera/ReadNodeBool.h>
#include <omronsentech_camera/ReadNodeEnum.h>
#include <omronsentech_camera/ReadNodeInt.h>
#include <omronsentech_camera/ReadNodeFloat.h>
#include <omronsentech_camera/ReadNodeString.h>

#include <omronsentech_camera/WriteNode.h>
#include <omronsentech_camera/WriteNodeBool.h>
#include <omronsentech_camera/WriteNodeEnumInt.h>
#include <omronsentech_camera/WriteNodeEnumStr.h>
#include <omronsentech_camera/WriteNodeInt.h>
#include <omronsentech_camera/WriteNodeFloat.h>
#include <omronsentech_camera/WriteNodeString.h>
#include <omronsentech_camera/ExecuteNode.h>

#include <omronsentech_camera/EnableChunk.h>
#include <omronsentech_camera/EnableTrigger.h>
#include <omronsentech_camera/EnableEventNode.h>
#include <omronsentech_camera/EnableImageAcquisition.h>
#include <omronsentech_camera/EnableEventAcquisition.h>

#include <omronsentech_camera/GetImageAcquisitionStatus.h>
#include <omronsentech_camera/GetEventAcquisitionStatusList.h>
#include <omronsentech_camera/GetEventNodeStatusList.h>
#include <omronsentech_camera/GetChunkList.h>
#include <omronsentech_camera/GetTriggerList.h>
#include <omronsentech_camera/GetEnumList.h>
#include <omronsentech_camera/GetGenICamNodeInfo.h>
#include <omronsentech_camera/SendSoftTrigger.h>
#include <omronsentech_camera/GetLastError.h>

#include <StApi_TL.h>
#include "omronsentech_camera/stparameter.h"

#include "omronsentech_camera/stheader.h"

namespace stcamera
{

#define RETURN_ERR(X, MSG) \
  ROS_ERROR("%s %s %d: \n\t%s error: %s", \
      __FILE__,__func__,__LINE__,camera_namespace_.c_str(),MSG); \
  last_error_ = X; return false;

#define CHECK_NULLPTR(P, X, MSG) if (nullptr == P) \
  { RETURN_ERR(X, MSG); } 

#define CATCH_COMMON_ERR() catch(const StApi::CStGenTLErrorException &x) \
  {\
    RETURN_ERR(x.GetError(), x.GetDescription());\
  }\
  catch(GenICam::GenericException &x)\
  {\
    RETURN_ERR(GENICAM_ERROR, x.GetDescription());\
  }\
  catch(...)\
  {\
  }\
  RETURN_ERR(UNKNOWN_ERROR, UNKNOWN_ERROR_STR);

  struct StCallback
  {
    std::string topic_name_;
    StApi::IStRegisteredCallbackReleasable *cb_;
  };

  class StCameraInterface;

  typedef std::map<std::string, StCameraInterface*> MapCameraInterface; 

  typedef std::map<std::string, struct StCallback> MapCallback;

  typedef std::map<std::string, GenApi::INode*> MapChunk;

  typedef std::map<std::string, ros::Publisher> MapPublisher;

  class StCameraInterface
  {
    public:

      StCameraInterface(StApi::IStDeviceReleasable *dev,
                        ros::NodeHandle nh_parent, 
                        const std::string &camera_namespace, 
                        StParameter *param,
                        uint32_t queue_size = STCAMERA_QUEUE_SIZE);

      virtual ~StCameraInterface();

      bool deviceIsLost();

    protected:

      void eventSystemCB(StApi::IStCallbackParamBase *p, void *pvContext);

      void eventInterfaceCB(StApi::IStCallbackParamBase *p, void *pvContext);

      void eventDeviceCB(StApi::IStCallbackParamBase *p, void *pvContext);

      void eventGenApiNodeCB(GenApi::INode *p, void *pvContext);

      void eventDataStreamCB(StApi::IStCallbackParamBase *p, void *pvContext);

      bool readNodeCallback(omronsentech_camera::ReadNode::Request &req,
          omronsentech_camera::ReadNode::Response &res);

      bool readNodeBoolCallback(omronsentech_camera::ReadNodeBool::Request &req,
          omronsentech_camera::ReadNodeBool::Response &res);

      bool readNodeEnumCallback(omronsentech_camera::ReadNodeEnum::Request &req,
          omronsentech_camera::ReadNodeEnum::Response &res);

      bool readNodeIntCallback(omronsentech_camera::ReadNodeInt::Request &req,
          omronsentech_camera::ReadNodeInt::Response &res);

      bool readNodeFloatCallback(
          omronsentech_camera::ReadNodeFloat::Request &req,
          omronsentech_camera::ReadNodeFloat::Response &res);

      bool readNodeStringCallback(
          omronsentech_camera::ReadNodeString::Request &req,
          omronsentech_camera::ReadNodeString::Response &res);

      bool writeNodeCallback(
          omronsentech_camera::WriteNode::Request &req,
          omronsentech_camera::WriteNode::Response &res);

      bool writeNodeBoolCallback(
          omronsentech_camera::WriteNodeBool::Request &req,
          omronsentech_camera::WriteNodeBool::Response &res);

      bool writeNodeEnumIntCallback(
          omronsentech_camera::WriteNodeEnumInt::Request &req,
          omronsentech_camera::WriteNodeEnumInt::Response &res);

      bool writeNodeEnumStrCallback(
          omronsentech_camera::WriteNodeEnumStr::Request &req,
          omronsentech_camera::WriteNodeEnumStr::Response &res);

      bool writeNodeIntCallback(
          omronsentech_camera::WriteNodeInt::Request &req,
          omronsentech_camera::WriteNodeInt::Response &res);

      bool writeNodeFloatCallback(
          omronsentech_camera::WriteNodeFloat::Request &req,
          omronsentech_camera::WriteNodeFloat::Response &res);

      bool writeNodeStringCallback(
          omronsentech_camera::WriteNodeString::Request &req,
          omronsentech_camera::WriteNodeString::Response &res);

      bool executeNodeCallback(
          omronsentech_camera::ExecuteNode::Request &req,
          omronsentech_camera::ExecuteNode::Response &res);

      bool enableChunkCallback(
          omronsentech_camera::EnableChunk::Request &req,
          omronsentech_camera::EnableChunk::Response &res);

      bool enableTriggerCallback( 
          omronsentech_camera::EnableTrigger::Request &req,
          omronsentech_camera::EnableTrigger::Response &res);

      bool enableEventNodeCallback( 
          omronsentech_camera::EnableEventNode::Request &req,
          omronsentech_camera::EnableEventNode::Response &res);

      bool enableImageAcquisitionCallback( 
          omronsentech_camera::EnableImageAcquisition::Request &req,
          omronsentech_camera::EnableImageAcquisition::Response &res);

      bool enableEventAcquisitionCallback( 
          omronsentech_camera::EnableEventAcquisition::Request &req,
          omronsentech_camera::EnableEventAcquisition::Response &res);

      bool getImageAcquisitionStatusCallback( 
          omronsentech_camera::GetImageAcquisitionStatus::Request &req,
          omronsentech_camera::GetImageAcquisitionStatus::Response &res);

      bool getEventAcquisitionStatusListCallback( 
          omronsentech_camera::GetEventAcquisitionStatusList::Request &req,
          omronsentech_camera::GetEventAcquisitionStatusList::Response &res);

      bool getEventNodeStatusListCallback( 
          omronsentech_camera::GetEventNodeStatusList::Request &req,
          omronsentech_camera::GetEventNodeStatusList::Response &res);

      bool getChunkListCallback(omronsentech_camera::GetChunkList::Request &req,
          omronsentech_camera::GetChunkList::Response &res);

      bool getTriggerListCallback(
          omronsentech_camera::GetTriggerList::Request &req,
          omronsentech_camera::GetTriggerList::Response &res);

      bool getEnumListCallback(omronsentech_camera::GetEnumList::Request &req,
          omronsentech_camera::GetEnumList::Response &res);

      bool getGenICamNodeInfoCallback(
          omronsentech_camera::GetGenICamNodeInfo::Request &req,
          omronsentech_camera::GetGenICamNodeInfo::Response &res);

      bool sendSoftTriggerCallback(
          omronsentech_camera::SendSoftTrigger::Request &req,
          omronsentech_camera::SendSoftTrigger::Response &res);

      bool getLastErrorCallback(
          omronsentech_camera::GetLastError::Request &req,
          omronsentech_camera::GetLastError::Response &res);

      void initializeCameraInfo();

      GenApi::INodeMap *getNodeMap(std::string &module_name);

      MapCallback *getCallbackMap(std::string &module_name);

      void publishEventDefault(omronsentech_camera::Event &msg);

      std::string camera_namespace_;

      StApi::CIStDevicePtr tl_dev_;

      StApi::CIStDataStreamPtr tl_ds_;

      ros::NodeHandle nh_; 

      StParameter *param_;

      image_transport::ImageTransport it_; 

      camera_info_manager::CameraInfoManager cinfo_;

      image_transport::CameraPublisher it_campub_;

      ros::Publisher msg_chunk_;

      MapPublisher  map_msg_event_;

      uint32_t queue_size_;
  
      ros::ServiceServer  srv_read_node_;
      ros::ServiceServer  srv_read_node_bool_;
      ros::ServiceServer  srv_read_node_enum_;
      ros::ServiceServer  srv_read_node_int_;
      ros::ServiceServer  srv_read_node_float_;
      ros::ServiceServer  srv_read_node_string_;

      ros::ServiceServer  srv_write_node_;
      ros::ServiceServer  srv_write_node_bool_;
      ros::ServiceServer  srv_write_node_enum_int_;
      ros::ServiceServer  srv_write_node_enum_str_;
      ros::ServiceServer  srv_write_node_int_;
      ros::ServiceServer  srv_write_node_float_;
      ros::ServiceServer  srv_write_node_string_;
      ros::ServiceServer  srv_execute_node_;
      
      ros::ServiceServer  srv_enable_chunk_;    
      ros::ServiceServer  srv_enable_trigger_;  
      ros::ServiceServer  srv_enable_event_node_;
      ros::ServiceServer  srv_enable_image_acquisition_;
      ros::ServiceServer  srv_enable_event_acquisition_;
      
      ros::ServiceServer  srv_get_image_acquisition_status_;
      ros::ServiceServer  srv_get_event_acquisition_status_list_;
      ros::ServiceServer  srv_get_event_node_status_list_;
      ros::ServiceServer  srv_get_chunk_list_;
      ros::ServiceServer  srv_get_trigger_list_;
      ros::ServiceServer  srv_get_enum_list_;
      ros::ServiceServer  srv_get_genicam_node_info_;

      ros::ServiceServer  srv_send_soft_trigger_;  
      ros::ServiceServer  srv_get_last_error_;  

      std::mutex mtx_acquisition_;
      bool bool_acquisition_is_started_;

      std::mutex mtx_event_;

      bool bool_event_system_;
      bool bool_event_interface_;
      bool bool_event_device_;
      bool bool_event_datastream_;

      MapCallback map_event_system_;
      MapCallback map_event_interface_;
      MapCallback map_event_localdevice_;
      MapCallback map_event_remotedevice_;
      MapCallback map_event_datastream_;

      std::mutex mtx_chunk_;
      MapChunk  map_chunk_;

      int32_t last_error_;

      bool destroyed_;
  };
}
#endif