Skip to content

Commit

Permalink
Format code according to the PCL style guide
Browse files Browse the repository at this point in the history
  • Loading branch information
@samontab
samontab committed Nov 3, 2014
1 parent b2a9663 commit ba2ce7a
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 69 deletions.
14 changes: 7 additions & 7 deletions io/include/pcl/io/robot_eye_grabber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand All @@ -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);
};
}

Expand Down
118 changes: 56 additions & 62 deletions io/src/robot_eye_grabber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,60 +154,56 @@ pcl::RobotEyeGrabber::consumerThreadLoop ()
boost::shared_array<unsigned char> 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))
{
point_cloud_xyzi_->push_back (xyzi);
}
}


if (point_cloud_xyzi_->size () > signal_point_cloud_size_)
{
if (point_cloud_signal_->num_slots () > 0)
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -255,23 +251,23 @@ 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;
}



/////////////////////////////////////////////////////////////////////////////
void
pcl::RobotEyeGrabber::socketThreadLoop()
pcl::RobotEyeGrabber::socketThreadLoop ()
{
asyncSocketReceive();
io_service_.reset();
Expand All @@ -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<unsigned char>(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<unsigned char>(dup));
}

asyncSocketReceive ();
Expand All @@ -312,7 +307,7 @@ pcl::RobotEyeGrabber::socketCallback(const boost::system::error_code&, std::size
void
pcl::RobotEyeGrabber::start ()
{
resetPointCloud ();
resetPointCloud ();

if (isRunning ())
return;
Expand All @@ -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;
}

Expand All @@ -342,7 +337,6 @@ pcl::RobotEyeGrabber::stop ()
if (!isRunning ())
return;


terminate_thread_ = true;

socket_->close ();
Expand Down

0 comments on commit ba2ce7a

Please sign in to comment.