如何在Moveit2中利用点云添加障碍物

Question

环境：ros2 humble
已完成：
1.在config中添加sensor_3d.yaml

# config/sensors_3d.yaml
sensors:  
  - point_cloud_camera

point_cloud_camera:
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /depth_camera/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

2.看到了这个提问https://robotics.stackexchange.com/questions/100378/moveit2-add-pointcloud2-to-occupancymapmonitor%E5%9C%A8launch%E6%96%87%E4%BB%B6%E4%B8%AD%E5%8A%A0%E5%85%A5

 octomap_config = {
        'octomap_frame': 'camera_link',
        'octomap_resolution': 0.05,
        'max_range': 5.0
    }
    
    sensors_3d_path = os.path.join(
        get_package_share_directory('mybot_elite'),
        'config/sensors_3d.yaml'
    )
    with open(sensors_3d_path, 'r') as f:
        octomap_updater_config = yaml.safe_load(f)

    # 合并配置
    octomap_updater_config.setdefault('planning_scene_monitor', {}).update(octomap_config)

    # 将参数列表更新为：
    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
        octomap_updater_config,  # 添加合并后的传感器配置
        {"use_sim_time": True}
    ]

依旧没有成功，Rviz中点云话题有信息，但filtered_cloud没有信息

Answer 1

让 “紫雾凌寒” 来帮你回答,本回答结合 DeepSeek

如果还有问题，请评论或点击头像私信，如果有帮助到您, 请关注、采纳、点赞

1. 关键点分析：
   - 要在MoveIt2中利用点云添加障碍物，关键在于如何将点云数据与MoveIt2的环境模型进行整合，使得MoveIt2能够识别并将这些点云作为障碍物来处理。需要了解MoveIt2的环境感知和碰撞检测机制，以及如何将点云数据转换为适合MoveIt2处理的格式。
2. 解决方案：
   - 步骤一：获取点云数据
   • 首先，你需要从你的点云相机中获取点云数据。假设你已经有了ROS 2节点来接收和处理点云数据，例如使用pointcloud_to_laserscan包将点云转换为激光扫描数据（如果需要）。
   • 示例代码（简单的点云订阅示例）：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2

class PointCloudSubscriber(Node):
    def __init__(self):
        super().__init__('point_cloud_subscriber')
        self.subscription = self.create_subscription(
            PointCloud2,
            'your_point_cloud_topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        # 这里可以对接收到的点云数据进行进一步处理，例如保存到文件等
        self.get_logger().info('Received point cloud data')


def main(args=None):
    rclpy.init(args=args)

    point_cloud_subscriber = PointCloudSubscriber()

    rclpy.spin(point_cloud_subscriber)

    point_cloud_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• 步骤二：处理点云数据为MoveIt2可用格式
  • MoveIt2通常使用Octomap来表示环境。你可以使用octomap_msgs包将点云数据转换为Octomap。
  • 示例代码（将点云转换为Octomap）：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from octomap_msgs.msg import Octomap
from octomap_msgs.srv import OctomapFromPointCloud
import numpy as np


class PointCloudToOctomap(Node):
    def __init__(self):
        super().__init__('point_cloud_to_octomap')
        self.point_cloud_sub = self.create_subscription(
            PointCloud2,
            'your_point_cloud_topic',
            self.point_cloud_callback,
            10)
        self.octomap_pub = self.create_publisher(Octomap, 'octomap', 10)
        self.create_client = self.create_client(OctomapFromPointCloud, 'octomap_from_pointcloud')
        while not self.create_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = OctomapFromPointCloud.Request()

    def point_cloud_callback(self, msg):
        points = np.array(list(pointcloud_to_xyz_array(msg)))
        self.req.pointcloud = msg
        self.create_client.call_async(self.req).add_done_callback(self.response_callback)

    def response_callback(self, future):
        try:
            response = future.result()
            self.octomap_pub.publish(response.octomap)
        except Exception as e:
            self.get_logger().error('Service call failed %r' % (e,))


def pointcloud_to_xyz_array(pointcloud_msg):
    assert pointcloud_msg.point_step == 32
    assert pointcloud_msg.is_bigendian == False
    assert pointcloud_msg.fields[0].name == 'x' and pointcloud_msg.fields[0].offset == 0 and pointcloud_msg.fields[0].datatype == 7
    assert pointcloud_msg.fields[1].name == 'y' and pointcloud_msg.fields[1].offset == 4 and pointcloud_msg.fields[1].datatype == 7
    assert pointcloud_msg.fields[2].name == 'z' and pointcloud_msg.fields[2].offset == 8 and pointcloud_msg.fields[2].datatype == 7

    record_step = pointcloud_msg.point_step
    buff = pointcloud_msg.data
    for i in range(0, len(buff), record_step):
        yield (
            np.frombuffer(buff[i:i + record_step], dtype=np.float32)[0],
            np.frombuffer(buff[i:i + record_step], dtype=np.float32)[1],
            np.frombuffer(buff[i:i + record_step], dtype=np.float32)[2]
        )


def main(args=None):
    rclpy.init(args=args)

    point_cloud_to_octomap = PointCloudToOctomap()

    rclpy.spin(point_cloud_to_octomap)

    point_cloud_to_octomap.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• 步骤三：在MoveIt2中添加障碍物
  • 一旦你有了合适格式的障碍物数据（例如Octomap），你可以通过MoveIt2的规划场景接口来添加障碍物。
  • 示例代码（在MoveIt2中添加Octomap作为障碍物）：

import rclpy
from rclpy.node import Node
from moveit_msgs.msg import PlanningScene
from octomap_msgs.msg import Octomap


class AddObstacleFromOctomap(Node):
    def __init__(self):
        super().__init__('add_obstacle_from_octomap')
        self.planning_scene_pub = self.create_publisher(PlanningScene, 'planning_scene', 10)
        self.octomap_sub = self.create_subscription(
            Octomap,
            'octomap',
            self.octomap_callback,
            10)

    def octomap_callback(self, msg):
        planning_scene_msg = PlanningScene()
        planning_scene_msg.world.octomap = msg
        planning_scene_msg.is_diff = True
        self.planning_scene_pub.publish(planning_scene_msg)


def main(args=None):
    rclpy.init(args=args)

    add_obstacle_from_octomap = AddObstacleFromOctomap()

    rclpy.spin(add_obstacle_from_octomap)

    add_obstacle_from_octomap.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

1. 多种解决方案优缺点：
   - 优点：
   • 基于Octomap的方法：Octomap能够有效地表示3D环境，并且MoveIt2对其有较好的支持。它可以处理复杂的点云数据，并且能够实时更新环境模型。
   • 缺点：
   • 基于Octomap的方法：转换过程可能会消耗一定的计算资源，尤其是在处理大量点云数据时。并且Octomap的构建和更新需要一定的时间，可能会影响实时性。
2. 总结：
   要在MoveIt2中利用点云添加障碍物，首先要获取点云数据，然后将其处理为MoveIt2可用的格式（如Octomap），最后通过MoveIt2的规划场景接口添加障碍物。上述代码示例展示了主要的实现步骤，但实际应用中可能需要根据具体需求进行调整和优化，比如调整点云处理的参数、优化Octomap的构建速度等。

希望以上解答对您有所帮助。如果您有任何疑问，欢迎在评论区提出。