Program Listing for File stcamera_interface_impl.hpp
↰ Return to documentation for file (/tmp/ws/src/stcamera_ros2/stcamera_components/include/impl/stcamera_interface_impl.hpp)
/******************************************************************************
* Software License Agreement (BSD License)
*
* Copyright (C) 2023, 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_IMPL_H
#define STCAMERA_STCAMERA_INTERFACE_IMPL_H
#include <map>
#include <sstream>
#include "../stcamera_interface.hpp"
#include <StApi_TL.h>
#include <StApi_IP.h>
#define ENABLED_STAPI_ZERO_COPY 1
namespace stcamera
{
#define CATCH_COMMON_ERR_RES(RES) catch(const StApi::CStGenTLErrorException &x) \
{\
RETURN_ERR_RES((EOSError_t)x.GetError(), x.GetDescription(), RES);\
}\
catch(GenICam::GenericException &x)\
{\
RETURN_ERR_RES(OSError_GenICamError, x.GetDescription(), RES);\
}\
catch(...)\
{\
RETURN_ERR_RES(OSError_UnknownError, OSERROR_STR_UNKNOWN_ERROR, RES);\
}
typedef struct StCallback_t
{
std::string topic_name_;
std::string event_name_;
StApi::IStRegisteredCallbackReleasable *cb_;
}StCallback_t;
typedef std::map<std::string, StCallback_t> MapCallback;
typedef std::map<std::string, GenApi::INode*> MapChunk;
#if ENABLED_STAPI_ZERO_COPY
// Custom memory allocator class implementation.
class CImageAllocator : public StApi::IStAllocator
{
public:
explicit CImageAllocator(const std::string &camera_namespace) : camera_namespace_(camera_namespace)
{}
// Function for memory allocation.
void Allocate(void **pCreatedBuffer, size_t nSize, void **pContext) throw()
{
try
{
sensor_msgs::msg::Image *pimage = new sensor_msgs::msg::Image();
pimage->header.frame_id = camera_namespace_;
// Memory allocation of the requested size.
pimage->data.resize(nSize);
*pCreatedBuffer = &pimage->data[0];
// Assigning the number of allocation to pContext.
// Value assigned to pContext is passed as an argument when the memory is deallocated.
*(sensor_msgs::msg::Image**)pContext = pimage;
map_buffer_image_.insert(std::make_pair(*pCreatedBuffer, pimage));
// Display the requested memory size.
//std::cout << "Allocate Size = " << nSize << ", Image=" << pimage << ", Buffer=" << *pCreatedBuffer << std::endl;
}
catch (...)
{
*pCreatedBuffer = NULL;
}
}
// Function for memory deallocation.
void Deallocate(void *ptr, size_t /*nSize*/, void *pContext) throw()
{
// Display the size of the memory to be freed.
//std::cout << "Deallocate Size = " << nSize << ", Buffer=" << ptr << std::endl;
sensor_msgs::msg::Image *pimage = (sensor_msgs::msg::Image *)pContext;
map_buffer_image_.erase(ptr);
// Free the allocated memory.
pimage->data.resize(0);
delete pimage;
}
// Function called when registered allocator is no longer needed.
void OnDeregister() throw()
{
}
sensor_msgs::msg::Image* get_image(void* ptr)
{
auto it = map_buffer_image_.find(ptr);
if(it != map_buffer_image_.end())
{
//std::cout << "CImageAllocator::get_image Buffer=" << ptr <<", Image=" << it->second << std::endl;
return(it->second);
}
//std::cout << "CImageAllocator::get_image Buffer=" << ptr <<", Image=nullptr" << std::endl;
return(nullptr);
}
protected:
std::map<void *, sensor_msgs::msg::Image *> map_buffer_image_;
const std::string camera_namespace_;
};
#endif //ENABLED_STAPI_ZERO_COPY
class StCameraInterfaceImpl: public StCameraInterface
{
public:
StCameraInterfaceImpl(StApi::IStDeviceReleasable *dev,
rclcpp::Node *nh_parent,
const std::string &camera_namespace,
StParameter *param,
rclcpp::Clock &clock,
uint32_t queue_size = STCAMERA_QUEUE_SIZE);
virtual ~StCameraInterfaceImpl();
bool deviceIsLost() override;
protected:
void initDeviceAndDataStream();
void registerDeviceLostEvent();
void activateChunks();
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(
const std::shared_ptr<stcamera_msgs::srv::ReadNode_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNode_Response> res);
bool readNodeBoolCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeBool_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeBool_Response> res);
bool readNodeEnumCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeEnum_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeEnum_Response> res);
bool readNodeIntCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeInt_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeInt_Response> res);
bool readNodeFloatCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeFloat_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeFloat_Response> res);
bool readNodePortCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodePort_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodePort_Response> res);
bool readNodeRegisterCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeRegister_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeRegister_Response> res);
bool readNodeRegisterInfoCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeRegisterInfo_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeRegisterInfo_Response> res);
bool readNodeStringCallback(
const std::shared_ptr<stcamera_msgs::srv::ReadNodeString_Request> req,
std::shared_ptr<stcamera_msgs::srv::ReadNodeString_Response> res);
bool writeNodeCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNode_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNode_Response> res);
bool writeNodeBoolCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeBool_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeBool_Response> res);
bool writeNodeEnumIntCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeEnumInt_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeEnumInt_Response> res);
bool writeNodeEnumStrCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeEnumStr_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeEnumStr_Response> res);
bool writeNodeIntCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeInt_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeInt_Response> res);
bool writeNodeFloatCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeFloat_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeFloat_Response> res);
bool writeNodePortCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodePort_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodePort_Response> res);
bool writeNodeRegisterCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeRegister_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeRegister_Response> res);
bool writeNodeStringCallback(
const std::shared_ptr<stcamera_msgs::srv::WriteNodeString_Request> req,
std::shared_ptr<stcamera_msgs::srv::WriteNodeString_Response> res);
bool executeNodeCallback(
const std::shared_ptr<stcamera_msgs::srv::ExecuteNode_Request> req,
std::shared_ptr<stcamera_msgs::srv::ExecuteNode_Response> res);
bool enableChunkCallback(
const std::shared_ptr<stcamera_msgs::srv::EnableChunk_Request> req,
std::shared_ptr<stcamera_msgs::srv::EnableChunk_Response> res);
bool enableTriggerCallback(
const std::shared_ptr<stcamera_msgs::srv::EnableTrigger_Request> req,
std::shared_ptr<stcamera_msgs::srv::EnableTrigger_Response> res);
bool enableEventNodeCallback(
const std::shared_ptr<stcamera_msgs::srv::EnableEventNode_Request> req,
std::shared_ptr<stcamera_msgs::srv::EnableEventNode_Response> res);
bool enableImageAcquisitionCallback(
const std::shared_ptr<stcamera_msgs::srv::EnableImageAcquisition_Request> req,
std::shared_ptr<stcamera_msgs::srv::EnableImageAcquisition_Response> res);
bool enableEventAcquisitionCallback(
const std::shared_ptr<stcamera_msgs::srv::EnableEventAcquisition_Request> req,
std::shared_ptr<stcamera_msgs::srv::EnableEventAcquisition_Response> res);
bool getImageAcquisitionStatusCallback(
const std::shared_ptr<stcamera_msgs::srv::GetImageAcquisitionStatus_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetImageAcquisitionStatus_Response> res);
bool getEventAcquisitionStatusListCallback(
const std::shared_ptr<stcamera_msgs::srv::GetEventAcquisitionStatusList_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetEventAcquisitionStatusList_Response> res);
bool getEventNodeStatusListCallback(
const std::shared_ptr<stcamera_msgs::srv::GetEventNodeStatusList_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetEventNodeStatusList_Response> res);
bool getChunkListCallback(
const std::shared_ptr<stcamera_msgs::srv::GetChunkList_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetChunkList_Response> res);
bool getTriggerListCallback(
const std::shared_ptr<stcamera_msgs::srv::GetTriggerList_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetTriggerList_Response> res);
bool getEnumListCallback(
const std::shared_ptr<stcamera_msgs::srv::GetEnumList_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetEnumList_Response> res);
bool getGenICamNodeInfoCallback(
const std::shared_ptr<stcamera_msgs::srv::GetGenICamNodeInfo_Request> req,
std::shared_ptr<stcamera_msgs::srv::GetGenICamNodeInfo_Response> res);
bool sendSoftTriggerCallback(
const std::shared_ptr<stcamera_msgs::srv::SendSoftTrigger_Request> req,
std::shared_ptr<stcamera_msgs::srv::SendSoftTrigger_Response> res);
void initializeCameraInfo();
GenApi::INodeMap *getNodeMap(const std::string &genicam_module);
MapCallback *getCallbackMap(const std::string &genicam_module);
void publishEventDefault(stcamera_msgs::msg::Event &msg);
protected:
void initPublishers();
void initServices();
protected:
StApi::CIStDevicePtr tl_dev_;
camera_info_manager::CameraInfoManager camera_info_manager_;
const uint32_t queue_size_;
bool bool_event_system_;
bool bool_event_interface_;
bool bool_event_device_;
bool bool_event_datastream_;
bool destroyed_;
StApi::CIStRegisteredCallbackPtr ist_registered_callback_system_;
StApi::CIStRegisteredCallbackPtr ist_registered_callback_interface_;
StApi::CIStRegisteredCallbackPtr ist_registered_callback_device_;
StApi::CIStRegisteredCallbackPtr ist_registered_callback_datastream_;
image_transport::CameraPublisher it_campub_;
StApi::CIStDataStreamPtr tl_ds_;
rclcpp::Publisher<stcamera_msgs::msg::Event>::SharedPtr def_event_;
rclcpp::Publisher<stcamera_msgs::msg::Chunk>::SharedPtr msg_chunk_;
std::mutex mtx_acquisition_;
bool bool_acquisition_is_started_;
std::mutex mtx_event_;
MapPublisher map_msg_event_;
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_;
#if ENABLED_STAPI_ZERO_COPY
CImageAllocator image_allocator_;
StApi::CIStImageBufferPtr p_stimage_buffer_;
StApi::CIStPixelFormatConverterPtr p_stpixel_format_converter_;
#else
sensor_msgs::msg::Image image_;
#endif //ENABLED_STAPI_ZERO_COPY
sensor_msgs::msg::CameraInfo camera_info_;
protected:
rclcpp::Service<stcamera_msgs::srv::ReadNode>::SharedPtr srv_read_node_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeBool>::SharedPtr srv_read_node_bool_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeEnum>::SharedPtr srv_read_node_enum_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeInt>::SharedPtr srv_read_node_int_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeFloat>::SharedPtr srv_read_node_float_;
rclcpp::Service<stcamera_msgs::srv::ReadNodePort>::SharedPtr srv_read_node_port_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeRegister>::SharedPtr srv_read_node_register_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeRegisterInfo>::SharedPtr srv_read_node_register_info_;
rclcpp::Service<stcamera_msgs::srv::ReadNodeString>::SharedPtr srv_read_node_string_;
rclcpp::Service<stcamera_msgs::srv::WriteNode>::SharedPtr srv_write_node_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeBool>::SharedPtr srv_write_node_bool_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeEnumInt>::SharedPtr srv_write_node_enum_int_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeEnumStr>::SharedPtr srv_write_node_enum_str_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeInt>::SharedPtr srv_write_node_int_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeFloat>::SharedPtr srv_write_node_float_;
rclcpp::Service<stcamera_msgs::srv::WriteNodePort>::SharedPtr srv_write_node_port_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeRegister>::SharedPtr srv_write_node_register_;
rclcpp::Service<stcamera_msgs::srv::WriteNodeString>::SharedPtr srv_write_node_string_;
rclcpp::Service<stcamera_msgs::srv::ExecuteNode>::SharedPtr srv_execute_node_;
rclcpp::Service<stcamera_msgs::srv::EnableChunk>::SharedPtr srv_enable_chunk_;
rclcpp::Service<stcamera_msgs::srv::EnableTrigger>::SharedPtr srv_enable_trigger_;
rclcpp::Service<stcamera_msgs::srv::EnableEventNode>::SharedPtr srv_enable_event_node_;
rclcpp::Service<stcamera_msgs::srv::EnableImageAcquisition>::SharedPtr srv_enable_image_acquisition_;
rclcpp::Service<stcamera_msgs::srv::EnableEventAcquisition>::SharedPtr srv_enable_event_acquisition_;
rclcpp::Service<stcamera_msgs::srv::GetImageAcquisitionStatus>::SharedPtr srv_get_image_acquisition_status_;
rclcpp::Service<stcamera_msgs::srv::GetEventAcquisitionStatusList>::SharedPtr srv_get_event_acquisition_status_list_;
rclcpp::Service<stcamera_msgs::srv::GetEventNodeStatusList>::SharedPtr srv_get_event_node_status_list_;
rclcpp::Service<stcamera_msgs::srv::GetChunkList>::SharedPtr srv_get_chunk_list_;
rclcpp::Service<stcamera_msgs::srv::GetTriggerList>::SharedPtr srv_get_trigger_list_;
rclcpp::Service<stcamera_msgs::srv::GetEnumList>::SharedPtr srv_get_enum_list_;
rclcpp::Service<stcamera_msgs::srv::GetGenICamNodeInfo>::SharedPtr srv_get_genicam_node_info_;
rclcpp::Service<stcamera_msgs::srv::SendSoftTrigger>::SharedPtr srv_send_soft_trigger_;
};
}
#endif //STCAMERA_STCAMERA_INTERFACE_IMPL_H