sick_scan_services.cpp
Go to the documentation of this file.
1 /*
2  * @brief Implementation of ROS services for sick_scan
3  *
4  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
5  * Copyright (C) 2021, SICK AG, Waldkirch
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in the
15  * documentation and/or other materials provided with the distribution.
16  * * Neither the name of Osnabrück University nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  * Created on: 12.01.2021
33  *
34  * Authors:
35  * Michael Lehning <michael.lehning@lehning.de>
36  *
37  * Based on the TiM communication example by SICK AG.
38  *
39  */
40 
42 
44 : m_common_tcp(common_tcp), m_cola_binary(cola_binary)
45 {
46  if(nh)
47  {
51  }
52 }
53 
55 {
56 }
57 
65 bool sick_scan::SickScanServices::sendSopasAndCheckAnswer(const std::string& sopasCmd, std::vector<unsigned char>& sopasReplyBin, std::string& sopasReplyString)
66 {
67  if(m_common_tcp)
68  {
69  std::string sopasRequest = std::string("\x02") + sopasCmd + "\x03";
70  int result = -1;
71  if (m_cola_binary)
72  {
73  std::vector<unsigned char> reqBinary;
74  m_common_tcp->convertAscii2BinaryCmd(sopasRequest.c_str(), &reqBinary);
75  result = m_common_tcp->sendSopasAndCheckAnswer(reqBinary, &sopasReplyBin, -1);
76  }
77  else
78  {
79  result = m_common_tcp->sendSopasAndCheckAnswer(sopasRequest.c_str(), &sopasReplyBin, -1);
80  }
81  if (result != 0)
82  {
83  ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command \"" << sopasCmd << "\"");
84  }
85  else
86  {
87  sopasReplyString = m_common_tcp->sopasReplyToString(sopasReplyBin);
88  ROS_INFO_STREAM("SickScanServices: Request \"" << sopasCmd << "\" successfully sent, received reply \"" << sopasReplyString << "\"");
89  return true;
90  }
91  }
92  else
93  {
94  ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: m_common_tcp not initialized");
95  }
96  return false;
97 }
98 
105 bool sick_scan::SickScanServices::serviceCbColaMsg(sick_scan::ColaMsgSrv::Request &service_request, sick_scan::ColaMsgSrv::Response &service_response)
106 {
107  std::string sopasCmd = service_request.request;
108  std::vector<unsigned char> sopasReplyBin;
109  std::string sopasReplyString;
110 
111  if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString))
112  {
113  ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\"");
114  return false;
115  }
116 
117  ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
118  ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
119 
120  service_response.response = sopasReplyString;
121  return true;
122 }
123 
131 bool sick_scan::SickScanServices::serviceCbECRChangeArr(sick_scan::ECRChangeArrSrv::Request &service_request, sick_scan::ECRChangeArrSrv::Response &service_response)
132 {
133  std::string sopasCmd = std::string("sEN ECRChangeArr ") + (service_request.active ? "1" : "0");
134  std::vector<unsigned char> sopasReplyBin;
135  std::string sopasReplyString;
136 
137  if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString))
138  {
139  ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\"");
140  return false;
141  }
142 
143  ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
144  ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
145 
146  return true;
147 }
148 
156 bool sick_scan::SickScanServices::serviceCbLIDoutputstate(sick_scan::LIDoutputstateSrv::Request &service_request, sick_scan::LIDoutputstateSrv::Response &service_response)
157 {
158  std::string sopasCmd = std::string("sEN LIDoutputstate ") + (service_request.active ? "1" : "0");
159  std::vector<unsigned char> sopasReplyBin;
160  std::string sopasReplyString;
161 
162  if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString))
163  {
164  ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\"");
165  return false;
166  }
167 
168  ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
169  ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
170 
171  return true;
172 }
sick_scan::SickScanServices::~SickScanServices
virtual ~SickScanServices()
Definition: sick_scan_services.cpp:54
sick_scan::SickScanServices::m_srv_server_LIDoutputstate
ros::ServiceServer m_srv_server_LIDoutputstate
service "LIDoutputstate", &sick_scan::SickScanServices::serviceCbLIDoutputstate
Definition: sick_scan_services.h:106
sick_scan::SickScanServices::m_srv_server_ECRChangeArr
ros::ServiceServer m_srv_server_ECRChangeArr
service "ECRChangeArr", &sick_scan::SickScanServices::serviceCbECRChangeArr
Definition: sick_scan_services.h:105
sick_scan::SickScanServices::serviceCbLIDoutputstate
bool serviceCbLIDoutputstate(sick_scan::LIDoutputstateSrv::Request &service_request, sick_scan::LIDoutputstateSrv::Response &service_response)
Definition: sick_scan_services.cpp:156
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
sick_scan::SickScanServices::SickScanServices
SickScanServices(ros::NodeHandle *nh=0, sick_scan::SickScanCommonTcp *common_tcp=0, bool cola_binary=true)
Definition: sick_scan_services.cpp:43
sick_scan::SickScanServices::serviceCbColaMsg
bool serviceCbColaMsg(sick_scan::ColaMsgSrv::Request &service_request, sick_scan::ColaMsgSrv::Response &service_response)
Definition: sick_scan_services.cpp:105
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
sick_scan::SickScanCommonTcp
Definition: sick_scan_common_tcp.h:68
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
sick_scan_services.h
sick_scan::SickScanServices::serviceCbECRChangeArr
bool serviceCbECRChangeArr(sick_scan::ECRChangeArrSrv::Request &service_request, sick_scan::ECRChangeArrSrv::Response &service_response)
Definition: sick_scan_services.cpp:131
sick_scan::SickScanServices::sendSopasAndCheckAnswer
bool sendSopasAndCheckAnswer(const std::string &sopasCmd, std::vector< unsigned char > &sopasReplyBin, std::string &sopasReplyString)
Definition: sick_scan_services.cpp:65
sick_scan::SickScanServices::m_srv_server_ColaMsg
ros::ServiceServer m_srv_server_ColaMsg
service "ColaMsg", &sick_scan::SickScanServices::serviceCbColaMsg
Definition: sick_scan_services.h:104
ros::NodeHandle


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19