Skip to content
New issue

Have a question about this project? # for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “#”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? # to your account

chore: satisfy roslint #29

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 41 additions & 43 deletions include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,62 +38,60 @@
* Author: Paul Bovbel
*/

#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET

#include "ros/ros.h"
#include "boost/thread/mutex.hpp"

#include "nodelet/nodelet.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "sensor_msgs/PointCloud2.h"

#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET_H
#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET_H

#include <boost/thread/mutex.hpp>
#include <message_filters/subscriber.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <tf2_ros/buffer.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>

namespace pointcloud_to_laserscan
{
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
/**
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
* pointcloud_to_laserscan implementation.
*/
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
{

public:
PointCloudToLaserScanNodelet();
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
{
public:
PointCloudToLaserScanNodelet();

private:
virtual void onInit();
private:
virtual void onInit();

void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason);
void cloudCb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
void failureCb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason);

void connectCb();
void connectCb();

void disconnectCb();
void disconnectCb();

ros::NodeHandle nh_, private_nh_;
ros::Publisher pub_;
boost::mutex connect_mutex_;
ros::NodeHandle nh_, private_nh_;
ros::Publisher pub_;
boost::mutex connect_mutex_;

boost::shared_ptr<tf2_ros::Buffer> tf2_;
boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
boost::shared_ptr<MessageFilter> message_filter_;
boost::shared_ptr<tf2_ros::Buffer> tf2_;
boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
boost::shared_ptr<MessageFilter> message_filter_;

// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;
double tolerance_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_;
double inf_epsilon_;
};
// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;
double tolerance_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_;
double inf_epsilon_;
};

} // pointcloud_to_laserscan
} // namespace pointcloud_to_laserscan

#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET_H
14 changes: 9 additions & 5 deletions src/pointcloud_to_laserscan_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,12 @@
* Author: Paul Bovbel
*/

#include <ros/ros.h>
#include <nodelet/loader.h>
#include <ros/ros.h>
#include <string>

int main(int argc, char **argv){
int main(int argc, char** argv)
{
ros::init(argc, argv, "pointcloud_to_laserscan_node");
ros::NodeHandle private_nh("~");
int concurrency_level;
Expand All @@ -54,12 +56,14 @@ int main(int argc, char **argv){
nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv);

boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
if(concurrency_level) {
if (concurrency_level)
{
spinner.reset(new ros::MultiThreadedSpinner(concurrency_level));
}else{
}
else
{
spinner.reset(new ros::MultiThreadedSpinner());
}
spinner->spin();
return 0;

}
Loading