sick_mrs1000_communication.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017, Osnabrück University
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, 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 name of Osnabrück University 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  * Author:
30  * Sebastian Pütz <spuetz@uos.de>
31  *
32  */
33 
35 
36 namespace sick_tim
37 {
38 
40  const std::string &port,
41  int &timelimit,
42  ScanAndCloudParser* parser)
43 : SickTimCommonTcp(hostname, port, timelimit, parser),
44  scan_and_cloud_parser_(parser),
45  cloud_pub_(nh_.advertise<sensor_msgs::PointCloud2>("cloud", 300)),
46  diagnosed_cloud_publisher_(cloud_pub_, diagnostics_,
47  diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10),
48  diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_ - config_.time_offset)
49  )
50 {
51  expectedFrequency_ = 12.5; // by data sheet 50 Hz, 4 x 12,5 Hz
52 }
53 
55 {
56 
57 
58 }
59 
61 {
63 
64  unsigned char receiveBuffer[65536];
65  int actual_length = 0;
66  static unsigned int iteration_count = 0;
67 
68  int result = get_datagram(receiveBuffer, 65536, &actual_length);
69  if (result != 0)
70  {
71  ROS_ERROR("Read Error when getting datagram: %i.", result);
72  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Read Error when getting datagram.");
73  return ExitError; // return failure to exit node
74  }
75  if(actual_length <= 0)
76  return ExitSuccess; // return success to continue looping
77 
78  // ----- if requested, skip frames
79  if (iteration_count++ % (config_.skip + 1) != 0)
80  return ExitSuccess;
81 
83  {
84  std_msgs::String datagram_msg;
85  datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
86  datagram_pub_.publish(datagram_msg);
87  }
88 
89  sensor_msgs::LaserScan scan;
90 
91  /*
92  * datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
93  */
94  char* buffer_pos = (char*)receiveBuffer;
95  char *dstart, *dend;
96  while( (dstart = strchr(buffer_pos, 0x02)) && (dend = strchr(dstart + 1, 0x03)) )
97  {
98  size_t dlength = dend - dstart;
99  *dend = '\0';
100  dstart++;
101  sensor_msgs::PointCloud2 cloud;
102  int success = scan_and_cloud_parser_->parse_datagram(dstart, dlength, config_, scan, cloud);
103 
104  if (success == ExitSuccess)
105  {
106 
107  /*
108  * cloud.header.frame_id == "" means we're still accumulating
109  * the layers of the point cloud, so don't publish yet.
110  */
111  if(cloud.header.frame_id != "")
112  {
113  ROS_DEBUG_STREAM("Publish cloud with " << cloud.height * cloud.width
114  << " points in the frame \"" << cloud.header.frame_id << "\".");
116  }
117 
118  /*
119  * scan.header.frame_id == "" means the laser scan was of a layer
120  * different than layer 0, so don't publish it (because the points
121  * don't lie in a plane)
122  */
123  if(scan.header.frame_id != "")
124  {
125  diagnosticPub_->publish(scan);
126  }
127 
128  }
129  buffer_pos = dend + 1;
130  }
131 
132  return ExitSuccess; // return success to continue looping
133 }
134 
136 {
137  int init_base = SickTimCommonTcp::init_scanner();
138  if(init_base != ExitSuccess)
139  {
140  return init_base;
141  }
142 
143  /*
144  * Set Maintenance access mode to allow mode change to be sent
145  */
146  std::vector<unsigned char> access_reply;
147  int result = sendSOPASCommand("\x02sMN SetAccessMode 03 F4724744\x03\0", &access_reply);
148  if (result != 0)
149  {
150  ROS_ERROR("SOPAS - Error setting access mode");
151  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
152  return ExitError;
153  }
154  std::string access_reply_str = replyToString(access_reply);
155  if (access_reply_str != "sAN SetAccessMode 1")
156  {
157  ROS_ERROR_STREAM("SOPAS - Error setting access mode, unexpected response : " << access_reply_str);
158  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
159  return ExitError;
160  }
161 
162  /*
163  * Application selection and switching 'sWN SetActiveApplication'
164  */
165  const char setActiveApplication[] = {"\x02sWN SetActiveApplications 1 RANG 1\x03\0"};
166  std::vector<unsigned char> activeApplicationReply;
167  result = sendSOPASCommand(setActiveApplication, &activeApplicationReply);
168  if (result != 0)
169  {
170  ROS_ERROR("SOPAS - SetActiveApplications failed 'sWN SetActiveApplications'.");
171  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - 'sWN SetActiveApplications 1 RANG 1'.");
172  return ExitError;
173  }
174  std::string activeApplicationReplyStr = replyToString(activeApplicationReply);
175  ROS_INFO_STREAM("active application reply: " << activeApplicationReplyStr);
176 
177  /*
178  * Start streaming 'LMDscandata'.
179  */
180  const char requestScanData[] = {"\x02sEN LMDscandata 1\x03\0"};
181  result = sendSOPASCommand(requestScanData, NULL);
182  if (result != 0)
183  {
184  ROS_ERROR("SOPAS - Error starting to stream 'LMDscandata'.");
185  diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error starting to stream 'LMDscandata'.");
186  return ExitError;
187  }
188 
189  return ExitSuccess;
190 
191 }
192 
193 } /* namespace sick_tim */
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher datagram_pub_
virtual void publish(const boost::shared_ptr< T > &message)
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan > * diagnosticPub_
static std::string replyToString(const std::vector< unsigned char > &reply)
Converts reply from sendSOPASCommand to string.
virtual int get_datagram(unsigned char *receiveBuffer, int bufferSize, int *actual_length)
Read a datagram from the device.
diagnostic_updater::DiagnosedPublisher< sensor_msgs::PointCloud2 > diagnosed_cloud_publisher_
diagnostic_updater::Updater diagnostics_
#define ROS_DEBUG_STREAM(args)
virtual int sendSOPASCommand(const char *request, std::vector< unsigned char > *reply)
Send a SOPAS command to the device and print out the response to the console.
virtual int parse_datagram(char *datagram, size_t datagram_length, SickTimConfig &config, sensor_msgs::LaserScan &scan, sensor_msgs::PointCloud2 &cloud)=0
#define ROS_INFO_STREAM(args)
void broadcast(int lvl, const std::string msg)
#define ROS_ERROR_STREAM(args)
SickMrs1000Communication(const std::string &hostname, const std::string &port, int &timelimit, ScanAndCloudParser *parser)
#define ROS_ERROR(...)


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Wed Jun 17 2020 04:05:36