ROS2系列教程：Moveit2碰撞检测与避障

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import PlanningSceneInterface
4
from geometry_msgs.msg import Pose
5

6
def main():
7
    rclpy.init()
8
    node = rclpy.create_node('add_table')
9
    scene = PlanningSceneInterface(node)
10

11
    # 1. 一张桌子：桌面（大扁盒）+ 四条腿（圆柱），都在 base_link 系下
12
    table_top = Pose()
13
    table_top.position.x = 0.45
14
    table_top.position.y = 0.0
15
    table_top.position.z = 0.10   # 桌面高度
16
    scene.add_box('table_top', (0.5, 0.5, 0.02), table_top)
17

18
    for i, (dx, dy) in enumerate([(-0.2, -0.2), (-0.2, 0.2),
19
                                  (0.2, -0.2), (0.2, 0.2)]):
20
        leg = Pose()
21
        leg.position.x = 0.45 + dx
22
        leg.position.y = dy
23
        leg.position.z = 0.05
24
        scene.add_cylinder(f'table_leg_{i}', height=0.10, radius=0.02, pose=leg)
25

26
    node.get_logger().info('已添加桌子和 4 条腿')
27
    rclpy.spin_once(node, timeout_sec=1.0)
28
    node.destroy_node()
29
    rclpy.shutdown()
30

31
if __name__ == '__main__':
32
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import PlanningSceneInterface
4
from geometry_msgs.msg import Pose
5

6
def main():
7
    rclpy.init()
8
    node = rclpy.create_node('moving_obstacle')
9
    scene = PlanningSceneInterface(node)
10
    rate = node.create_rate(10)   # 10Hz 更新
11

12
    t = 0.0
13
    while rclpy.ok():
14
        # 障碍物沿 x 方向来回移动（正弦摆动）
15
        pose = Pose()
16
        pose.position.x = 0.30 + 0.15 * __import__('math').sin(t)
17
        pose.position.y = 0.0
18
        pose.position.z = 0.25
19
        scene.add_box('moving_box', (0.05, 0.05, 0.05), pose)
20
        t += 0.1
21
        rclpy.spin_once(node, timeout_sec=0.05)
22
        rate.sleep()
23

24
    node.destroy_node()
25
    rclpy.shutdown()
26

27
if __name__ == '__main__':
28
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import PlanningSceneInterface, RobotCommander
4

5
def main():
6
    rclpy.init()
7
    node = rclpy.create_node('check_collision')
8
    robot = RobotCommander(node)
9
    scene = PlanningSceneInterface(node)
10

11
    # 1. 构造一个"可疑"的关节状态（比如让手臂完全收起）
12
    joint_values = robot.get_current_state().joint_state.position
13
    # 假设规划组关节是前 3 个
14
    joint_values[:3] = [0.0, 0.0, 0.0]
15

16
    # 2. 检查这个状态是否自碰撞/环境碰撞
17
    from moveit_msgs.srv import GetStateValidity
18
    cli = node.create_client(GetStateValidity, '/check_state_validity')
19
    req = GetStateValidity.Request()
20
    req.robot_state = robot.get_current_state()
21
    req.robot_state.joint_state.position = joint_values
22
    req.group_name = 'arm_group'
23

24
    if not cli.wait_for_service(timeout_sec=2.0):
25
        node.get_logger().error('check_state_validity 服务不可用')
26
        return
27
    fut = cli.call_async(req)
28
    rclpy.spin_until_future_complete(node, fut)
29
    resp = fut.result()
30

31
    if resp.valid:
32
        print('该状态有效（无碰撞）')
33
    else:
34
        print('该状态碰撞！')
35
        # 打印哪些 link 对发生了碰撞
36
        for c in resp.contacts:
37
            print(f'  碰撞: {c.contact_body_1} <-> {c.contact_body_2}')
38

39
    node.destroy_node()
40
    rclpy.shutdown()
41

42
if __name__ == '__main__':
43
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import PlanningSceneInterface
4
from moveit_msgs.srv import GetPlanningScene
5

6
def main():
7
    rclpy.init()
8
    node = rclpy.create_node('check_pair')
9
    cli = node.create_client(GetPlanningScene, '/get_planning_scene')
10
    req = GetPlanningScene.Request()
11
    req.components.components = [req.components.WORLD_OBJECT_GEOMETRY]
12

13
    if not cli.wait_for_service(timeout_sec=2.0):
14
        node.get_logger().error('get_planning_scene 服务不可用')
15
        return
16
    fut = cli.call_async(req)
17
    rclpy.spin_until_future_complete(node, fut)
18
    scene = fut.result().scene
19

20
    # 打印当前规划场景里所有物体
21
    for obj in scene.world.collision_objects:
22
        print(f'物体 id={obj.id} 帧={obj.header.frame_id} '
23
              f'形状数={len(obj.primitives) + len(obj.meshes)}')
24

25
    node.destroy_node()
26
    rclpy.shutdown()
27

28
if __name__ == '__main__':
29
    main()

1
#!/usr/bin/env python3
2
import rclpy
3
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
4
from geometry_msgs.msg import Pose
5

6
def main():
7
    rclpy.init()
8
    node = rclpy.create_node('plan_avoid')
9
    group = MoveGroupCommander('arm_group', node)
10
    scene = PlanningSceneInterface(node)
11

12
    # 1. 在路径中间放一个盒子
13
    pose = Pose()
14
    pose.position.x = 0.25
15
    pose.position.y = 0.0
16
    pose.position.z = 0.20
17
    scene.add_box('wall', (0.05, 0.30, 0.30), pose)
18

19
    # 2. 目标在盒子另一侧
20
    group.set_position_target([0.45, 0.0, 0.25])
21
    plan = group.plan()
22
    if getattr(plan, 'joint_trajectory', None) and plan.joint_trajectory.points:
23
        print(f'规划成功！轨迹点数={len(plan.joint_trajectory.points)}')
24
        group.execute(plan, wait=True)
25
        print('执行完成——轨迹绕过了盒子')
26
    else:
27
        print('规划失败：可能被盒子挡住了')
28

29
    # 3. 移除盒子
30
    scene.remove_world_object('wall')
31
    node.destroy_node()
32
    rclpy.shutdown()
33

34
if __name__ == '__main__':
35
    main()