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
00037
00038
00039 #include <sick_tim/sick_tim_common.h>
00040
00041 namespace sick_tim
00042 {
00043
00044 SickTimCommon::SickTimCommon(AbstractParser* parser) :
00045 diagnosticPub_(NULL), expectedFrequency_(15.0), parser_(parser)
00046
00047 {
00048 dynamic_reconfigure::Server<sick_tim::SickTimConfig>::CallbackType f;
00049 f = boost::bind(&sick_tim::SickTimCommon::update_config, this, _1, _2);
00050 dynamic_reconfigure_server_.setCallback(f);
00051
00052
00053 ros::NodeHandle pn("~");
00054 pn.param<bool>("publish_datagram", publish_datagram_, false);
00055 if (publish_datagram_)
00056 datagram_pub_ = nh_.advertise<std_msgs::String>("datagram", 1000);
00057
00058
00059 pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1000);
00060
00061 diagnostics_.setHardwareID("none");
00062 diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>(pub_, diagnostics_,
00063
00064 diagnostic_updater::FrequencyStatusParam(&expectedFrequency_, &expectedFrequency_, 0.1, 10),
00065
00066 diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0/expectedFrequency_));
00067 ROS_ASSERT(diagnosticPub_ != NULL);
00068 }
00069
00070 int SickTimCommon::stop_scanner()
00071 {
00072
00073
00074
00075 const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
00076 int result = sendSOPASCommand(requestScanData0, NULL);
00077 if (result != 0)
00078
00079 printf("\nSOPAS - Error stopping streaming scan data!\n");
00080 else
00081 printf("\nSOPAS - Stopped streaming scan data.\n");
00082
00083 return result;
00084 }
00085
00086 SickTimCommon::~SickTimCommon()
00087 {
00088 delete diagnosticPub_;
00089 delete parser_;
00090
00091 printf("sick_tim driver exiting.\n");
00092 }
00093
00094
00095 int SickTimCommon::init()
00096 {
00097 int result = init_device();
00098 if(result != 0) {
00099 ROS_FATAL("Failed to init device: %d", result);
00100 return result;
00101 }
00102 result = init_scanner();
00103 if(result != 0) {
00104 ROS_FATAL("Failed to init scanner: %d", result);
00105 }
00106 return result;
00107 }
00108
00109 int SickTimCommon::init_scanner()
00110 {
00111
00112
00113
00114 const char requestDeviceIdent[] = "\x02sRI0\x03\0";
00115 std::vector<unsigned char> identReply;
00116 int result = sendSOPASCommand(requestDeviceIdent, &identReply);
00117 if (result != 0)
00118 {
00119 ROS_ERROR("SOPAS - Error reading variable 'DeviceIdent'.");
00120 diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'DeviceIdent'.");
00121 }
00122
00123
00124
00125
00126 const char requestSerialNumber[] = "\x02sRN SerialNumber\x03\0";
00127 std::vector<unsigned char> serialReply;
00128 result = sendSOPASCommand(requestSerialNumber, &serialReply);
00129 if (result != 0)
00130 {
00131 ROS_ERROR("SOPAS - Error reading variable 'SerialNumber'.");
00132 diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'SerialNumber'.");
00133 }
00134
00135
00136 identReply.push_back(0);
00137 serialReply.push_back(0);
00138 std::string identStr;
00139 for(std::vector<unsigned char>::iterator it = identReply.begin(); it != identReply.end(); it++) {
00140 if(*it > 13)
00141 identStr.push_back(*it);
00142 }
00143 std::string serialStr;
00144 for(std::vector<unsigned char>::iterator it = serialReply.begin(); it != serialReply.end(); it++) {
00145 if(*it > 13)
00146 serialStr.push_back(*it);
00147 }
00148 diagnostics_.setHardwareID(identStr + " " + serialStr);
00149
00150
00151
00152
00153 const char requestFirmwareVersion[] = {"\x02sRN FirmwareVersion\x03\0"};
00154 result = sendSOPASCommand(requestFirmwareVersion, NULL);
00155 if (result != 0)
00156 {
00157 ROS_ERROR("SOPAS - Error reading variable 'FirmwareVersion'.");
00158 diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'FirmwareVersion'.");
00159 }
00160
00161
00162
00163
00164 const char requestScanData[] = {"\x02sEN LMDscandata 1\x03\0"};
00165 result = sendSOPASCommand(requestScanData, NULL);
00166 if (result != 0)
00167 {
00168 ROS_ERROR("SOPAS - Error starting to stream 'LMDscandata'.");
00169 diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error starting to stream 'LMDscandata'.");
00170 return EXIT_FAILURE;
00171 }
00172
00173 return EXIT_SUCCESS;
00174 }
00175
00176 int SickTimCommon::loopOnce()
00177 {
00178 diagnostics_.update();
00179
00180 unsigned char receiveBuffer[65536];
00181 int actual_length = 0;
00182 static unsigned int iteration_count = 0;
00183
00184 int result = get_datagram(receiveBuffer, 65536, &actual_length);
00185 if (result != 0)
00186 {
00187 ROS_ERROR("Read Error when getting datagram: %i.", result);
00188 diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Read Error when getting datagram.");
00189 return EXIT_FAILURE;
00190 }
00191 if(actual_length <= 0)
00192 return EXIT_SUCCESS;
00193
00194
00195 if (iteration_count++ % (config_.skip + 1) != 0)
00196 return EXIT_SUCCESS;
00197
00198 if (publish_datagram_)
00199 {
00200 std_msgs::String datagram_msg;
00201 datagram_msg.data = std::string(reinterpret_cast<char*>(receiveBuffer));
00202 datagram_pub_.publish(datagram_msg);
00203 }
00204
00205 sensor_msgs::LaserScan msg;
00206 int success = parser_->parse_datagram((char*)receiveBuffer, (size_t)actual_length, config_, msg);
00207 if (success == EXIT_SUCCESS)
00208 diagnosticPub_->publish(msg);
00209
00210 return EXIT_SUCCESS;
00211 }
00212
00213 void SickTimCommon::check_angle_range(SickTimConfig &conf)
00214 {
00215 if (conf.min_ang > conf.max_ang)
00216 {
00217 ROS_WARN("Minimum angle must be greater than maximum angle. Adjusting min_ang.");
00218 conf.min_ang = conf.max_ang;
00219 }
00220 }
00221
00222 void SickTimCommon::update_config(sick_tim::SickTimConfig &new_config, uint32_t level)
00223 {
00224 check_angle_range(new_config);
00225 config_ = new_config;
00226 }
00227
00228 }