sick_tim_common_mockup.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2015, Osnabrück University
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Osnabrück University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *  Created on: 30.01.2015
00030  *
00031  *      Author:
00032  *         Martin Günther <mguenthe@uos.de>
00033  *
00034  */
00035 
00036 #include <sick_tim/sick_tim_common_mockup.h>
00037 
00038 namespace sick_tim
00039 {
00040 
00041 SickTimCommonMockup::SickTimCommonMockup(AbstractParser* parser) : SickTimCommon(parser)
00042 {
00043   sub_ = nh_.subscribe("datagram", 1, &SickTimCommonMockup::datagramCB, this);
00044 }
00045 
00046 SickTimCommonMockup::~SickTimCommonMockup()
00047 {
00048 }
00049 
00050 int SickTimCommonMockup::close_device()
00051 {
00052   ROS_INFO("Mockup - close_device()");
00053   return 0;
00054 }
00055 
00059 int SickTimCommonMockup::sendSOPASCommand(const char* request, std::vector<unsigned char> * reply)
00060 {
00061   ROS_ERROR("Mockup - sendSOPASCommand(), this should never be called");
00062   return ExitError;
00063 }
00064 
00065 /*
00066  * provided as a separate method (not inside constructor) so we can return error codes
00067  */
00068 int SickTimCommonMockup::init_device()
00069 {
00070   ROS_INFO("Mockup - init_device()");
00071   return ExitSuccess;
00072 }
00073 
00074 /*
00075  * provided as a separate method (not inside constructor) so we can return error codes
00076  */
00077 int SickTimCommonMockup::init_scanner()
00078 {
00079   ROS_INFO("Mockup - init_scanner()");
00080   return ExitSuccess;
00081 }
00082 
00083 int SickTimCommonMockup::get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length)
00084 {
00085   ROS_DEBUG("Mockup - get_datagram()");
00086 
00087   // wait for next datagram
00088   while(!datagram_msg_)
00089   {
00090     if (!ros::ok())
00091       return ExitError;
00092 
00093     ros::Duration(0.01).sleep();
00094     ros::spinOnce();
00095   }
00096 
00097   // copy datagram to receiveBuffer
00098   std::vector<char> str(datagram_msg_->data.begin(), datagram_msg_->data.end());
00099   str.push_back('\0');
00100   *actual_length = datagram_msg_->data.length();
00101   datagram_msg_.reset();
00102 
00103   if (bufferSize < *actual_length + 1)
00104   {
00105     ROS_ERROR("Mockup - Buffer too small!");
00106     return ExitError;
00107   }
00108 
00109   strncpy(reinterpret_cast<char *>(receiveBuffer), &str[0], *actual_length + 1);
00110 
00111   return ExitSuccess;
00112 }
00113 
00114 void SickTimCommonMockup::datagramCB(const std_msgs::String::ConstPtr &msg)
00115 {
00116 if (datagram_msg_)
00117   ROS_WARN("Mockup - dropping datagram message");
00118 
00119 datagram_msg_ = msg;
00120 }
00121 
00122 } /* namespace sick_tim */


sick_tim
Author(s): Jochen Sprickerhof , Martin Günther , Sebastian Pütz
autogenerated on Thu May 9 2019 02:32:02