From a36ab152a8c4b9e4c223e6fa169d54c24f064833 Mon Sep 17 00:00:00 2001
From: ShanoWhite <48310776+ShanoWhite@users.noreply.github.com>
Date: Wed, 30 Oct 2019 14:48:58 +0800
Subject: [PATCH] PointCloud2
I write a c++ program to publish a pointcloud2 message,data of points is from the folder named "point/dream....txt"
---
pc/CMakeLists.txt | 214 +
pc/package.xml | 73 +
pc/src/listener.cpp | 23 +
pc/src/talker.cpp | 80 +
point/dream1.txt | 4614 ++++++++++++
point/dream10.txt | 7680 +++++++++++++++++++
point/dream11.txt | 6842 +++++++++++++++++
point/dream2.txt | 4085 ++++++++++
point/dream3.txt | 13337 +++++++++++++++++++++++++++++++++
point/dream4.txt | 17031 ++++++++++++++++++++++++++++++++++++++++++
point/dream5.txt | 9738 ++++++++++++++++++++++++
point/dream6.txt | 7939 ++++++++++++++++++++
point/dream7.txt | 10043 +++++++++++++++++++++++++
point/dream8.txt | 7860 +++++++++++++++++++
point/dream9.txt | 3820 ++++++++++
15 files changed, 93379 insertions(+)
create mode 100644 pc/CMakeLists.txt
create mode 100644 pc/package.xml
create mode 100644 pc/src/listener.cpp
create mode 100644 pc/src/talker.cpp
create mode 100644 point/dream1.txt
create mode 100644 point/dream10.txt
create mode 100644 point/dream11.txt
create mode 100644 point/dream2.txt
create mode 100644 point/dream3.txt
create mode 100644 point/dream4.txt
create mode 100644 point/dream5.txt
create mode 100644 point/dream6.txt
create mode 100644 point/dream7.txt
create mode 100644 point/dream8.txt
create mode 100644 point/dream9.txt
diff --git a/pc/CMakeLists.txt b/pc/CMakeLists.txt
new file mode 100644
index 00000000..3475f449
--- /dev/null
+++ b/pc/CMakeLists.txt
@@ -0,0 +1,214 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(pc)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# sensor_msgs# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES pc
+ CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/pc.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/pc_node.cpp)
+ add_executable(talk src/talker.cpp)
+ add_executable(listen src/listener.cpp)
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+ target_link_libraries(talk
+ ${catkin_LIBRARIES}
+ )
+ target_link_libraries(listen
+ ${catkin_LIBRARIES}
+ )
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_pc.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/pc/package.xml b/pc/package.xml
new file mode 100644
index 00000000..1a7877b8
--- /dev/null
+++ b/pc/package.xml
@@ -0,0 +1,73 @@
+
+
+ pc
+ 0.0.0
+ The pc package
+
+
+
+
+ sean
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ message_generation
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ roscpp
+ rospy
+ sensor_msgs
+ std_msgs
+ message_runtime
+
+
+
+
+
+
+
+
diff --git a/pc/src/listener.cpp b/pc/src/listener.cpp
new file mode 100644
index 00000000..4a301d88
--- /dev/null
+++ b/pc/src/listener.cpp
@@ -0,0 +1,23 @@
+#include
+#include
+#include
+#include
+
+using namespace std;
+
+void callback(sensor_msgs::PointCloud2 msg)
+{
+ ROS_INFO("seq is %d,stamp is %.5f,frame_id is ",msg.header.seq,msg.header.stamp.toSec());
+ ROS_INFO("===============");
+ ROS_INFO("I heard :%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",msg.data[0],msg.data[1],msg.data[2],msg.data[3],msg.data[4],msg.data[5],msg.data[6],msg.data[7],msg.data[8],msg.data[9]);
+ //ROS_INFO("data is %d %d %d",msg.data[0],msg.data[1000],msg.data[500]);
+}
+
+int main(int argc,char** argv)
+{
+ ros::init(argc,argv,"listen");
+ ros::NodeHandle n;
+ ros::Subscriber listener=n.subscribe("points",100,callback);
+ ros::spin();
+ return 0;
+}
diff --git a/pc/src/talker.cpp b/pc/src/talker.cpp
new file mode 100644
index 00000000..8f3e6fed
--- /dev/null
+++ b/pc/src/talker.cpp
@@ -0,0 +1,80 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+using namespace std;
+
+int main(int argc,char** argv)
+{
+ ros::init(argc,argv,"talk");
+ ros::NodeHandle n;
+ ros::Publisher talker=n.advertise("points",100);
+ ros::Rate rate(1);
+
+ //pc.header.seq=111;
+ //pc.header.stamp=ros::Time::now();
+ //pc.header.frame_id="en?";
+ int count=0;//依次读如11个txt文件
+ while(count<12)
+ {
+ string filename="/home/sean/Desktop/point/dream";
+ filename=filename+to_string(count)+".txt";
+ ifstream in(filename);//将点集写入缓存
+ int partpoint;//一个数字接一个数字的存入vector
+ vector iv;
+ while(in>>partpoint)
+ {
+ iv.push_back(partpoint);
+ }
+
+ sensor_msgs::PointCloud2 pc;
+
+ pc.header.seq=count;
+ pc.header.stamp=ros::Time::now();//写入时间戳
+ pc.header.frame_id=filename;
+
+ pc.height=1;//为无序数组
+ pc.width=(iv.size()/2);//点的个数
+
+ pc.fields.resize(2);//Field数组含有两个成员，x与y
+
+ pc.fields[0].name="x";
+ pc.fields[0].offset=0;
+ pc.fields[0].datatype=5;//我认为txt里数字格式是uint8 INT32
+ pc.fields[0].count=1;//一个点内含有两个数字
+
+ pc.fields[1].name="y";
+ pc.fields[1].offset=0;
+ pc.fields[1].datatype=5;//我认为txt里数字格式是uint8 INT32
+ pc.fields[1].count=1;//一个点内含有两个数字
+
+
+ pc.is_bigendian=false;//数据不是夸大的
+ pc.point_step=32;//一个点占的比特数应该是32
+ pc.row_step=(32*iv.size()/2);//一行占的比特数应该是32*（txt文本中的数字个数）
+
+ pc.data.resize(iv.size());
+ //输入data中的数据，就是将vector中的数字依次存入
+ for(int i=0;i