pcl_calib.cfg
#!/usr/bin/env python
PACKAGE = "depth_camera_calib"
NODENAME="depth_camera_calib"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100)
# gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1)
# gen.add("str_param", str_t, 0, "A string parameter", "Hello World")
# gen.add("bool_param", bool_t, 0, "A Boolean parameter", True)
#
# # size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"),
# gen.const("Medium", int_t, 1, "A medium constant"),
# gen.const("Large", int_t, 2, "A large constant"),
# gen.const("ExtraLarge", int_t, 3, "An extra large constant")], "An enum to set size")
#
# gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)
#
gen.add("x_min", double_t, 0, "x_min", -0.1, -0.5, 0.5)
gen.add("x_max", double_t, 0, "x_max", 0.1, -0.5, 0.5)
gen.add("y_min", double_t, 0, "y_min", -0.1, -0.5, 0.5)
gen.add("y_max", double_t, 0, "y_max", 0.1, -0.5, 0.5)
gen.add("z_min", double_t, 0, "z_min", 0.7, 0, 2)
gen.add("z_max", double_t, 0, "z_max", 2, 0, 2)
gen.add("calib_xy_1", bool_t, 0, "calib_xy_1", False)
gen.add("calib_xy_2", bool_t, 0, "calib_xy_2", False)
gen.add("calib_z_1", bool_t, 0, "calib_z_1", False)
gen.add("calib_z_2", bool_t, 0, "calib_z_2", False)
gen.add("reset_calib", bool_t, 0, "reset_calib", False)
gen.add("prt_current_high_point_after_calib", bool_t, 0, "prt_current_high_point", False)
exit(gen.generate(PACKAGE, NODENAME, "pclCalib"))
pcl_calib_config.yaml
#red
world_x_1: "-15"
world_y_1: "25"
world_x_2: "15"
world_y_2: "5"
world_z_1: "0.9"
world_z_2: "28"
input_channel: /camera/depth_registered/points
output_channel: /filtered
cal_depth_camera_world.py
//
// Created by hao on 2022/5/21.
//
#include <ros/ros.h>
// PCL specific includes 。PCL 的相关的头文件
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>
#include <dynamic_reconfigure/server.h>
#include "depth_camera_calib/pclCalibConfig.h"
#include<cmath>
#include "tf_cloud_to_world.h"
//申明发布器
ros::Publisher pub;
double x_max=100;
double x_min=-100;
double y_max=100;
double y_min=-100;
double z_max=100;
double z_min=-100;
bool calib_xy_1= false;
bool calib_xy_2= false;
bool calib_z_1= false;
bool calib_z_2= false;
std::vector<double> cal_1_x;
std::vector<double> cal_1_y;
std::vector<double> cal_1_z;
std::vector<double> cal_2_x;
std::vector<double> cal_2_y;
std::vector<double> cal_2_z;
bool calib_xy_1_done= false;
bool calib_xy_2_done= false;
bool calib_z_1_done= false;
bool calib_z_2_done= false;
bool load_calib_txt_done= false;
const std::string CALIB_ID_XY_1="CALIB_ID_XY_1";
const std::string CALIB_ID_XY_2="CALIB_ID_XY_2";
const std::string CALIB_ID_Z_1="CALIB_ID_Z_1";
const std::string CALIB_ID_Z_2="CALIB_ID_Z_2";
const std::string CALIB_ID_NONE="CALIB_ID_NONE";
bool load_calib_txt= false;
bool prt_current_high_point_after_calib=false;
void do_prt_current_high_point(std::vector<pcl::PointXYZRGBA>* keep_points){
double _x=0;
double _y=0;
double _z=100;
for (auto &point:*keep_points){
// 选择距离相机最近的点,就是z最小的点
if (point.z<_z){
_x=point.x;
_y=point.y;
_z=point.z;
}
}
double world_x=0;
double world_y=0;
double world_z=0;
tf_cloud_to_world tf;
tf.cloud_to_world(_x,_y,_z,world_x,world_y,world_z);
ROS_INFO("highest point: world coordinate: %f %f %f",world_x,world_y,world_z);
sleep(1);
}
void write_txt(const std::string& calib_id,int data_size)
{
double sum_x=0;
double sum_y=0;
double sum_z=0;
double avg_x=0;
double avg_y=0;
double avg_z=0;
if (calib_id==CALIB_ID_XY_1){
for (auto &value:cal_1_x)
sum_x+=value;
for (auto &value:cal_1_y)
sum_y+=value;
avg_x= round(sum_x/data_size*10000)/10000;
avg_y= round(sum_y/data_size*10000)/10000;
avg_z=-100;
}
else if (calib_id==CALIB_ID_XY_2){
for (auto &value:cal_2_x)
sum_x+=value;
for (auto &value:cal_2_y)
sum_y+=value;
avg_x= round(sum_x/data_size*10000)/10000;
avg_y= round(sum_y/data_size*10000)/10000;
avg_z=-100;
}
else if (calib_id==CALIB_ID_Z_1){
for (auto &value:cal_1_z)
sum_z+=value;
avg_x= -100;
avg_y= -100;
avg_z= round(sum_z/data_size*10000)/10000;
}
else if (calib_id==CALIB_ID_Z_2){
for (auto &value:cal_2_z)
sum_z+=value;
avg_x= -100;
avg_y= -100;
avg_z= round(sum_z/data_size*10000)/10000;
}
else {
avg_x= -100;
avg_y= -100;
avg_z=-100;
}
try {
//世界坐标从档案里读取
std::ofstream outfile("/home/hao/Documents/depth_camera_calib.txt", std::ios::out|std::ios::app);
std::string point1_world_x;
std::string point1_world_y;
std::string point1_world_z;
ros::param::get("world_x_1",point1_world_x);
ros::param::get("world_y_1",point1_world_y);
ros::param::get("world_z_1",point1_world_z);
std::string point2_world_x;
std::string point2_world_y;
std::string point2_world_z;
ros::param::get("world_x_2",point2_world_x);
ros::param::get("world_y_2",point2_world_y);
ros::param::get("world_z_2",point2_world_z);
if (calib_id==CALIB_ID_XY_1) {
outfile <<"cloud_x_1:" << avg_x << std::endl;
outfile <<"cloud_y_1:" << avg_y << std::endl;
outfile <<"world_x_1:" << point1_world_x << std::endl;
outfile <<"world_y_1:" << point1_world_y << std::endl;
calib_xy_1_done= true;
}
else if (calib_id==CALIB_ID_XY_2) {
outfile <<"cloud_x_2:" << avg_x << std::endl;
outfile <<"cloud_y_2:" << avg_y << std::endl;
outfile <<"world_x_2:" << point2_world_x << std::endl;
outfile <<"world_y_2:" << point2_world_y << std::endl;
calib_xy_2_done= true;
}
else if (calib_id==CALIB_ID_Z_1) {
outfile <<"cloud_z_1:" << avg_z << std::endl;
outfile <<"world_z_1:" << point1_world_z << std::endl;
calib_z_1_done= true;
}
else if (calib_id==CALIB_ID_Z_2) {
outfile <<"cloud_z_2:" << avg_z << std::endl;
outfile <<"world_z_2:" << point2_world_z << std::endl;
calib_z_2_done= true;
}
outfile.close();
}catch(...)
{
std::cout<<"write txt error"<<std::endl;
}
}
bool add_vector(const std::string& calib_id,std::vector<pcl::PointXYZRGBA>* keep_points,int data_size){
double _x=0;
double _y=0;
double _z=100;
for (auto &point:*keep_points){
// 选择距离相机最近的点,就是z最小的点
if (point.z<_z){
_x=point.x;
_y=point.y;
_z=point.z;
}
}
if (calib_id==CALIB_ID_XY_1 && cal_1_x.size()<(data_size+1)){
cal_1_x.push_back(_x);
cal_1_y.push_back(_y);
}
else if (calib_id==CALIB_ID_XY_2 && cal_2_x.size()<(data_size+1)){
cal_2_x.push_back(_x);
cal_2_y.push_back(_y);
}
else if (calib_id==CALIB_ID_Z_1 && cal_1_z.size()<(data_size+1)){
cal_1_z.push_back(_z);
}
else if (calib_id==CALIB_ID_Z_2 && cal_2_z.size()<(data_size+1)){
cal_2_z.push_back(_z);
}
if ((calib_id==CALIB_ID_XY_1 && cal_1_x.size()==data_size) ||
(calib_id==CALIB_ID_XY_2 && cal_2_x.size()==data_size)||
(calib_id==CALIB_ID_Z_1 && cal_1_z.size()==data_size)||
(calib_id==CALIB_ID_Z_2 && cal_2_z.size()==data_size))
{
write_txt(calib_id,data_size);
}
return true;
}
void do_calib(std::vector<pcl::PointXYZRGBA>* keep_points,int data_size)
{
std::string calib_id;
if (calib_xy_1 && !calib_xy_2 && !calib_z_1 && !calib_z_2)
calib_id=CALIB_ID_XY_1;
else if(!calib_xy_1 && calib_xy_2 && !calib_z_1 && !calib_z_2)
calib_id=CALIB_ID_XY_2;
else if(!calib_xy_1 && !calib_xy_2 && calib_z_1 && !calib_z_2)
calib_id=CALIB_ID_Z_1;
else if(!calib_xy_1 && !calib_xy_2 && !calib_z_1 && calib_z_2)
calib_id=CALIB_ID_Z_2;
else
calib_id=CALIB_ID_NONE;
if ((calib_id == CALIB_ID_NONE) ||
(calib_id == CALIB_ID_XY_1 && calib_xy_1_done) ||
(calib_id == CALIB_ID_XY_2 && calib_xy_2_done) ||
(calib_id == CALIB_ID_Z_1 && calib_z_1_done) ||
(calib_id == CALIB_ID_Z_2 && calib_z_2_done) )
{ return; }
else {add_vector(calib_id,keep_points,data_size);}
}
void dynamic_cb(depth_camera_calib::pclCalibConfig &config, uint32_t level) {
// ROS_INFO("Reconfigure Request: x_max:%f x_min:%f y_max:%f y_min:%f z_max:%f z_min:%f calib_xy_1:%s calib_xy_2:%s calib_z_1:%s calib_z_2:%s reset_calib:%s",
// config.x_max,
// config.x_min,
// config.y_max,
// config.y_min,
// config.z_max,
// config.z_min,
//
// config.calib_xy_1?"True":"False",
// config.calib_xy_2?"True":"False",
// config.calib_z_1?"True":"False",
// config.calib_z_2?"True":"False",
// config.reset_calib?"True":"False"
// );
x_max=config.x_max;
x_min=config.x_min;
y_max=config.y_max;
y_min=config.y_min;
z_max=config.z_max;
z_min=config.z_min;
calib_xy_1=config.calib_xy_1;
calib_xy_2=config.calib_xy_2;
calib_z_1=config.calib_z_1;
calib_z_2=config.calib_z_2;
// load_calib_txt=config.load_calib_txt;
prt_current_high_point_after_calib=config.prt_current_high_point_after_calib;
if (config.reset_calib) {
ROS_INFO("Reconfigure Request: reset all");
calib_xy_1_done = false;
calib_xy_2_done= false;
calib_z_1_done= false;
calib_z_2_done= false;
cal_1_x.clear();
cal_1_y.clear();
cal_1_z.clear();
cal_2_x.clear();
cal_2_y.clear();
cal_2_z.clear();
}
}
//回调函数
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// std::string frame_id=cloud_msg->head->frame_id;
auto* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_points(new pcl::PointCloud<pcl::PointXYZRGBA>());
pcl::fromPCLPointCloud2(*cloud, *cloud_points);
auto *keep_points=new std::vector<pcl::PointXYZRGBA>();
for (auto &point:*cloud_points){
if (point.x <= x_max && point.x >= x_min && point.y <= y_max && point.y >= y_min && point.z <= z_max && point.z >= z_min) {
keep_points->push_back(point);
}
}
auto* filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
filtered_cloud->width=keep_points->size();
filtered_cloud->height=1;
filtered_cloud->resize(filtered_cloud->width*filtered_cloud->height);
for (size_t i = 0; i < keep_points->size(); ++i) //循环填充点云数据
{
filtered_cloud->points[i]=(*keep_points)[i];
}
pcl::PCLPointCloud2 filtered_cloud2;
pcl::toPCLPointCloud2 (*filtered_cloud, filtered_cloud2);
sensor_msgs::PointCloud2 senser_msg_cloud2;//声明的输出的点云的格式
pcl_conversions::moveFromPCL(filtered_cloud2, senser_msg_cloud2);//第一个参数是输入，后面的是输出
senser_msg_cloud2.header.frame_id="camera_depth_optical_frame";
pub.publish (senser_msg_cloud2);
do_calib(keep_points,5);
// if (load_calib_txt && !load_calib_txt_done){
//
// }
if(prt_current_high_point_after_calib){
do_prt_current_high_point(keep_points);
}
}
int main (int argc, char** argv){
ros::init(argc, argv, "depth_camera_calib");
ros::NodeHandle nh;
dynamic_reconfigure::Server<depth_camera_calib::pclCalibConfig> server;
dynamic_reconfigure::Server<depth_camera_calib::pclCalibConfig>::CallbackType f;
f = boost::bind(&dynamic_cb, _1, _2);
server.setCallback(f);
std::string input_channel;
std::string output_channel;
ros::param::get("input_channel",input_channel);
ros::param::get("output_channel",output_channel);
// std::cout<<"input_channel:"<<input_channel<<std::endl;
// std::cout<<"output_channel:"<<output_channel<<std::endl;
//设定点云call back
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> (input_channel, 1, cloud_cb);
//设定点云处理后的 publisher
pub = nh.advertise<sensor_msgs::PointCloud2> (output_channel, 1);
ROS_INFO("Spinning node");
ros::spin();
return 0;
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
