sick_scan_messages.h
Go to the documentation of this file.
1 #include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */
2 /*
3  * @brief Implementation of ROS messages for sick_scan
4  *
5  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
6  * Copyright (C) 2021, SICK AG, Waldkirch
7  * All rights reserved.
8  *
9 * Licensed under the Apache License, Version 2.0 (the "License");
10 * you may not use this file except in compliance with the License.
11 * You may obtain a copy of the License at
12 *
13 * http://www.apache.org/licenses/LICENSE-2.0
14 *
15 * Unless required by applicable law or agreed to in writing, software
16 * distributed under the License is distributed on an "AS IS" BASIS,
17 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
18 * See the License for the specific language governing permissions and
19 * limitations under the License.
20 *
21 *
22 * All rights reserved.
23 *
24 * Redistribution and use in source and binary forms, with or without
25 * modification, are permitted provided that the following conditions are met:
26 *
27 * * Redistributions of source code must retain the above copyright
28 * notice, this list of conditions and the following disclaimer.
29 * * Redistributions in binary form must reproduce the above copyright
30 * notice, this list of conditions and the following disclaimer in the
31 * documentation and/or other materials provided with the distribution.
32 * * Neither the name of Osnabrueck University nor the names of its
33 * contributors may be used to endorse or promote products derived from
34 * this software without specific prior written permission.
35 * * Neither the name of SICK AG nor the names of its
36 * contributors may be used to endorse or promote products derived from
37 * this software without specific prior written permission
38 * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
39 * contributors may be used to endorse or promote products derived from
40 * this software without specific prior written permission
41 *
42 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
43 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
44 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
45 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
46 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
47 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
48 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
49 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
50 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
51 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52 * POSSIBILITY OF SUCH DAMAGE.
53  *
54  * Created on: 12.01.2021
55  *
56  * Authors:
57  * Michael Lehning <michael.lehning@lehning.de>
58  *
59  * Based on the TiM communication example by SICK AG.
60  *
61  */
62 
63 #ifndef SICK_SCAN_MESSAGES_H_
64 #define SICK_SCAN_MESSAGES_H_
65 
66 #include "sick_scan/sick_scan_common.h"
67 #include "sick_scan/sick_scan_common_tcp.h"
68 
69 namespace sick_scan_xd
70 {
71 
72  typedef uint8_t* uint8_ptr;
73  template<typename T> inline bool readBinaryBuffer(uint8_ptr& buffer, int & bufferlen, T& value)
74  {
75  if((int)sizeof(value) > bufferlen)
76  {
77  ROS_ERROR_STREAM("## ERROR SickScanMessages::readBinaryBuffer(): bufferlen=" << bufferlen << " byte, " << sizeof(value) << " byte required.");
78  return false;
79  }
80  memcpy(&value, buffer, sizeof(value));
81  swap_endian((unsigned char *) &value, sizeof(value));
82  buffer += sizeof(value);
83  bufferlen -= (int)sizeof(value);
84  return true;
85  }
86 
87  class SickScanMessages
88  {
89  public:
90 
91  SickScanMessages(rosNodePtr nh = 0);
92 
93  virtual ~SickScanMessages();
94 
95  /*
96  * @brief parses and converts a lidar LIDoutputstate message to a ros LIDoutputstate message
97  *
98  * Example LIDoutputstate message from 20210111_sick_tim781s_mon_elephant.pcapng.json:
99  * "tcp.description": ".......4sSN LIDoutputstate .................................E",
100  * "tcp.payload": "02:02:02:02:00:00:00:34:73:53:4e:20:4c:49:44:6f:75:74:70:75:74:73:74:61:74:65:20:00:00:00:00:00:00:01:00:00:01:26:00:00:00:01:22:00:00:00:00:00:01:00:00:00:04:02:00:00:00:00:00:00:45"
101  * 33 byte LIDoutputstate payload after "sSN LIDoutputstate ": "00:00:00:00:00:00:01:00:00:01:26:00:00:00:01:22:00:00:00:00:00:01:00:00:00:04:02:00:00:00:00:00:00"
102  *
103  * @param[in] timeStamp timestamp on receiving the lidar message
104  * @param[in] receiveBuffer byte array of lidar message
105  * @param[in] receiveLength size of lidar message in byte
106  * @param[in] useBinaryProtocol binary lidar message (true, Cola-B) or ascii lidar message (false, Cola-A)
107  * @param[in] frame_id frame id of output message
108  * @param[out] output_msg converted output message
109  *
110  * @return true on success, false on error
111  */
112  static bool parseLIDoutputstateMsg(const rosTime& timeStamp, uint8_t* receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string& frame_id, sick_scan_msg::LIDoutputstateMsg& output_msg);
113 
114  /*
115  * @brief parses and converts a lidar LFErec message to a ros LFErec message
116  *
117  * Example LIDoutputstate message from 20210111_sick_tim781s_lferec_elephant.pcapng.json:
118  * "tcp.description": "........sSN LFErec ........................0..............3...........................0..............4....P......................0..............35.....",
119  * "tcp.payload": "02:02:02:02:00:00:00:8e:73:53:4e:20:4c:46:45:72:65:63:20:00:03:00:01:01:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:00:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:2e:00:09:ac:90:00:01:02:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:34:02:00:06:9f:50:00:01:03:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:35:00:09:ac:90:ac"
120  * 131 byte LFErec payload after "sSN LFErec ": "00:03:00:01:01:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:00:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:2e:00:09:ac:90:00:01:02:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:34:02:00:06:9f:50:00:01:03:00:00:00:00:3f:80:00:00:00:00:00:00:00:00:0d:05:ff:f9:22:30:01:00:00:00:00:00:00:00:01:07:b2:01:01:08:33:35:00:09:ac:90"
121  * LFErec payload starts with 16 byte fields_number "00:03" -> 3 fields, each field has (131-2)/3 = 43 byte
122  * Each field contains a field_index (1,2,3) and field_result (0: invalid/incorrect, 1: free/clear, 2: infringed)
123  * Note: field indices are sorted in reverse order, i.e. with 2 fields configured, we have typically
124  * output_msg.fields[0].field_index = 1, output_msg.fields[0].field_result_mrs = 0 (invalid)
125  * output_msg.fields[1].field_index = 2, output_msg.fields[1].field_result_mrs = 1 or 2 (clear=1 or infringed=2)
126  * output_msg.fields[2].field_index = 3, output_msg.fields[2].field_result_mrs = 1 or 2 (clear=1 or infringed=2)
127  *
128  * @param[in] timeStamp timestamp on receiving the lidar message
129  * @param[in] receiveBuffer byte array of lidar message
130  * @param[in] receiveLength size of lidar message in byte
131  * @param[in] useBinaryProtocol binary lidar message (true, Cola-B) or ascii lidar message (false, Cola-A)
132  * @param[in] eval_field_logic USE_EVAL_FIELD_LMS5XX_LOGIC or USE_EVAL_FIELD_TIM7XX_LOGIC
133  * @param[in] frame_id frame id of output message
134  * @param[out] output_msg converted output message
135  *
136  * @return true on success, false on error
137  */
138  static bool parseLFErecMsg(const rosTime& timeStamp, uint8_t* receiveBuffer, int receiveLength, bool useBinaryProtocol, EVAL_FIELD_SUPPORT eval_field_logic, const std::string& frame_id, sick_scan_msg::LFErecMsg& output_msg);
139 
140  /*
141  * @brief returns the sopas command keyword, e.g.: getSopasCmdKeyword("\x02\x02\x02\x02\x00\x00\x00\x8esSN LFErec \x3", 20) returns "LFErec".
142  */
143  static std::string getSopasCmdKeyword(const uint8_t* sopasRequest, int requestLength);
144 
145  protected:
146 
147  /*
148  * Member data
149  */
150 
151  }; /* class SickScanMessages */
152 
153 } /* namespace sick_scan_xd */
154 #endif /* SICK_SCAN_MESSAGES_H_ */
sick_scan_xd::SickScanMessages
Definition: sick_scan_messages.h:87
swap_endian
void swap_endian(unsigned char *ptr, int numBytes)
Universal swapping function.
Definition: sick_scan_common.cpp:125
sick_scan_xd::SickScanMessages::SickScanMessages
SickScanMessages(rosNodePtr nh=0)
Definition: sick_scan_messages.cpp:64
sick_scan_xd
Definition: abstract_parser.cpp:65
sick_scan_common.h
sick_scan_xd::LFErecMsg
::sick_scan_xd::LFErecMsg_< std::allocator< void > > LFErecMsg
Definition: LFErecMsg.h:61
sick_scan_xd::readBinaryBuffer
bool readBinaryBuffer(uint8_ptr &buffer, int &bufferlen, T &value)
Definition: sick_scan_messages.h:73
sick_scan_xd::SickScanMessages::getSopasCmdKeyword
static std::string getSopasCmdKeyword(const uint8_t *sopasRequest, int requestLength)
Definition: sick_scan_messages.cpp:447
sick_scan_common_tcp.h
sick_scan_xd::EVAL_FIELD_SUPPORT
EVAL_FIELD_SUPPORT
Definition: sick_generic_parser.h:93
sick_scan_xd::LIDoutputstateMsg
::sick_scan_xd::LIDoutputstateMsg_< std::allocator< void > > LIDoutputstateMsg
Definition: LIDoutputstateMsg.h:110
ros::NodeHandle
sick_scan_xd::uint8_ptr
uint8_t * uint8_ptr
Definition: sick_scan_messages.h:72
sick_scan_xd::SickScanMessages::parseLFErecMsg
static bool parseLFErecMsg(const rosTime &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, EVAL_FIELD_SUPPORT eval_field_logic, const std::string &frame_id, sick_scan_msg::LFErecMsg &output_msg)
Definition: sick_scan_messages.cpp:308
ros::Time
sick_scan_base.h
sick_scan_xd::SickScanMessages::~SickScanMessages
virtual ~SickScanMessages()
Definition: sick_scan_messages.cpp:68
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(...)
Definition: sick_scan_ros2_example.cpp:72
roswrap::message_traits::timeStamp
ros::Time * timeStamp(M &m)
returns TimeStamp<M>::pointer(m);
Definition: message_traits.h:317
sick_scan_xd::SickScanMessages::parseLIDoutputstateMsg
static bool parseLIDoutputstateMsg(const rosTime &timeStamp, uint8_t *receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string &frame_id, sick_scan_msg::LIDoutputstateMsg &output_msg)
Definition: sick_scan_messages.cpp:102

sick_scan_xd
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Fri Oct 25 2024 02:47:11