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 10_pc_objects

Browse Source
This commit is contained in:
Robot 2023-10-31 10:23:49 +08:00
parent 2b54b8272a
commit a06f1cd0c0
2 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
demo_cpp/10_pc_objects.cpp
View File

@ -1,14 +1,13 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_listener.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_ros/transforms.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
std::shared_ptr<rclcpp::Node> node;
tf2_ros::Buffer::SharedPtr tf_buffer_;
@ -20,7 +19,7 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
"base_footprint",
msg->header.frame_id,
msg->header.stamp
);
);
if (!result)
{
return;
@ -31,7 +30,7 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
*msg,
pc_footprint,
*tf_buffer_
);
);
pcl::PointCloud<pcl::PointXYZ> cloud_src;
pcl::fromROSMsg(pc_footprint, cloud_src);
@ -106,13 +105,13 @@ void PointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
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
);
node->get_logger(),
"object %d pos=(%.2f , %.2f , %.2f)",
i,
object_x,
object_y,
object_z
);
}
RCLCPP_INFO(node->get_logger(), "---------------------" );
}

19
demo_cpp/10_pc_objects_class.cpp
View File

@ -1,28 +1,28 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_ros/transform_listener.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_ros/transforms.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
class PointcloudObjects : public rclcpp::Node
{
public:
PointcloudObjects()
: Node("pointcloud_cluster")
: Node("pointcloud_objects_node")
{
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
pc_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/kinect2/sd/points",
1,
std::bind(&PointcloudObjects::pointcloudCallback, this, std::placeholders::_1));
"/kinect2/sd/points",
1,
std::bind(&PointcloudObjects::pointcloudCallback, this, std::placeholders::_1)
);
}
private:
@ -127,11 +127,12 @@ private:
float object_z = points_z_sum/point_num;
RCLCPP_INFO(
this->get_logger(),
"object %d pos = ( %.2f , %.2f , %.2f)" ,
"object %d pos=(%.2f , %.2f , %.2f)",
i,
object_x,
object_y,
object_z);
object_z
);
}
RCLCPP_INFO(this->get_logger(), "---------------------" );
}