在 ROS 中发布法线

Question

我是 ROS 的新手，我对正常情况感到困惑。这是我的情况。

我订阅了我的相机（realsense 相机），我得到了点云，我将点云从 ROS_to_pcl 转换，最后我使用 python_pcl 的函数 make_NormalEstimation() 来获取法线。到目前为止，一切都很好！ 现在我想以某种方式将这些法线发布到一个 ROS 主题中，并且发布我的意思是在 RVIZ 中也将它们可视化。 python_pcl 函数 make_NormalEstimation() 以向量的形式返回 4 个值。第 3 个值是 normal_x、normal_y、normal_z，第 4 个值是曲率。我想通过 PoseStamed 消息发布和可视化 RVIZ 中的法线。据我所知，PoseStamped 消息需要一个姿势和一个四元数。对于姿势字段，我使用点云中所需点的 x、y、z 来找到法线。但是当涉及到四元数时（这是我的主要问题和斗争），我不知道该使用什么！我尝试使用返回的值，因为它们是 quaternion_x, quaternion_y, quaernion_z, quaternion_w 但结果不太好......

所以。我的问题是：

• 如何使用 make_NormalEstimation() 的返回值来 创建 PoseStamed 消息？
• 有没有办法将返回值转换为四元数？
• 我是否遗漏了有关返回值的某些内容？
• 在 ROS 中是否有另一种查找和使用法线的方法？
• 如何在 ROS 中生成和发布法线？不仅是 normal_x, normal_y, normal_z 值，还有它的方向。
• 我是否必须同时发布它的 normal_x、normal_y、normal_z 和 方向还是只是 normal_x、normal_y、normal_z 值？和 如果是这样，机器人如何知道它需要接近的方向 兴趣点？

对不起，我的问题很混乱！我真的希望它们有意义！

提前致谢！

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)

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