满屏网站做多大尺寸,图片制作在线制作免费,菏泽哪里有做网站的,新昌建设局网站ros中发布点云数据xyz以及带颜色的点云数据xyzrgb ros中发布点云数据xyz可以直接用python来做或者C(看个人偏好) ros中发布带颜色的点云数据xyzrgb环境1.新建ROS工作空间2.创建功能包 ros中发布点云数据xyz
可以直接用python来做或者C(看个人偏好)
在这里我们带有颜色的点云数… ros中发布点云数据xyz以及带颜色的点云数据xyzrgb ros中发布点云数据xyz可以直接用python来做或者C(看个人偏好) ros中发布带颜色的点云数据xyzrgb环境1.新建ROS工作空间2.创建功能包 ros中发布点云数据xyz
可以直接用python来做或者C(看个人偏好)
在这里我们带有颜色的点云数据格式为x y z c 其中c值为float型有四种值1.0,2.0,3.0,4.0
代码文件b.py具体内容如下
import rospy
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge
import numpy as np
import os
import cv2
from a import *
import open3d as o3dDATA_PATH/home/cxh/project/0618/point_cloud_selected.txt
if __name____main__:rospy.init_node(jizhui_node,anonymousTrue)# cam_pubrospy.Publisher(kitti_cam,Image,queue_size10)pcl_pubrospy.Publisher(/jizhui_pcl,PointCloud2,queue_size1)#创建点云发布者queue_size10表示消息队列的大小bridgeCvBridge()#创建一个CvBridge实例用于在OpenCV图像与ROS图像消息之间进行转换# raterospy.Rate(1)#设置发布频率为10Hzraterospy.Rate(5,resetTrue)frame0#初始化帧计数器def load_point_cloud(file_path):从文本文件中加载点云数据point_cloud []color[]with open(file_path, r) as f:for line in f:if line.strip(): # 忽略空行try:# 假设每行的格式为: x y zx, y, z, c map(lambda x: round(float(x) / 100, 5) if x ! line.strip().split()[-1] else float(x), line.strip().split())point_cloud.append([x, y, z])#读取c的值并把c的值映射成对应RGB值# 1.0灰色[220,220,220]# 2.0蓝色[173,216,230]# 3.0黄色[255,215,0]# 4.0红色[255,182,193]# r, g, b color_mapping(c)# color.append([r, g, b])# print(x y z,point_cloud)except ValueError:rospy.logerr(Error parsing line: {}.format(line))return np.array(point_cloud)# 循环播放通过frame控制帧数while not rospy.is_shutdown():#主循环在ROS节点关闭前一直运行# 读取点云数据 point_cloud load_point_cloud(DATA_PATH)#,color# print(shape:,point_cloud.shape)#49行3列publish_pcl(point_cloud,pcl_pub,map)#调用publish_pcl函数将点云数据发布到ROS主题jizhui_pcl。color,# pcl_pub.publish(pcl2_msg)rospy.loginfo(published)#在日志中记录发布信息rate.sleep()#按照设定的频率进行休眠文件a.py具体内容如下
from sensor_msgs.msg import PointCloud2,PointField
import sensor_msgs.point_cloud2 as pcl2from std_msgs.msg import Header
import rospy
import numpy as np
def publish_pcl(point_cloud,pcl_pub,frame_id):#定义点云数据的ROS发布者。if point_cloud.size 0:rospy.logwarn(Empty point cloud data, skipping publish.)returnheaderHeader()header.stamprospy.Time.now()header.frame_idframe_idpoint_cloud_messagepcl2.create_cloud_xyz32(header,point_cloud)pcl_pub.publish(point_cloud_message)发布到ros的步骤如下
#启动 ROS master
#启动一个终端键入
roscore
#在python文件b.py下运行代码
python b.py
#再另起一个终端键入
rviz启动了 RViz 后点击界面左下角的Add按钮并添加一个 PointCloud2 显示 即可在 RViz 中看到点云了 **注意**对于发布带颜色的点云数据由于python版没有creat_xyzrgb32 这个功能函数只有C有python的需要自己写一个这样的功能函数。我本人也参考b站博主学习视频链接: 用python将着色点云在ros中发布—解析PointCloud2与Rviz可视化源码弄了一下午没成功就改用C来做了 ps这里还有一个特别需要注意的点就是有的点云图很大且高小窗口下不容易发现我们一开始就因为这个问题郁闷了很久一遍遍检查代码后来发现早就生成了只是没有吧窗口放大多拖拉几下就能找到躲在角落处的点云图
ros中发布带颜色的点云数据xyzrgb
环境
环境: Ubuntu20.04 ROS noetic C
1.新建ROS工作空间
mkdir -p PointCloundShow_ws/src
cd PointCloundShow_ws/src
catkin_init_workspace
cd ..
catkin_make 2.创建功能包
cd src
catkin_create_pkg pointcloundshow pcl_ros roscpp rospy sensor_msgs std_msgs 在功能包的src文件夹下新建cpp文件pcl_colored.cpp 代码文件pcl_colored.cpp内容如下
#include ros/ros.h
#include sensor_msgs/PointCloud2.h
#include pcl/point_cloud.h
#include pcl/point_types.h
#include pcl_conversions/pcl_conversions.h
#include fstream
#include sstream
#include vector
#include iostream
#include stringusing namespace std;
uint32_t color_mapping(float c) {if (c 1.0) {return (220 16) | (220 8) | 220; // 灰色} else if (c 2.0) {return (173 16) | (216 8) | 230; // } else if (c 3.0) {return (255 16) | (215 8) | 0; // } else {return (255 16) | (182 8) | 193; // }
}void readPointCloudFromFile(const string filename, pcl::PointCloudpcl::PointXYZRGB::Ptr cloud) {ifstream infile(filename);if (!infile.is_open()) {cerr Could not open file: filename endl;return;}string line;while (getline(infile, line)) {istringstream iss(line);float x, y, z, c;if (!(iss x y z c)) {cerr Error reading line: line endl;continue;}// 缩小 x, y, z 的值100倍x / 100.0f;y / 100.0f;z / 100.0f;pcl::PointXYZRGB point;point.x x;point.y y;point.z z;uint32_t rgb color_mapping(c);point.rgb *reinterpret_castfloat*(rgb);cloud-points.push_back(point);}cloud-width cloud-points.size();cloud-height 1;cloud-is_dense true;infile.close();
}
int main(int argc, char** argv) {// 初始化ROS节点ros::init(argc, argv, pcl_create_xyzrgb);ros::NodeHandle nh;// 创建一个发布者ros::Publisher pcl_pub nh.advertisesensor_msgs::PointCloud2(pcl_output_colored, 1);// 创建一个点云对象pcl::PointCloudpcl::PointXYZRGB::Ptr cloud(new pcl::PointCloudpcl::PointXYZRGB());// 从文件读取点云数据string filename /home/cxh/project/0618/point_cloud_selected.txt;readPointCloudFromFile(filename, cloud);// 将点云数据转换为ROS消息sensor_msgs::PointCloud2 output;pcl::toROSMsg(*cloud, output);output.header.frame_id map;// 发布点云消息ros::Rate loop_rate(1);while (ros::ok()) {output.header.stamp ros::Time::now();pcl_pub.publish(output);ros::spinOnce();loop_rate.sleep();}return 0;
}
然后将下面的编译规则写到功能包的CMakeLists.txt文件中
find_package(PCL REQUIRED)
include_directories(include${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_executable(pcl_colored.cpp src/pcl_colored.cpp)
target_link_libraries(pcl_colored.cpp ${catkin_LIBRARIES} ${PCL_LIBRARIES})翻倒CMakeLists.txt文件的最后一行添加我们的如下: 如图所示因为我们用C写了两个文件一个是pcl_create.cpp另一个是目前的pcl_colored.cpp所以我们这个是追加到后面的供参考
回到工作空间路径下也就是PointCloundShow_ws输入catkin_make进行编译 然后再更新一下:source devel/setup.bash**注意**只要你修改过你工作空间任何一处代码每次都需要重新编译和更新另外每新建了一个cpp文件都需要在CMakeLists.txt文件做添加 然后新起一个终端终端运行roscore指令 再在刚的source命令那个终端运行rosrun pointcloundshow pcl_create 最后再另起一个终端允许rviz指令
打开rviz 在rviz中左下角点Add增加PointCloud2d topic 选 /pcl_output fixed Frame 输入odom 或者直接点左下角的Add然后会弹出一个名字为rviz的框选择By topic下的对应发布的点云名字 即可看到发布的带有颜色的点云图
