sick_tim_common_mockup.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015, 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  * Created on: 30.01.2015
30  *
31  * Author:
32  * Martin Günther <mguenthe@uos.de>
33  *
34  */
35 
37 
38 namespace sick_tim
39 {
40 
42 {
43  sub_ = nh_.subscribe("datagram", 1, &SickTimCommonMockup::datagramCB, this);
44 }
45 
47 {
48 }
49 
51 {
52  ROS_INFO("Mockup - close_device()");
53  return 0;
54 }
55 
59 int SickTimCommonMockup::sendSOPASCommand(const char* request, std::vector<unsigned char> * reply)
60 {
61  ROS_ERROR("Mockup - sendSOPASCommand(), this should never be called");
62  return ExitError;
63 }
64 
65 /*
66  * provided as a separate method (not inside constructor) so we can return error codes
67  */
69 {
70  ROS_INFO("Mockup - init_device()");
71  return ExitSuccess;
72 }
73 
74 /*
75  * provided as a separate method (not inside constructor) so we can return error codes
76  */
78 {
79  ROS_INFO("Mockup - init_scanner()");
80  return ExitSuccess;
81 }
82 
83 int SickTimCommonMockup::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
84 {
85  ROS_DEBUG("Mockup - get_datagram()");
86 
87  // wait for next datagram
88  while(!datagram_msg_)
89  {
90  if (!ros::ok())
91  return ExitError;
92 
93  ros::Duration(0.01).sleep();
94  ros::spinOnce();
95  }
96 
97  // copy datagram to receiveBuffer
98  std::vector<char> str(datagram_msg_->data.begin(), datagram_msg_->data.end());
99  str.push_back('\0');
100  *actual_length = datagram_msg_->data.length();
101  datagram_msg_.reset();
102 
103  if (bufferSize < *actual_length + 1)
104  {
105  ROS_ERROR("Mockup - Buffer too small!");
106  return ExitError;
107  }
108 
109  strncpy(reinterpret_cast<char *>(receiveBuffer), &str[0], *actual_length + 1);
110 
111  return ExitSuccess;
112 }
113 
114 void SickTimCommonMockup::datagramCB(const std_msgs::String::ConstPtr &msg)
115 {
116 if (datagram_msg_)
117  ROS_WARN("Mockup - dropping datagram message");
118 
119 datagram_msg_ = msg;
120 }
121 
122 } /* namespace sick_tim */
SickTimCommonMockup(AbstractParser *parser)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
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.
#define ROS_WARN(...)
void datagramCB(const std_msgs::String::ConstPtr &msg)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
virtual int get_datagram(unsigned char *receiveBuffer, int bufferSize, int *actual_length)
Read a datagram from the device.
std_msgs::String::ConstPtr datagram_msg_
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Tue May 7 2019 03:13:47