stcamera_interface_gev.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (C) 2018, OMRON SENTECH. All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the names of OMRON SENTECH nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *****************************************************************************/
29 
31 
32 namespace stcamera
33 {
35  StApi::IStDeviceReleasable *dev,
36  ros::NodeHandle nh_parent,
37  const std::string &camera_namespace,
38  StParameter *param,
39  uint32_t queue_size):
40  StCameraInterface(dev, nh_parent, camera_namespace, param,
41  queue_size),
42  srv_get_gige_ip_(nh_.advertiseService(std::string(STSRV_G_gige_ip),
43  &StCameraInterfaceGEV::getIPCallback, this)),
44  srv_set_gige_ip_(nh_.advertiseService(std::string(STSRV_S_gige_ip),
45  &StCameraInterfaceGEV::setIPCallback, this))
46  {
47  }
48 
50  {
51  }
52 
54  omronsentech_camera::GetGigEIP::Request &req,
55  omronsentech_camera::GetGigEIP::Response &res)
56  {
57  try
58  {
59  GenApi::CNodeMapPtr p_nodemap_iface(
60  tl_dev_->GetIStInterface()->GetIStPort()->GetINodeMap());
61  GenApi::CIntegerPtr p_integer_ip_iface(
62  p_nodemap_iface->GetNode("GevInterfaceSubnetIPAddress"));
63  GenApi::CIntegerPtr p_integer_subnet_iface(
64  p_nodemap_iface->GetNode("GevInterfaceSubnetMask"));
65  GenApi::CIntegerPtr p_integer_gw_iface(
66  p_nodemap_iface->GetNode("GevInterfaceGateway"));
67  res.if_ip_address = p_integer_ip_iface->ToString();
68  res.if_ip_mask = p_integer_subnet_iface->ToString();
69  res.if_ip_gateway = p_integer_gw_iface->ToString();
70 
71 
72  GenApi::CNodeMapPtr p_nodemap_dev(
73  tl_dev_->GetLocalIStPort()->GetINodeMap());
74  GenApi::CIntegerPtr p_integer_ip_dev(
75  p_nodemap_dev->GetNode("GevDeviceIPAddress"));
76  GenApi::CIntegerPtr p_integer_subnet_dev(
77  p_nodemap_dev->GetNode("GevDeviceSubnetMask"));
78  GenApi::CIntegerPtr p_integer_gw_dev(
79  p_nodemap_dev->GetNode("GevDeviceGateway"));
80  res.dev_ip_address = p_integer_ip_dev->ToString();
81  res.dev_ip_mask = p_integer_subnet_dev->ToString();
82  res.dev_ip_gateway = p_integer_gw_dev->ToString();
83  return true;
84  }
86  }
87 
89  omronsentech_camera::SetGigEIP::Request &req,
90  omronsentech_camera::SetGigEIP::Response &res)
91  {
92  std::lock_guard<std::mutex> lock1(mtx_acquisition_);
94  {
96  }
97  try
98  {
99  StApi::IStInterface *p_iface = tl_dev_->GetIStInterface();
100  if (p_iface->GetIStInterfaceInfo()->GetTLType().compare(TLTypeGEVName) !=
101  0)
102  {
103  last_error_ = GenTL::GC_ERR_INVALID_ID;
104  return false;
105  }
106 
107  GenApi::CNodeMapPtr p_nodemap(p_iface->GetIStPort()->GetINodeMap());
108  GenApi::CIntegerPtr p_dev_selector(p_nodemap->GetNode("DeviceSelector"));
109  GenICam::gcstring strMyID = tl_dev_->GetIStDeviceInfo()->GetID();
110  const int64_t max_index = p_dev_selector->GetMax();
111  for (int64_t k = 0; k <= max_index; k++)
112  {
113  p_dev_selector->SetValue(k);
114  GenApi::CStringPtr p_device_id(p_nodemap->GetNode("DeviceID"));
115  GenICam::gcstring strDeviceID = p_device_id->GetValue();
116 
117  if (strDeviceID.compare(strMyID.c_str()) != 0) continue;
118 
119  // deregister current callback callback
120  MapCallback *cblist[3] =
121  {
125  };
126  for (int i = 0; i < 3; i++)
127  {
128  MapCallback *cb = cblist[i];
129  for (MapCallback::iterator it = cb->begin(); it != cb->end(); it++)
130  {
131  StApi::IStRegisteredCallbackReleasable *cbf = it->second.cb_;
132  cbf->Release();
133  MapPublisher::iterator itpub =
134  map_msg_event_.find(it->second.topic_name_);
135  if (itpub != map_msg_event_.end())
136  {
137  map_msg_event_.erase(itpub);
138  }
139  }
140  cb->clear();
141  }
142 
143  // delete datastream and device
144  tl_ds_.Reset(NULL);
145  tl_dev_.Reset(NULL);
146 
147  // update IP
148  GenApi::CIntegerPtr p_integer_ip(
149  p_nodemap->GetNode("GevDeviceForceIPAddress"));
150  GenApi::CIntegerPtr p_integer_subnet(
151  p_nodemap->GetNode("GevDeviceForceSubnetMask"));
152  GenApi::CIntegerPtr p_integer_gw(
153  p_nodemap->GetNode("GevDeviceForceGateway"));
154  GenApi::CCommandPtr p_command(
155  p_nodemap->GetNode("GevDeviceForceIP"));
156 
157  p_integer_ip->SetValue(ntohl(inet_addr(req.ip_address.c_str())));
158  p_integer_subnet->SetValue(ntohl(inet_addr(req.ip_mask.c_str())));
159  p_integer_gw->SetValue(ntohl(inet_addr(req.ip_gateway.c_str())));
160  p_command->Execute();
161 
162  p_iface->UpdateDeviceList();
163 
164  // re-open device
165  tl_dev_.Reset(p_iface->CreateIStDevice(strMyID,
166  GenTL::DEVICE_ACCESS_CONTROL));
167 
168  // re-open datastream module
169  bool_event_datastream_ = false;
170  tl_ds_.Reset(tl_dev_->CreateIStDataStream(0));
171  StApi::RegisterCallback(tl_ds_, *this,
173 
174  // re-register only device lost
175  if (tl_dev_->GetIStInterface()->GetIStSystem()->GetStSystemVendor() ==
176  StApi::StSystemVendor_Sentech)
177  {
178  struct StCallback stc;
179  stc.topic_name_ = STMSG_event;
180 
181  std::string callback_node = "EventDeviceLost";
182  GenApi::CNodeMapPtr p(tl_dev_->GetLocalIStPort()->GetINodeMap());
183  GenApi::CNodePtr node_callback(p->GetNode(callback_node.c_str()));
184  stc.cb_ = StApi::RegisterCallback(node_callback, *this,
186  (void*)(&map_event_localdevice_), GenApi::cbPostInsideLock);
187  GenApi::CEnumerationPtr p_event_selector(p->GetNode("EventSelector"));
188  GenApi::CEnumEntryPtr p_event_selector_entry(
189  p_event_selector->GetEntryByName("DeviceLost"));
190  p_event_selector->SetIntValue(p_event_selector_entry->GetValue());
191  GenApi::CEnumerationPtr p_event_notif(
192  p->GetNode("EventNotification"));
193  GenApi::CEnumEntryPtr p_event_notif_entry(
194  p_event_notif->GetEntryByName("On"));
195  p_event_notif->SetIntValue(p_event_notif_entry->GetValue());
196  tl_dev_->StartEventAcquisitionThread();
197  map_event_localdevice_[callback_node] = stc;
198  bool_event_device_ = true;
199  }
200 
201  // check if chunk is enabled.
202  GenApi::CNodeMapPtr mp = tl_dev_->GetRemoteIStPort()->GetINodeMap();
203  GenApi::INode *node_sel = mp->GetNode("ChunkSelector");
204  GenApi::INode *node_enable = mp->GetNode("ChunkEnable");
205  if (node_sel && node_enable)
206  {
207  GenApi::CEnumerationPtr chunk_selector(node_sel);
208  GenApi::NodeList_t nodelist;
209  chunk_selector->GetEntries(nodelist);
210  for (GenApi::NodeList_t::iterator it = nodelist.begin();
211  it != nodelist.end(); it++)
212  {
213  if (GenApi::IsAvailable(*it))
214  {
215  GenApi::CEnumEntryPtr enum_entry(*it);
216  std::string chunk_name = enum_entry->GetSymbolic().c_str();
217  chunk_selector->SetIntValue(enum_entry->GetValue());
218  GenApi::CBooleanPtr chunk_enable(node_enable);
219  if (GenApi::IsReadable(chunk_enable) &&
220  chunk_enable->GetValue() == true)
221  {
222  GenICam::gcstring chunk_value_name("Chunk");
223  chunk_value_name.append(chunk_name.c_str());
224  GenApi::CNodePtr p_chunk_value(mp->GetNode(chunk_value_name));
225  if (!p_chunk_value) continue;
226  MapChunk::iterator itm = map_chunk_.find(p_chunk_value
227  ->GetName().c_str());
228  if (itm == map_chunk_.end() || itm->second == nullptr)
229  {
230  map_chunk_[p_chunk_value->GetName().c_str()] = p_chunk_value;
231  }
232  }
233  }
234  }
235  }
236  ROS_INFO("IP Address of the GigE Camera has been updated. "
237  "You have to re-register any custom callback event.");
238 
239  return true;
240  }
241  }
243  }
244 
245 } // end of namespace stcamera
StApi::IStRegisteredCallbackReleasable * cb_
Class to control a connected GigEVision camera.
#define ACQ_ALREADY_ON_ERROR
Definition: stheader.h:73
StApi::CIStDataStreamPtr tl_ds_
#define STSRV_S_gige_ip
Definition: stheader.h:150
bool setIPCallback(omronsentech_camera::SetGigEIP::Request &req, omronsentech_camera::SetGigEIP::Response &res)
#define CHECK_NULLPTR(P, X, MSG)
#define ACQ_ALREADY_ON_ERROR_STR
Definition: stheader.h:74
bool getIPCallback(omronsentech_camera::GetGigEIP::Request &req, omronsentech_camera::GetGigEIP::Response &res)
StApi::CIStDevicePtr tl_dev_
#define ROS_INFO(...)
std::map< std::string, struct StCallback > MapCallback
Base class to control a connected camera.
#define STSRV_G_gige_ip
Definition: stheader.h:147
#define STMSG_event
Definition: stheader.h:124
Class to handle ROS parameter.
Definition: stparameter.h:51
void eventDataStreamCB(StApi::IStCallbackParamBase *p, void *pvContext)
#define CATCH_COMMON_ERR()
StCameraInterfaceGEV(StApi::IStDeviceReleasable *dev, ros::NodeHandle nh_parent, const std::string &camera_namespace, StParameter *param, uint32_t queue_size=STCAMERA_QUEUE_SIZE)
void eventGenApiNodeCB(GenApi::INode *p, void *pvContext)
Class to control a connected GigEVision camera.


omronsentech_camera
Author(s): OSE ROS Support
autogenerated on Tue Jul 2 2019 19:44:14