Skip to content

Commit

Permalink
Merge pull request ros-drivers#94 from ros-drivers/pcap_port
Browse files Browse the repository at this point in the history 
velodyne_driver: use port number for PCAP data (ros-drivers#46, ros-drivers#66)
  • Loading branch information
@jack-oquin
jack-oquin committed Jun 16, 2016
2 parents 2887506 + 075cb76 commit 618a73a
Show file tree
Hide file tree
Showing 3 changed files with 90 additions and 76 deletions.
39 changes: 20 additions & 19 deletions velodyne_driver/include/velodyne_driver/input.h
View file
Expand Up @@ -2,6 +2,7 @@
*
* Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
* Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
* Copyright (C) 2015, Jack O'Quin
*
* License: Modified BSD Software License Agreement
*
Expand All @@ -18,7 +19,7 @@
*
* Classes:
*
* velodyne::Input -- pure virtual base class to access the data
* velodyne::Input -- base class for accessing the data
* independently of its source
*
* velodyne::InputSocket -- derived class reads live data from the
Expand All @@ -41,13 +42,14 @@

namespace velodyne_driver
{
static uint16_t UDP_PORT_NUMBER = 2368;
static uint16_t DATA_PORT_NUMBER = 2368; // default data port
static uint16_t POSITION_PORT_NUMBER = 8308; // default position port

/** @brief Pure virtual Velodyne input base class */
/** @brief Velodyne input base class */
class Input
{
public:
Input() {}
Input(ros::NodeHandle private_nh, uint16_t port);
virtual ~Input() {}

/** @brief Read one Velodyne packet.
Expand All @@ -58,15 +60,12 @@ namespace velodyne_driver
* -1 if end of file
* > 0 if incomplete packet (is this possible?)
*/
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset) = 0;
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
const double time_offset) = 0;


/** @brief Set source IP, from where packets are accepted
*
* @param ip IP of a Velodyne LIDAR e.g. 192.168.51.70
*/
virtual void setDeviceIP( const std::string& ip ) { devip_str_ = ip; }
protected:
ros::NodeHandle private_nh_;
uint16_t port_;
std::string devip_str_;
};

Expand All @@ -75,13 +74,15 @@ namespace velodyne_driver
{
public:
InputSocket(ros::NodeHandle private_nh,
uint16_t udp_port = UDP_PORT_NUMBER);
uint16_t port = DATA_PORT_NUMBER);
virtual ~InputSocket();

virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset);
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
const double time_offset);
void setDeviceIP( const std::string& ip );
private:

private:
int sockfd_;
in_addr devip_;
};
Expand All @@ -96,28 +97,28 @@ namespace velodyne_driver
{
public:
InputPCAP(ros::NodeHandle private_nh,
double packet_rate,
uint16_t port = DATA_PORT_NUMBER,
double packet_rate = 0.0,
std::string filename="",
bool read_once=false,
bool read_fast=false,
double repeat_delay=0.0);
virtual ~InputPCAP();

virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset);
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
const double time_offset);
void setDeviceIP( const std::string& ip );

private:

ros::Rate packet_rate_;
std::string filename_;
FILE *fp_;
pcap_t *pcap_;
bpf_program pcap_packet_filter_;
char errbuf_[PCAP_ERRBUF_SIZE];
bool empty_;
bool read_once_;
bool read_fast_;
double repeat_delay_;
ros::Rate packet_rate_;
bpf_program velodyne_pointdata_filter_;
};

} // velodyne_driver namespace
Expand Down
22 changes: 9 additions & 13 deletions velodyne_driver/src/driver/driver.cc
View file
Expand Up @@ -79,7 +79,7 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
private_nh.param("pcap", dump_file, std::string(""));

int udp_port;
private_nh.param("port", udp_port, (int)UDP_PORT_NUMBER);
private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER);

// Initialize dynamic reconfigure
srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_driver::
Expand All @@ -104,25 +104,21 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
TimeStampStatusParam()));

// open Velodyne input device or file
if (dump_file != "")
if (dump_file != "") // have PCAP file?
{
input_.reset(new velodyne_driver::InputPCAP(private_nh,
packet_rate,
dump_file));
// read data from packet capture file
input_.reset(new velodyne_driver::InputPCAP(private_nh, udp_port,
packet_rate, dump_file));
}
else
{
// read data from live socket
input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port));
}

std::string devip;
private_nh.param("device_ip", devip, std::string(""));
if(!devip.empty())
ROS_INFO_STREAM("Set device ip to " << devip << ", only accepting packets from this address." );
input_->setDeviceIP(devip);

// raw data output topic
output_ = node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
// raw packet output topic
output_ =
node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
}

/** poll the device
Expand Down
105 changes: 61 additions & 44 deletions velodyne_driver/src/lib/input.cc
View file
@@ -1,6 +1,7 @@
/*
* Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
* Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
* Copyright (C) 2015, Jack O'Quin
*
* License: Modified BSD Software License Agreement
*
Expand All @@ -11,8 +12,8 @@
*
* Input classes for the Velodyne HDL-64E 3D LIDAR:
*
* Input -- virtual base class than can be used to access the data
* independently of its source
* Input -- base class used to access the data independently of
* its source
*
* InputSocket -- derived class reads live data from the device
* via a UDP socket
Expand All @@ -22,6 +23,8 @@
*/

#include <unistd.h>
#include <string>
#include <sstream>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <poll.h>
Expand All @@ -32,24 +35,44 @@

namespace velodyne_driver
{
static const size_t packet_size = sizeof(velodyne_msgs::VelodynePacket().data);
static const size_t packet_size =
sizeof(velodyne_msgs::VelodynePacket().data);

////////////////////////////////////////////////////////////////////////
// Input base class implementation
////////////////////////////////////////////////////////////////////////

/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number.
*/
Input::Input(ros::NodeHandle private_nh, uint16_t port):
private_nh_(private_nh),
port_(port)
{
private_nh.param("device_ip", devip_str_, std::string(""));
if (!devip_str_.empty())
ROS_INFO_STREAM("Only accepting packets from IP address: "
<< devip_str_);
}

////////////////////////////////////////////////////////////////////////
// InputSocket class implementation
////////////////////////////////////////////////////////////////////////

/** @brief constructor
*
* @param private_nh private node handle for driver
* @param udp_port UDP port number to connect
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
*/
InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t udp_port):
Input()
InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port):
Input(private_nh, port)
{
sockfd_ = -1;

// connect to Velodyne UDP port
ROS_INFO_STREAM("Opening UDP socket: port " << udp_port);
ROS_INFO_STREAM("Opening UDP socket: port " << port);
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
if (sockfd_ == -1)
{
Expand All @@ -60,7 +83,7 @@ namespace velodyne_driver
sockaddr_in my_addr; // my address information
memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
my_addr.sin_family = AF_INET; // host byte order
my_addr.sin_port = htons(udp_port); // short, in network byte order
my_addr.sin_port = htons(port); // port in network byte order
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP

if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
Expand All @@ -84,12 +107,6 @@ namespace velodyne_driver
(void) close(sockfd_);
}

void InputSocket::setDeviceIP(const std::string &ip)
{
devip_str_ = ip;
inet_aton(ip.c_str(),&devip_);
}

/** @brief Get one velodyne packet. */
int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
{
Expand Down Expand Up @@ -150,7 +167,8 @@ namespace velodyne_driver
// socket using a blocking read.
ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
packet_size, 0,
(sockaddr*) &sender_address, &sender_address_len);
(sockaddr*) &sender_address,
&sender_address_len);

if (nbytes < 0)
{
Expand All @@ -166,7 +184,8 @@ namespace velodyne_driver
// read successful,
// if packet is not from the lidar scanner we selected by IP,
// continue otherwise we are done
if( devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr )
if(devip_str_ != ""
&& sender_address.sin_addr.s_addr != devip_.s_addr)
continue;
else
break; //done
Expand All @@ -190,31 +209,25 @@ namespace velodyne_driver

/** @brief constructor
*
* @param private_nh private node handle for driver
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
* @param packet_rate expected device packet frequency (Hz)
* @param filename PCAP dump file name
* @param read_once read PCAP in a loop, unless false
* @param read_fast read PCAP at device rate, unless false
* @param repeat_delay time to wait before repeating PCAP data
*/
InputPCAP::InputPCAP(ros::NodeHandle private_nh,
double packet_rate,
std::string filename,
bool read_once,
bool read_fast,
double repeat_delay):
Input(),
packet_rate_(packet_rate)
InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port,
double packet_rate, std::string filename,
bool read_once, bool read_fast, double repeat_delay):
Input(private_nh, port),
packet_rate_(packet_rate),
filename_(filename)
{
filename_ = filename;
fp_ = NULL;
pcap_ = NULL;
empty_ = true;

// get parameters using private node handle
private_nh.param("read_once", read_once_, read_once);
private_nh.param("read_fast", read_fast_, read_fast);
private_nh.param("repeat_delay", repeat_delay_, repeat_delay);
private_nh.param("read_once", read_once_, false);
private_nh.param("read_fast", read_fast_, false);
private_nh.param("repeat_delay", repeat_delay_, 0.0);

if (read_once_)
ROS_INFO("Read input file only once.");
Expand All @@ -231,22 +244,23 @@ namespace velodyne_driver
ROS_FATAL("Error opening Velodyne socket dump file.");
return;
}
}

std::stringstream filter;
if( devip_str_ != "" ) // using specific IP?
{
filter << "src host " << devip_str_ << " && ";
}
filter << "udp dst port " << port;
pcap_compile(pcap_, &pcap_packet_filter_,
filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
}

/** destructor */
InputPCAP::~InputPCAP(void)
{
pcap_close(pcap_);
}

void InputPCAP::setDeviceIP(const std::string &ip)
{
std::string filter_str = "src host " + devip_str_ + " && udp src port 2368 && udp dst port 2368";
if( devip_str_ != "" )
pcap_compile(pcap_, &velodyne_pointdata_filter_, filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN);
}

/** @brief Get one velodyne packet. */
int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
{
Expand All @@ -258,8 +272,11 @@ namespace velodyne_driver
int res;
if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
{
// if packet is not from the lidar scanner we selected by IP, continue
if( !devip_str_.empty() && (pcap_offline_filter( &velodyne_pointdata_filter_, header, pkt_data ) == 0) )
// Skip packets not for the correct port and from the
// selected IP address.
if (!devip_str_.empty() &&
(0 == pcap_offline_filter(&pcap_packet_filter_,
header, pkt_data)))
continue;

// Keep the reader from blowing through the file.
Expand Down

0 comments on commit 618a73a

Please sign in to comment.