Skip to content

Commit

Permalink
perf: reset the terminal debug infomation
Browse files Browse the repository at this point in the history
  • Loading branch information
@steelwu
steelwu committed Jun 4, 2019
1 parent 5bfcd4b commit f9612a1
Show file tree
Hide file tree
Showing 7 changed files with 118 additions and 102 deletions.
62 changes: 40 additions & 22 deletions rslidar_driver/src/input.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,9 @@ Input::Input(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh

private_nh.param("device_ip", devip_str_, std::string(""));
if (!devip_str_.empty())
ROS_INFO_STREAM("Only accepting packets from IP address: " << devip_str_);
{
ROS_INFO_STREAM("[driver][input] accepting packets from IP address: " << devip_str_);
}
}

int Input::getRpm(void)
Expand Down Expand Up @@ -87,18 +89,18 @@ InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(priv
inet_aton(devip_str_.c_str(), &devip_);
}

ROS_INFO_STREAM("Opening UDP socket: port " << port);
ROS_INFO_STREAM("[driver][socket] Opening UDP socket: port " << port);
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
if (sockfd_ == -1)
{
perror("socket"); // TODO: ROS_ERROR errno
ROS_ERROR("[driver][socket] create socket fail");
return;
}

int opt = 1;
if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&opt, sizeof(opt)))
{
perror("setsockopt error!\n");
ROS_ERROR("[driver][socket] setsockopt fail");
return;
}

Expand All @@ -110,13 +112,13 @@ InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(priv

if (bind(sockfd_, (sockaddr*)&my_addr, sizeof(sockaddr)) == -1)
{
perror("bind"); // TODO: ROS_ERROR errno
ROS_ERROR("[driver][socket] socket bind fail");
return;
}

if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
perror("non-block");
ROS_ERROR("[driver][socket] fcntl fail");
return;
}
}
Expand Down Expand Up @@ -149,12 +151,14 @@ int InputSocket::getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_o
if (retval < 0) // poll() error?
{
if (errno != EINTR)
ROS_ERROR("poll() error: %s", strerror(errno));
{
ROS_ERROR("[driver][socket] poll() error: %s", strerror(errno));
}
return 1;
}
if (retval == 0) // poll() timeout?
{
ROS_WARN("Rslidar poll() timeout");
ROS_WARN("[driver][socket] Rslidar poll() timeout");

char buffer_data[8] = "re-con";
memset(&sender_address, 0, sender_address_len); // initialize to zeros
Expand All @@ -166,7 +170,7 @@ int InputSocket::getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_o
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
{
ROS_ERROR("poll() reports Rslidar error");
ROS_ERROR("[driver][socket] poll() reports Rslidar error");
return 1;
}
} while ((fds[0].revents & POLLIN) == 0);
Expand All @@ -176,20 +180,23 @@ int InputSocket::getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_o
{
if (errno != EWOULDBLOCK)
{
perror("recvfail");
ROS_INFO("recvfail");
ROS_ERROR("[driver][socket] recvfail");
return 1;
}
}
else if ((size_t)nbytes == packet_size)
{
if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
{
continue;
}
else
{
break; // done
}
}

ROS_DEBUG_STREAM("incomplete rslidar packet read: " << nbytes << " bytes");
ROS_WARN_STREAM("[driver][socket] incomplete rslidar packet read: " << nbytes << " bytes");
}
if (flag == 0)
{
Expand Down Expand Up @@ -250,18 +257,24 @@ InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_ra
private_nh.param("repeat_delay", repeat_delay_, 0.0);

if (read_once_)
ROS_INFO("Read input file only once.");
{
ROS_INFO("[driver][pcap] Read input file only once.");
}
if (read_fast_)
ROS_INFO("Read input file as quickly as possible.");
{
ROS_INFO("[driver][pcap] Read input file as quickly as possible.");
}
if (repeat_delay_ > 0.0)
ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
{
ROS_INFO("[driver][pcap] Delay %.3f seconds before repeating input file.", repeat_delay_);
}

// Open the PCAP dump file
// ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
ROS_INFO_STREAM("Opening PCAP file " << filename_);
ROS_INFO_STREAM("[driver][pcap] Opening PCAP file " << filename_);
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
{
ROS_FATAL("Error opening rslidar socket dump file.");
ROS_FATAL("[driver][pcap] Error opening rslidar socket dump file.");
return;
}

Expand All @@ -271,7 +284,12 @@ InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_ra
filter << "src host " << devip_str_ << " && ";
}
filter << "udp dst port " << port;
pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
int ret = pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
if (ret < 0)
{
ROS_FATAL_STREAM("[driver][pcap] pcap compile filter fail. filter: "<<filter.str());
return;
}
}

/** destructor */
Expand Down Expand Up @@ -334,23 +352,23 @@ int InputPCAP::getPacket(rslidar_msgs::rslidarPacket* pkt, const double time_off

if (empty_) // no data in file?
{
ROS_WARN("Error %d reading rslidar packet: %s", res, pcap_geterr(pcap_));
ROS_ERROR("[driver][pcap] Error %d reading rslidar packet: %s", res, pcap_geterr(pcap_));
return -1;
}

if (read_once_)
{
ROS_INFO("end of file reached -- done reading.");
ROS_INFO("[driver][pcap] end of file reached -- done reading.");
return -1;
}

if (repeat_delay_ > 0.0)
{
ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
ROS_INFO("[driver][pcap] end of file reached -- delaying %.3f seconds.", repeat_delay_);
usleep(rint(repeat_delay_ * 1000000.0));
}

ROS_DEBUG("replaying rslidar dump file");
ROS_INFO("[driver][pcap] replaying rslidar dump file");

// I can't figure out how to rewind the file, because it
// starts with some kind of header. So, close the file
Expand Down
4 changes: 2 additions & 2 deletions rslidar_driver/src/nodelet.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,10 @@ class DriverNodelet : public nodelet::Nodelet
{
if (running_)
{
NODELET_INFO("shutting down driver thread");
NODELET_INFO("[driver][nodelet] shutting down driver thread");
running_ = false;
deviceThread_->join();
NODELET_INFO("driver thread stopped");
NODELET_INFO("[driver][nodelet] sdriver thread stopped");
}
}

Expand Down
19 changes: 10 additions & 9 deletions rslidar_driver/src/rsdriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ rslidarDriver::rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh)
}
else
{
ROS_ERROR_STREAM("unknown LIDAR model: " << config_.model);
ROS_ERROR_STREAM("[driver] unknown LIDAR model: " << config_.model);
packet_rate = 2600.0;
}
std::string deviceName(std::string("Robosense ") + model_full_name);
Expand All @@ -61,7 +61,7 @@ rslidarDriver::rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh)

int npackets = (int)ceil(packet_rate / frequency);
private_nh.param("npackets", config_.npackets, npackets);
ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
ROS_INFO_STREAM("[driver] publishing " << config_.npackets << " packets per scan");

std::string dump_file;
private_nh.param("pcap", dump_file, std::string(""));
Expand All @@ -75,17 +75,17 @@ rslidarDriver::rslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh)
private_nh.param("cut_angle", cut_angle, -0.01);
if (cut_angle < 0.0)
{
ROS_INFO_STREAM("Cut at specific angle feature deactivated.");
ROS_INFO_STREAM("[driver] Cut at specific angle feature deactivated.");
}
else if (cut_angle < 360)
{
ROS_INFO_STREAM("Cut at specific angle feature activated. "
ROS_INFO_STREAM("[driver] Cut at specific angle feature activated. "
"Cutting rslidar points always at "
<< cut_angle << " degree.");
}
else
{
ROS_ERROR_STREAM("cut_angle parameter is out of range. Allowed range is "
ROS_ERROR_STREAM("[driver] cut_angle parameter is out of range. Allowed range is "
<< "between 0.0 and 360 negative values to deactivate this feature.");
cut_angle = -0.01;
}
Expand Down Expand Up @@ -211,6 +211,8 @@ bool rslidarDriver::poll(void)
config_.npackets = ceil(packets_rate*60/config_.rpm);

difop_input_->clearUpdateFlag();

ROS_INFO_STREAM("[driver] update npackets. rpm: "<<config_.rpm<<", npkts: "<<config_.npackets);
}
scan->packets.resize(config_.npackets);
// use in standard behaviour only
Expand Down Expand Up @@ -265,7 +267,7 @@ bool rslidarDriver::poll(void)
}

// publish message using time of last packet read
ROS_DEBUG("Publishing a full rslidar scan.");
// ROS_DEBUG("[driver] Publishing a full rslidar scan.");
scan->header.stamp = scan->packets.back().stamp;
scan->header.frame_id = config_.frame_id;
msop_output_.publish(scan);
Expand All @@ -288,8 +290,7 @@ void rslidarDriver::difopPoll(void)
int rc = difop_input_->getPacket(&difop_packet_msg, config_.time_offset);
if (rc == 0)
{
// std::cout << "Publishing a difop data." << std::endl;
ROS_DEBUG("Publishing a difop data.");
// ROS_DEBUG("[driver] Publishing a difop data.");
*difop_packet_ptr = difop_packet_msg;
difop_output_.publish(difop_packet_ptr);
}
Expand All @@ -301,7 +302,7 @@ void rslidarDriver::difopPoll(void)

void rslidarDriver::callback(rslidar_driver::rslidarNodeConfig& config, uint32_t level)
{
ROS_INFO("Reconfigure Request");
// ROS_INFO("[driver] Reconfigure Request");
config_.time_offset = config.time_offset;
}

Expand Down
2 changes: 1 addition & 1 deletion rslidar_pointcloud/src/convert.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new r

void Convert::callback(rslidar_pointcloud::CloudNodeConfig& config, uint32_t level)
{
ROS_INFO("Reconfigure Request");
// ROS_INFO("[cloud][convert] Reconfigure Request");
// config_.time_offset = config.time_offset;
}

Expand Down
Loading

0 comments on commit f9612a1

Please sign in to comment.