Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00067
00068 int SickTimCommonMockup::init_device()
00069 {
00070 ROS_INFO("Mockup - init_device()");
00071 return ExitSuccess;
00072 }
00073
00074
00075
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
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
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 }