Feature/pointcloud library

usv_perception/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(usv_perception)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -14,10 +14,13 @@ rospy
 std_msgs
 geometry_msgs
 sensor_msgs
+genmsg
 message_generation
 )
 find_package(OpenCV 3 REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
+find_package(PCL REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -110,9 +113,9 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+# INCLUDE_DIRS src/pcl_lib
 #  LIBRARIES usv_perception
-#  CATKIN_DEPENDS other_catkin_pkg
+#  CATKIN_DEPENDS 
 #  DEPENDS system_lib
 )
 
@@ -123,14 +126,14 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/usv_perception.cpp
-# )
+add_library(pcl_lib
+   include/lidar_pcl.cpp include/lidar_pcl.h
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -211,3 +214,7 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+add_executable(lidar_pcl src/lidar_pcl.cpp)
+target_link_libraries(lidar_pcl ${catkin_LIBRARIES} ${PCL_LIBRARIES} pcl_lib)
+add_dependencies(lidar_pcl usv_perception_generate_messages_cpp)

usv_perception/include/lidar_pcl.cpp

@@ -0,0 +1,70 @@
+// ***********************************************************************
+/*!
+ *  \file    lidar_pointcloud.cpp
+ *  \brief   Implementation of hooks to Velodyne data for mover detection, 
+ *           and estimation and removal of ground elevation. Simple cannonical
+ *           code for exploratory purposes...
+ *  \date    March 27, 2020
+ *  \author  Ivana Collado González
+ *
+ *  This node reads Velodyne data; converts it to a local frame; 
+ *
+ *  Ver. 1.0
+ */
+// ***********************************************************************
+
+// INCLUDES --------------------------------------------------------------------
+#include "lidar_pcl.h"
+
+
+// CLASS ----------------------------------------------------------------------
+Lidar::Lidar(
+	const std::string &lidar_sub,
+  const std::string &obstacles_pub,
+  const std::string &pcl_pub, 
+  const int &queue_size)
+{
+  lidar_sub_ = lidar_node_.subscribe(lidar_sub, queue_size, 
+    &Lidar::LidarCallback, this);
+	obstacles_pub_ = lidar_node_.advertise<usv_perception::obstacles_list>(
+	  obstacles_pub, queue_size);
+  pcl_pub_ = lidar_node_.advertise<pcl::PointCloud<pcl::PointXYZ>>(
+	  pcl_pub, queue_size);
+  ROS_INFO("Lidar is ready");
+}
+
+// FUNCTIONS -------------------------------------------------------------------
+
+void Lidar::LidarCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &input) {
+  *cloud = *input;
+  //cloud.header = input.header
+}
+
+void Lidar::DetectObstacles(){
+  PassThrough();
+}
+
+void Lidar::PassThrough(){
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PassThrough<pcl::PointXYZ> pass;
+  pass.setInputCloud(cloud);
+  pass.setFilterFieldName ("z");
+  pass.setFilterLimits (0.0, 1.0);
+  pass.filter (*cloud_filtered);
+  *cloud_filtered.header.frame_id = "/velodyne";
+  //cloud_filtered.header = cloud.header
+  pcl_pub_.publish(cloud_filtered);
+}
+
+// FORWARD DECLARATIONS --------------------------------------------------------
+
+// TYPEDEFS AND DEFINES --------------------------------------------------------
+
+// ENUMS -----------------------------------------------------------------------
+
+// STRUCTS ---------------------------------------------------------------------
+
+// NON-CLASS FUNCTIONS ---------------------------------------------------------
+
+// CLASS FUNCTION IMPLEMENTATION  ----------------------------------------------
+

usv_perception/include/lidar_pcl.h

@@ -0,0 +1,77 @@
+/**
+ * @file: lidar_pointcloud.h
+ * @author: Ivana Collado
+ * @date: March 27, 2020
+ *
+ * @brief: Defines a class that will recueve and manipulate lidar pointcloud
+ */
+
+// INCLUDES --------------------------------------------------------------------
+// ROS
+#include <ros/ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <usv_perception/obstacles_list.h>
+
+// PCL
+#include <pcl/point_cloud.h>
+//#include <pcl/point_types.h>
+//#include <pcl_ros/transforms.h>
+//#include <pcl_ros/transforms.h>
+//#include <pcl_conversions/pcl_conversions.h>
+//#include <pcl_ros/pcl_conversions.h>
+//#include <pcl/conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/filters/passthrough.h>
+//#include <pcl/PCLPointCloud2.h>
+
+
+class Lidar {
+public:
+	/**
+	 * Constructor. 
+	 * @param obstacles_pub[in]: Publisher's topic name. 
+	 * @param lidar_sub[in]: Subscriber's topic name. 
+	 * @param queue_size[in]: Message queue size.
+	 */
+	Lidar(
+		const std::string &lidar_sub = "/velodyne_pcl",
+		const std::string &obstacles_pub = "/usv_perception/lidar_detector/obstacles",
+		const std::string &pcl_pub = "/usv_perception/lidar_detector/filtered_pcl",
+		const int &queue_size = 10
+	);
+
+	/** 
+	 * Callback to process point cloud. 
+	 * @param input[in]: Received pointcloud.  
+	 */
+	void LidarCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &input);
+
+	/** 
+	 * Filters only ROI 
+	 */
+	void DetectObstacles();
+
+private:
+
+	/** 
+	 * Filters only ROI 
+	 */
+	void PassThrough();
+
+	// ROS Node 
+	ros::NodeHandle lidar_node_;
+
+	// Publishers to Lidar PC.
+  ros::Subscriber lidar_sub_;
+	// Publishers for obsacle list.
+  ros::Publisher obstacles_pub_;
+	ros::Publisher pcl_pub_;
+
+	usv_perception::obstacles_list list_;
+	geometry_msgs::Vector3 obstacle_;
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+};

usv_perception/src/lidar_pcl.cpp

@@ -0,0 +1,22 @@
+/**
+ * @file: lidar_pcl.cpp
+ * @author: Ivana Collado
+ * @date: March 27, 2020
+ *
+ * @brief: 
+ */
+#include "lidar_pcl.h"
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "lidar_detector");
+	Lidar lidar_pcl;
+	ros::Rate loop_rate(100);
+  while (ros::ok())
+  { 
+    lidar_pcl.DetectObstacles();
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+	return 0;
+}