Webots Python 绘制机器人运动轨迹 无人车 无人机

前言

本代码由 Webots supervisor_draw_trail.wbt 示例 world 里的 supervisor_draw_trail.c 改写而来，并添加了对多智能体（多机器人）运动轨迹绘制的支持。

在自定义环境里的运行效果：

代码

from controller import Supervisor


class PathDrawer(Supervisor):
    def __init__(self):
        super().__init__()
        self.MAXIMUM_NUMBER_OF_COORDINATES = 20000
        self.REFRESH_FACTOR = 10
        self.index = 0
        self.first_step = True
        
        # ---- 修改为你的机器人数量 ----
        self.num_agents = 1      
        # ------------------------------

        for ind in range(self.num_agents):
            self.path_str = ""
            self.create_trail_shape(ind)
        self.get_nodes_and_fileds()

        # Get the target object node to track
        self.target_node = []
        for ind in range(self.num_agents):
            # ---- 需要在环境里将机器人 Define 别名: ROBOT0, ROBOT1, ROBOT2... -
            self.target_node.append(self.getFromDef(f"ROBOT{ind}"))  
            # -----------------------------------------------------------------

    def strcat(self, content):
        self.path_str += content

    def create_trail_shape(self, agent_ind):
        # If TRAIL exists in the world then silently remove it.
        existing_trail = self.getFromDef(f"TRAIL{agent_ind}")
        if (existing_trail):
            existing_trail.remove()

        # Create the TRAIL Shape.
        tmp_str = "{\n"
        self.strcat(f"DEF TRAIL{agent_ind} Shape {tmp_str}")
        self.strcat("  appearance Appearance {\n")
        self.strcat("    material Material {\n")
        self.strcat("      ambientIntensity 1\n")
        self.strcat("      diffuseColor 0 1 0\n")
        self.strcat("      emissiveColor 0 1 0\n")
        self.strcat("    }\n")
        self.strcat("  }\n")
        self.strcat(f"  geometry DEF TRAIL_LINE_SET{agent_ind} IndexedLineSet {tmp_str}")
        self.strcat("    coord Coordinate {\n")
        self.strcat("      point [\n")
        for _ in range(self.MAXIMUM_NUMBER_OF_COORDINATES):
            self.strcat("      0 0 0\n")
        self.strcat("      ]\n")
        self.strcat("    }\n")
        self.strcat("    coordIndex [\n")
        for _ in range(self.MAXIMUM_NUMBER_OF_COORDINATES):
            self.strcat("      0 0 -1\n")
        self.strcat("    ]\n")
        self.strcat("  }\n")
        self.strcat("}\n")

        # Import TRAIL and append it as the world root nodes.
        root_children_field = self.getRoot().getField("children")
        root_children_field.importMFNodeFromString(-1, self.path_str)
        print(f"已添加新路径节点{agent_ind}")

    def get_nodes_and_fileds(self):
        self.point_field_all = []
        self.coord_index_field_all = []
        for ind in range(self.num_agents):
            self.trail_line_set_node = self.getFromDef(f"TRAIL_LINE_SET{ind}")
            self.coordinates_node = self.trail_line_set_node.getField("coord").getSFNode()
            self.point_field = self.coordinates_node.getField("point")
            self.point_field_all.append(self.point_field)
            self.coord_index_field = self.trail_line_set_node.getField("coordIndex")
            self.coord_index_field_all.append(self.coord_index_field)

    def update_trail(self):
        for ind in range(self.num_agents):
            # Get the current target translation.
            target_translation = self.target_node[ind].getPosition()
            print(target_translation)
            # Add the new target translation in the line set.
            self.point_field_all[ind].setMFVec3f(self.index, target_translation)
            # Update the line set indices.
            if self.index > 0:
                # Link successive indices.
                self.coord_index_field_all[ind].setMFInt32(3 * (self.index - 1), self.index - 1)
                self.coord_index_field_all[ind].setMFInt32(3 * (self.index - 1) + 1, self.index)
            elif (self.index == 0 and self.first_step == False):
                # Link the first and the last indices.
                self.coord_index_field_all[ind].setMFInt32(3 * (self.MAXIMUM_NUMBER_OF_COORDINATES - 1), 0)
                self.coord_index_field_all[ind].setMFInt32(3 * (self.MAXIMUM_NUMBER_OF_COORDINATES - 1) + 1,
                                                self.MAXIMUM_NUMBER_OF_COORDINATES - 1)
            # Unset the next indices.
            self.coord_index_field_all[ind].setMFInt32(3 * self.index, self.index)
            self.coord_index_field_all[ind].setMFInt32(3 * self.index + 1, self.index)

        self.index += 1
        self.index = self.index % self.MAXIMUM_NUMBER_OF_COORDINATES
        self.first_step = False


if __name__ == "__main__":
    supervisor = PathDrawer()
    while supervisor.step(32 * 10) != -1:
        supervisor.update_trail()

使用方法

1. self.num_agents = 1 修改机器人数量；
2. 需要在环境里将机器人 Define 别名: ROBOT0, ROBOT1, ROBOT2…，以对应代码 getFromDef(f"ROBOT{ind}")；
3. 一般你会在 supervisor.py 里创建一个全局的 class 继承 Supervisor，因此只需要将上面的代码融合进你的代码即可。