diff --git a/misc/float_array_to_pc2/CMakeLists.txt b/misc/float_array_to_pc2/CMakeLists.txt new file mode 100644 index 00000000..8cb0a908 --- /dev/null +++ b/misc/float_array_to_pc2/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.0.2) +project(float_array_to_pc2) + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs cv pcl_conversions) + +catkin_package(CATKIN_DEPENDS roscpp sensor_msgs cv pcl_conversions) + +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable(float_array_to_pc2_node src/float_array_to_pc2_node.cpp) +target_link_libraries(float_array_to_pc2_node ${catkin_LIBRARIES}) + +install(TARGETS float_array_to_pc2_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/misc/float_array_to_pc2/package.xml b/misc/float_array_to_pc2/package.xml new file mode 100644 index 00000000..398fd5f6 --- /dev/null +++ b/misc/float_array_to_pc2/package.xml @@ -0,0 +1,16 @@ + + + float_array_to_pc2 + 0.0.0 + The float_array_to_pc2 package + + kajanan + + TODO + + catkin + roscpp + sensor_msgs + cv + pcl_conversions + diff --git a/misc/float_array_to_pc2/src/float_array_to_pc2_node.cpp b/misc/float_array_to_pc2/src/float_array_to_pc2_node.cpp new file mode 100644 index 00000000..74075dd2 --- /dev/null +++ b/misc/float_array_to_pc2/src/float_array_to_pc2_node.cpp @@ -0,0 +1,56 @@ +#include "cv/FloatArray.h" +#include "geometry_msgs/Point.h" +#include "pcl_conversions/pcl_conversions.h" +#include "ros/node_handle.h" +#include "ros/publisher.h" +#include "ros/ros.h" +#include "ros/subscriber.h" +#include "sensor_msgs/PointCloud2.h" + +class FloatArrayToPointCloud2Node +{ + public: + FloatArrayToPointCloud2Node(ros::NodeHandle nh); + void floatArraycallback(const cv::FloatArray::ConstPtr &msg); + + private: + ros::NodeHandle nh_; + ros::Subscriber float_array_sub_; + ros::Publisher points2_pub_; +}; + +FloatArrayToPointCloud2Node::FloatArrayToPointCloud2Node(ros::NodeHandle nh) + : nh_{nh}, float_array_sub_{nh_.subscribe( + "/cv/lane_detections", 10, + &FloatArrayToPointCloud2Node::floatArraycallback, this)}, + points2_pub_{nh_.advertise( + "/cv/lane_detections_cloud", 1)} +{ +} + +void FloatArrayToPointCloud2Node::floatArraycallback( + const cv::FloatArray::ConstPtr &msg) +{ + pcl::PointCloud output_cloud{}; + for (const cv::FloatList &list : msg->lists) { + for (const geometry_msgs::Point &element : list.elements) { + pcl::PointXYZ point = pcl::PointXYZ(static_cast(element.x), + static_cast(element.y), + static_cast(element.z)); + output_cloud.push_back(point); + } + } + + sensor_msgs::PointCloud2 output_msg; + pcl::toROSMsg(output_cloud, output_msg); + points2_pub_.publish(output_msg); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "float_array_to_pc2_node"); + ros::NodeHandle nh("~"); + FloatArrayToPointCloud2Node float_array_to_pc2_node(nh); + ros::spin(); + return 0; +}