ROS_Robot/wpr_simulation2
1
0
Fork 0
mirror of https://github.com/6-robot/wpr_simulation2.git synced 2025-09-15 12:58:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update demo_10

Browse Source
This commit is contained in:
Robot 2023-10-30 22:38:43 +08:00
parent bcf4ded6ab
commit 2b54b8272a
8 changed files with 179 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -144,13 +144,13 @@ ament_target_dependencies(10_mani_ctrl_class
"rclcpp" "sensor_msgs"
)
add_executable(11_pointcloud_data demo_cpp/11_pointcloud_data.cpp)
ament_target_dependencies(11_pointcloud_data
add_executable(10_pc_data demo_cpp/10_pc_data.cpp)
ament_target_dependencies(10_pc_data
"rclcpp" "sensor_msgs" "pcl_conversions" "pcl_ros"
)
add_executable(11_pointcloud_objects demo_cpp/11_pointcloud_objects.cpp)
ament_target_dependencies(11_pointcloud_objects
add_executable(10_pc_objects demo_cpp/10_pc_objects.cpp)
ament_target_dependencies(10_pc_objects
"rclcpp" "sensor_msgs" "pcl_conversions" "pcl_ros"
)
@ -203,8 +203,8 @@ install(
6_imu_data 6_imu_behavior
8_waypoint_navigation
9_cv_image 9_cv_hsv 9_cv_follow 9_cv_face_detect
10_mani_ctrl 10_mani_ctrl_class
11_pointcloud_data 11_pointcloud_objects 11_grab_object
10_pc_data 10_pc_objects
10_mani_ctrl 10_mani_ctrl_class 11_grab_object
12_fetch
demo_nav2_client
DESTINATION

39
demo_cmakelists/10_pc_data.txt Normal file
View File

@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.8)
project(pc_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
add_executable(pc_data src/pc_data.cpp)
ament_target_dependencies(pc_data
"rclcpp" "sensor_msgs" "pcl_conversions" "pcl_ros"
)
install(TARGETS pc_data
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

39
demo_cmakelists/10_pc_objects.txt Normal file
View File

@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.8)
project(pc_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
add_executable(pc_objects src/pc_objects.cpp)
ament_target_dependencies(pc_objects
"rclcpp" "sensor_msgs" "pcl_conversions" "pcl_ros"
)
install(TARGETS pc_objects
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

2
demo_cpp/11_pointcloud_data.cpp → demo_cpp/10_pc_data.cpp
View File

@ -29,7 +29,7 @@ int main(int argc, char **argv)
node = std::make_shared<rclcpp::Node>("pointcloud_data_node");
auto pc_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)).best_effort(),
1,
PointcloudCallback
);

36
demo_cpp/11_pointcloud_objects.cpp → demo_cpp/10_pc_objects.cpp
View File

@ -16,18 +16,27 @@ std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
bool result = tf_buffer_->canTransform("base_footprint", msg->header.frame_id, msg->header.stamp);
bool result = tf_buffer_->canTransform(
"base_footprint",
msg->header.frame_id,
msg->header.stamp
);
if (!result)
{
return;
}
sensor_msgs::msg::PointCloud2 pc_footprint;
pcl_ros::transformPointCloud("base_footprint", *msg, pc_footprint, *tf_buffer_);
pcl_ros::transformPointCloud(
"base_footprint",
*msg,
pc_footprint,
*tf_buffer_
);
pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
pcl::PointCloud<pcl::PointXYZ> cloud_src;
pcl::fromROSMsg(pc_footprint, cloud_src);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("x");
pass.setFilterLimits(0.5, 1.5);
@ -42,7 +51,7 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
pass.filter(cloud_src);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud_src.makeShared());
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
@ -67,19 +76,17 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
pass.setFilterLimits(plane_height + 0.2, 1.5);
pass.filter(cloud_src);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_src.makeShared());
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.1);
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_src.makeShared());
ec.extract(cluster_indices);
RCLCPP_INFO(node->get_logger(), "Number of clusters: %d", (int)cluster_indices.size());
int object_num = cluster_indices.size();
RCLCPP_INFO(node->get_logger(), "object_num = %d",object_num);
for(int i = 0 ; i < object_num ; i ++)
@ -98,7 +105,14 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
float object_x = points_x_sum/point_num;
float object_y = points_y_sum/point_num;
float object_z = points_z_sum/point_num;
RCLCPP_INFO(node->get_logger(), "object %d pos = ( %.2f , %.2f , %.2f)" ,i, object_x,object_y,object_z);
RCLCPP_INFO(
node->get_logger(),
"object-%d pos=( %.2f , %.2f , %.2f)" ,
i,
object_x,
object_y,
object_z
);
}
RCLCPP_INFO(node->get_logger(), "---------------------" );
}
@ -114,7 +128,7 @@ int main(int argc, char **argv)
auto pc_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)).best_effort(),
1,
PointcloudCallback
);

39
demo_cpp/11_pointcloud_objects_class.cpp → demo_cpp/10_pc_objects_class.cpp
View File

@ -21,28 +21,32 @@ public:
pc_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)).best_effort(),
1,
std::bind(&PointcloudObjects::pointcloudCallback, this, std::placeholders::_1));
}
private:
void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr input)
void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// 将点云数值从相机坐标系转换到机器人坐标系
bool result = tf_buffer_->canTransform("base_footprint", input->header.frame_id, input->header.stamp);
bool result = tf_buffer_->canTransform(
"base_footprint",
msg->header.frame_id,
msg->header.stamp
);
if (!result)
{
return;
}
sensor_msgs::msg::PointCloud2 pc_footprint;
pcl_ros::transformPointCloud("base_footprint", *input, pc_footprint, *tf_buffer_);
pcl_ros::transformPointCloud("base_footprint", *msg, pc_footprint, *tf_buffer_);
// 将点云数据从ROS格式转换到PCL格式
pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
pcl::PointCloud<pcl::PointXYZ> cloud_src;
pcl::fromROSMsg(pc_footprint, cloud_src);
// 对设定范围内的点云进行截取
pcl::PassThrough<pcl::PointXYZRGB> pass;
pcl::PassThrough<pcl::PointXYZ> pass;
// 截取x轴方向，前方0.5米到1.5米内的点云
pass.setInputCloud(cloud_src.makeShared());
pass.setFilterFieldName("x");
@ -61,7 +65,7 @@ private:
// 定义模型分类器
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud_src.makeShared());
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
@ -90,27 +94,24 @@ private:
pass.filter(cloud_src);
// 对截取后的点云进行欧式距离分割
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_src.makeShared()); // 输入截取后的点云数据到KD树
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.1); // 设置聚类的容差
ec.setMinClusterSize(100); // 设置每个聚类的最小点数
ec.setMaxClusterSize(25000); // 设置每个聚类的最大点数
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_src.makeShared());
ec.extract(cluster_indices);
// 输出聚类的数量
RCLCPP_INFO(this->get_logger(), "Number of clusters: %d", cluster_indices.size());
// 计算每个分割出来的点云团的中心坐标
int object_num = cluster_indices.size(); // 分割出的点云团个数
int object_num = cluster_indices.size();
RCLCPP_INFO(this->get_logger(), "object_num = %d",object_num);
// 计算每个分割出来的点云团的中心坐标
for(int i = 0 ; i < object_num ; i ++)
{
int point_num = cluster_indices[i].indices.size(); // 点云团i中的点数
int point_num = cluster_indices[i].indices.size(); // 点云团i中的点数
float points_x_sum = 0;
float points_y_sum = 0;
float points_z_sum = 0;
@ -124,7 +125,13 @@ private:
float object_x = points_x_sum/point_num;
float object_y = points_y_sum/point_num;
float object_z = points_z_sum/point_num;
RCLCPP_INFO(this->get_logger(), "object %d pos = ( %.2f , %.2f , %.2f)" ,i, object_x,object_y,object_z);
RCLCPP_INFO(
this->get_logger(),
"object %d pos = ( %.2f , %.2f , %.2f)" ,
i,
object_x,
object_y,
object_z);
}
RCLCPP_INFO(this->get_logger(), "---------------------" );
}

23
demo_package/10_pc_data.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pc_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="robot@6-robot.com">robot</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

23
demo_package/10_pc_objects.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pc_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="robot@6-robot.com">robot</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>