diff --git a/io/include/pcl/io/robot_eye_grabber.h b/io/include/pcl/io/robot_eye_grabber.h index 1b7b267d996..c3c3950645e 100644 --- a/io/include/pcl/io/robot_eye_grabber.h +++ b/io/include/pcl/io/robot_eye_grabber.h @@ -123,9 +123,9 @@ namespace pcl bool terminate_thread_; size_t signal_point_cloud_size_; unsigned short data_port_; - enum { max_length = 65535 }; - unsigned char receive_buffer_[max_length]; - unsigned int dataSize; + enum { MAX_LENGTH = 65535 }; + unsigned char receive_buffer_[MAX_LENGTH]; + unsigned int data_size_; boost::asio::ip::address sensor_address_; boost::asio::ip::udp::endpoint sender_endpoint_; @@ -142,10 +142,10 @@ namespace pcl void socketThreadLoop (); void asyncSocketReceive (); void resetPointCloud (); - void socketCallback (const boost::system::error_code& error, std::size_t numberOfBytes); - void convertPacketData (unsigned char *dataPacket, size_t length); - void computeXYZI (pcl::PointXYZI& pointXYZI, unsigned char* pointData); - void computeTimestamp (boost::uint32_t& timestamp, unsigned char* pointData); + void socketCallback (const boost::system::error_code& error, std::size_t number_of_bytes); + void convertPacketData (unsigned char *data_packet, size_t length); + void computeXYZI (pcl::PointXYZI& point_XYZI, unsigned char* point_data); + void computeTimestamp (boost::uint32_t& timestamp, unsigned char* point_data); }; } diff --git a/io/src/robot_eye_grabber.cpp b/io/src/robot_eye_grabber.cpp index b033221486d..d96bd0628b0 100644 --- a/io/src/robot_eye_grabber.cpp +++ b/io/src/robot_eye_grabber.cpp @@ -154,52 +154,49 @@ pcl::RobotEyeGrabber::consumerThreadLoop () boost::shared_array data; if (!packet_queue_.dequeue (data)) return; - - //convertPacketData (data.get(), 464); - convertPacketData (data.get(), dataSize); + convertPacketData (data.get(), data_size_); } } ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::convertPacketData (unsigned char *dataPacket, size_t length) +pcl::RobotEyeGrabber::convertPacketData (unsigned char *data_packet, size_t length) { - //Check for the presence of the header - size_t offset = 0; - - //The old packet data format does not start with an E since it just starts with laser data - if(dataPacket[0] != 'E') + //Check for the presence of the header + size_t offset = 0; + //The old packet data format does not start with an E since it just starts with laser data + if(data_packet[0] != 'E') + { + //old packet data format (just laser data) + offset = 0; + } + else + { + //The new packet data format contains this as a header + //char[6] "EBRBEP" + //uint32_t Timestamp // counts of a 66 MHz clock since power-on of eye. + size_t response_size = 6; //"EBRBEP" + if( !strncmp((char*)(data_packet), "EBRBEP", response_size) ) { - //old packet data format (just laser data) - offset = 0; + boost::uint32_t timestamp; // counts of a 66 MHz clock since power-on of eye. + computeTimestamp(timestamp, data_packet + response_size); + //std::cout << "Timestamp: " << timestamp << std::endl; + offset = (response_size + sizeof(timestamp)); } else { - //The new packet data format contains this as a header - // char[6] "EBRBEP" - // uint32_t Timestamp // counts of a 66 MHz clock since power-on of eye. - size_t responseSize = 6; //"EBRBEP" - if( !strncmp((char*)(dataPacket), "EBRBEP", responseSize) ) - { - boost::uint32_t timestamp; // counts of a 66 MHz clock since power-on of eye. - computeTimestamp(timestamp, dataPacket + responseSize); - //std::cout << "Timestamp: " << timestamp << std::endl; - offset = (responseSize + sizeof(timestamp)); - } - else - { - //Invalid packet received, ignore it. - return; - } + //Invalid packet received, ignore it. + return; } + } - const size_t bytesPerPoint = 8; - const size_t totalPoints = (length - offset)/ bytesPerPoint; + const size_t bytes_per_point = 8; + const size_t total_points = (length - offset)/ bytes_per_point; - for (int i = 0; i < totalPoints; ++i) + for (int i = 0; i < total_points; ++i) { PointXYZI xyzi; - computeXYZI (xyzi, dataPacket + i*bytesPerPoint + offset); + computeXYZI (xyzi, data_packet + i*bytes_per_point + offset); if (pcl::isFinite(xyzi)) { @@ -207,7 +204,6 @@ pcl::RobotEyeGrabber::convertPacketData (unsigned char *dataPacket, size_t lengt } } - if (point_cloud_xyzi_->size () > signal_point_cloud_size_) { if (point_cloud_signal_->num_slots () > 0) @@ -219,7 +215,7 @@ pcl::RobotEyeGrabber::convertPacketData (unsigned char *dataPacket, size_t lengt ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* pointData) +pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* point_data) { uint16_t buffer = 0; double az = 0.0; @@ -228,23 +224,23 @@ pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* pointDa uint16_t intensity = 0; buffer = 0x00; - buffer = pointData[0] << 8; - buffer |= pointData[1]; // First 2-byte read will be Azimuth + buffer = point_data[0] << 8; + buffer |= point_data[1]; // First 2-byte read will be Azimuth az = (buffer / 100.0); buffer = 0x00; - buffer = pointData[2] << 8; - buffer |= pointData[3]; // Second 2-byte read will be Elevation + buffer = point_data[2] << 8; + buffer |= point_data[3]; // Second 2-byte read will be Elevation el = (signed short int)buffer / 100.0; buffer = 0x00; - buffer = pointData[4] << 8; - buffer |= pointData[5]; // Third 2-byte read will be Range + buffer = point_data[4] << 8; + buffer |= point_data[5]; // Third 2-byte read will be Range range = (signed short int)buffer / 100.0; buffer = 0x00; - buffer = pointData[6] << 8; - buffer |= pointData[7]; // Fourth 2-byte read will be Intensity + buffer = point_data[6] << 8; + buffer |= point_data[7]; // Fourth 2-byte read will be Intensity intensity = buffer; point.x = range * std::cos (el * M_PI/180) * std::sin (az * M_PI/180); @@ -255,15 +251,15 @@ pcl::RobotEyeGrabber::computeXYZI (pcl::PointXYZI& point, unsigned char* pointDa ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::computeTimestamp(boost::uint32_t& timestamp, unsigned char* pointData) +pcl::RobotEyeGrabber::computeTimestamp (boost::uint32_t& timestamp, unsigned char* point_data) { boost::uint32_t buffer = 0; buffer = 0x00; - buffer = pointData[0] << 24; - buffer |= pointData[1] << 16; - buffer |= pointData[2] << 8; - buffer |= pointData[3]; + buffer = point_data[0] << 24; + buffer |= point_data[1] << 16; + buffer |= point_data[2] << 8; + buffer |= point_data[3]; timestamp = buffer; } @@ -271,7 +267,7 @@ pcl::RobotEyeGrabber::computeTimestamp(boost::uint32_t& timestamp, unsigned char ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::socketThreadLoop() +pcl::RobotEyeGrabber::socketThreadLoop () { asyncSocketReceive(); io_service_.reset(); @@ -280,29 +276,28 @@ pcl::RobotEyeGrabber::socketThreadLoop() ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::asyncSocketReceive() +pcl::RobotEyeGrabber::asyncSocketReceive () { // expecting at most max_length bytes (UDP packet). - socket_->async_receive_from(boost::asio::buffer(receive_buffer_, max_length), sender_endpoint_, - boost::bind(&RobotEyeGrabber::socketCallback, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); + socket_->async_receive_from(boost::asio::buffer(receive_buffer_, MAX_LENGTH), sender_endpoint_, + boost::bind(&RobotEyeGrabber::socketCallback, this, boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); } ///////////////////////////////////////////////////////////////////////////// void -pcl::RobotEyeGrabber::socketCallback(const boost::system::error_code&, std::size_t numberOfBytes) +pcl::RobotEyeGrabber::socketCallback (const boost::system::error_code&, std::size_t number_of_bytes) { if (terminate_thread_) return; if (sensor_address_ == boost::asio::ip::address_v4::any () - || sensor_address_ == sender_endpoint_.address ()) + || sensor_address_ == sender_endpoint_.address ()) { - dataSize = numberOfBytes; - unsigned char *dup = new unsigned char[numberOfBytes]; - memcpy (dup, receive_buffer_, numberOfBytes); - packet_queue_.enqueue (boost::shared_array(dup)); + data_size_ = number_of_bytes; + unsigned char *dup = new unsigned char[number_of_bytes]; + memcpy (dup, receive_buffer_, number_of_bytes); + packet_queue_.enqueue (boost::shared_array(dup)); } asyncSocketReceive (); @@ -312,7 +307,7 @@ pcl::RobotEyeGrabber::socketCallback(const boost::system::error_code&, std::size void pcl::RobotEyeGrabber::start () { - resetPointCloud (); + resetPointCloud (); if (isRunning ()) return; @@ -321,11 +316,11 @@ pcl::RobotEyeGrabber::start () try { - socket_.reset (new boost::asio::ip::udp::socket (io_service_, destinationEndpoint)); + socket_.reset (new boost::asio::ip::udp::socket (io_service_, destinationEndpoint)); } catch (std::exception &e) { - PCL_ERROR ("[pcl::RobotEyeGrabber::start] Unable to bind to socket! %s\n", e.what ()); + PCL_ERROR ("[pcl::RobotEyeGrabber::start] Unable to bind to socket! %s\n", e.what ()); return; } @@ -342,7 +337,6 @@ pcl::RobotEyeGrabber::stop () if (!isRunning ()) return; - terminate_thread_ = true; socket_->close ();