在 ROS 中发布法线

Question

有没有办法在 ROS 中发布给定点云的法线？ 我订阅了我的相机，我得到了点云，我将点云从 ROS 转换为 pcl，最后我使用 python pcl 的函数make_NormalEstimation()来获取法线。 到现在为止还挺好！ 现在我想以某种方式将这些法线发布到 ROS 主题。 理想情况下，我想将这些法线转换为 PoseStamped ROS 消息并发布它们。

谁能帮我？

提前致谢！

Answer 1

您可以简单地设置发布者并按需为列表中的每个法线创建消息。

import rospy
from geometry_msgs import PoseStamped

pose_pub = rospy.Publisher('/normals_topic', PoseStamped, queue_size=10)
def calc_and_publish():
    #Calculate your normals
    norms = calculate_norms()

    for n in norms:
        output_pose = PoseStamped
        output_pose.pose.position.x = n[0]
        output_pose.pose.position.y = n[1]
        output_pose.pose.position.z = n[2]
        
        pose_pub.publish(output_pose)

我应该注意，这会消耗大量资源，因为您将不得不在每条消息上多次循环点云。