ROS2+Gazebo简易自动驾驶（4）

文章列表

建模场地【已更新】
Turtle Bot 4 环境安装【已更新】
巡线【已更新】
红绿灯、停车牌建模【已更新】
红绿灯、停车牌识别【已更新】
SLAM导航【已更新】

第四步：红绿灯、停车牌建模

停车牌建模

太长不看版：
模型下载地址：: https://pan.baidu.com/s/1ZENd1Pc5kosZWbtixGgQtw?pwd=vs6e 提取码: vs6e
stop.SLDPRT是solidworks建模文件
stop.dae是用于导入gazebo的文件
参考第一篇博客：建模场地

停车牌比较复杂，有文字。因此我们在Solid Works中进行建模
绘制八边形，拉伸形成板子，设为红色：

在板子表面新建草图，使用文字工具在一条构造线上写STOP

然后把文字拉伸出厚度，设为白色
使用转换引用和等距实体，创建边框，拉伸并设为白色

绘制圆柱体柱子

从SolidWorks to Dae - 3dEncoder网站导出为dae格式
把stop.dae放在auto_drive功能包里的world文件夹中
修改turtlebot4_simulator/turtlebot4_gz_bringup/worlds/drive_track.sdf文件，加入stop.dae文件，参考代码如下

<?xml version="1.0" ?>
<sdf version="1.11">
  <world name="drive_track">
    <plugin name="gz::sim::systems::Physics" filename="gz-sim-physics-system" />
    <plugin name="gz::sim::systems::UserCommands" filename="gz-sim-user-commands-system" />
    <plugin name="gz::sim::systems::SceneBroadcaster" filename="gz-sim-scene-broadcaster-system" />
    <plugin name="gz::sim::systems::Contact" filename="gz-sim-contact-system" />

    <physics name="1ms" type="ignored">
      <max_step_size>0.003</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>

    <light name='sun' type='directional'>
        <cast_shadows>1</cast_shadows>
        <pose>0 0 10 0 -0 0</pose>
        <diffuse>0.8 0.8 0.8 1</diffuse>
        <specular>0.2 0.2 0.2 1</specular>
        <attenuation>
            <range>1000</range>
            <constant>0.90000000000000002</constant>
            <linear>0.01</linear>
            <quadratic>0.001</quadratic>
        </attenuation>
        <direction>-0.5 0.1 -0.9</direction>
        <spot>
            <inner_angle>0</inner_angle>
            <outer_angle>0</outer_angle>
            <falloff>0</falloff>
        </spot>
    </light>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic' />
    <scene>
        <ambient>0.4 0.4 0.4 1</ambient>
        <background>0.7 0.7 0.7 1</background>
        <shadows>1</shadows>
    </scene>

    <model name="field">
      <pose>0 0 0 0 0 0</pose>
      <static>true</static>
      <link name="field_link">
        <collision name="collision">
          <geometry>
            <!-- Collision mesh -->
            <mesh>
              <uri>（你的路径）/turtlebot4_ws/src/auto_drive/world/field_v2.dae</uri>
            </mesh>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <!-- Visual mesh -->
            <mesh>
              <uri>（你的路径）/turtlebot4_ws/src/auto_drive/world/field_v2.dae</uri>
            </mesh>
          </geometry>
        </visual>
      </link>
    </model>

    <!-- 从这里开始修改 -->
    <include>
      <uri>（你的路径）/turtlebot4_ws/src/auto_drive/world/stop.sdf</uri>
      <pose>-3.17 2.0 0.3 0.0 1.5708 0.0</pose>
    </include>
  </world>
</sdf>

红绿灯建模

因为我学艺不精，不会写gazebo插件。因此我们用另一种方式实现红绿灯的切换。创建两个红绿灯模型，一个红灯亮，一个绿灯亮。切换红绿灯的步骤就是删除绿灯模型，创建红灯模型。
因为红绿灯结构比较简单。我们直接用sdf代码创建，sdf文件同理存放在auto_drive/world文件夹里

red_light.sdf

<?xml version="1.0" ?>
<sdf version="1.6">
  <model name="traffic_light">
    <static>true</static>

    <link name="link">
      <pose>0 0 0 0 0 0</pose>

      <visual name="pole">
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.4</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0.5 0.5 0.5 1</ambient>
          <diffuse>0.5 0.5 0.5 1</diffuse>
        </material>
        <pose>0 0 0.2 0 0 0</pose>
      </visual>

      <visual name="light_box">
        <geometry>
          <box>
            <size>0.1 0.15 0.4</size>
          </box>
        </geometry>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
        </material>
        <pose>0 0 0.4 0 0 0</pose>
      </visual>

      <visual name="red_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <emissive>1 0 0 1</emissive>
          <ambient>1 0 0 1</ambient>
          <diffuse>1 0 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.52 0 0 0</pose>
      </visual>

      <visual name="yellow_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.1 0.1 0 1</ambient>
          <diffuse>0.1 0.1 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.4 0 0 0</pose>
      </visual>

      <visual name="green_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0 0.1 0 1</ambient>
          <diffuse>0 0.1 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.28 0 0 0</pose>
      </visual>
    </link>
  </model>
</sdf>

green_light.sdf

<?xml version="1.0" ?>
<sdf version="1.6">
  <model name="traffic_light">
    <static>true</static>

    <link name="link">
      <pose>0 0 0 0 0 0</pose>

      <visual name="pole">
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.4</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>0.5 0.5 0.5 1</ambient>
          <diffuse>0.5 0.5 0.5 1</diffuse>
        </material>
        <pose>0 0 0.2 0 0 0</pose>
      </visual>

      <visual name="light_box">
        <geometry>
          <box>
            <size>0.1 0.15 0.4</size>
          </box>
        </geometry>
        <material>
          <ambient>0.2 0.2 0.2 1</ambient>
          <diffuse>0.2 0.2 0.2 1</diffuse>
        </material>
        <pose>0 0 0.4 0 0 0</pose>
      </visual>

      <visual name="red_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.1 0 0 1</ambient>
          <diffuse>0.1 0 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.52 0 0 0</pose>
      </visual>

      <visual name="yellow_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <ambient>0.1 0.1 0 1</ambient>
          <diffuse>0.1 0.1 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.4 0 0 0</pose>
      </visual>

      <visual name="green_light">
        <geometry>
          <sphere>
            <radius>0.049</radius>
          </sphere>
        </geometry>
        <material>
          <emissive>0 1 0 1</emissive>
          <ambient>0 1 0 1</ambient>
          <diffuse>0 1 0 1</diffuse>
        </material>
        <pose>-0.04 0 0.28 0 0 0</pose>
      </visual>
    </link>
  </model>
</sdf>

代码的大致思路是：emissive增加亮度，同时控制rgb的饱和度来控制每个灯泡的颜色。
如何切换红绿灯模型呢。从终端敲命令是可以的，但是我们要在程序中自动切换。gazebo本身支持python接口，但是因为文档、示例代码极其不全，导致我查遍全网也没查到怎么调用这个该死的接口。以下是一段探索过程。。。
在仿真环境启动的情况下
用：gz service --list 查看gazebo服务

/world/drive_track/remove
/world/drive_track/create

这两服务应该是用于创建和删除模型的
用gz service --info --service /world/drive_track/remove 查看消息内容

Service providers [Address, Request Message Type, Response Message Type]:
tcp://10.11.33.122:44577, gz.msgs.Entity, gz.msgs.Boolean

用gz msg --info gz.msgs.Entity 查看request消息

Name: gz.msgs.Entity
File: gz/msgs/entity.proto
- message Entity {
enum Type {
  NONE = 0;
  LIGHT = 1;
  MODEL = 2;
  LINK = 3;
  VISUAL = 4;
  COLLISION = 5;
  SENSOR = 6;
  JOINT = 7;
  ACTOR = 8;
  WORLD = 9;
}
.gz.msgs.Header header = 1;
uint64 id = 2;
string name = 3;
.gz.msgs.Entity.Type type = 4;
}

同理，查看create：
gz service --info --service /world/drive_track/create

Service providers [Address, Request Message Type, Response Message Type]:
tcp://10.11.33.122:44577, gz.msgs.EntityFactory, gz.msgs.Boolean

gz msg --info gz.msgs.EntityFactory

Name: gz.msgs.EntityFactory
File: gz/msgs/entity_factory.proto
- message EntityFactory {
.gz.msgs.Header header = 1;
oneof from {
  string sdf = 2;
  string sdf_filename = 3;
  .gz.msgs.Model model = 4;
  .gz.msgs.Light light = 5;
  string clone_name = 6;
}
.gz.msgs.Pose pose = 7;
string name = 8;
bool allow_renaming = 9;
string relative_to = 10;
.gz.msgs.SphericalCoordinates spherical_coordinates = 11;
}

理论上，我们能直接用python调用gazebo接口，直接请求这些服务的（gazebo服务不同于ros服务）。参考教程：Gazebo Transport: Python Support. 但是很可惜，根本没有教程教我gz.msgs.EntityFactory怎么发送。github上源码封装的太深了，也不会用。
还好，How to add and remove models in Gazebo simulation dynamically | Introduction to ROS2 and Robotics救了我。
既然从终端可以创建模型，那么就可以用python在终端写指令，来创建模型。参考代码如下：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import time
import os
from threading import Thread
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

class ADNode(Node):
    def __init__(self):
        super().__init__('auto_drive_node')
        ''' 
        DO NOT USE BACK_SLASH OR ENTER

        create green light:
        gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "green_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/green_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'

        create red light:
        gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "red_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/red_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'

        remove red light:
        gz service --service /world/drive_track/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 2000 --req 'type: MODEL; name: "red_light"'
        '''
        self.create_green_light_cmd = """gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "green_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/green_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'"""
        self.create_red_light_cmd = """gz service --service /world/drive_track/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 2000 --req 'name: "red_light"; sdf_filename: "/home/zwz/turtlebot4_ws/src/auto_drive/world/red_light.sdf"; pose: {position: {x: -2, y: 4, z: 0}, orientation: {x: 0, y: 0, z: 1, w: 0}}'"""
        self.remove_red_light_cmd = """gz service --service /world/drive_track/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 2000 --req 'type: MODEL; name: "red_light"'"""
        # for create and remove traffic light
        os.system(self.create_red_light_cmd)

    def change_traffic_light(self):
        time.sleep(7)
        os.system(self.create_green_light_cmd)
        os.system(self.remove_red_light_cmd)


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

    ad_node = ADNode()

    rclpy.spin(ad_node)

    ad_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

注意，因为该死的gazebo的该死的bug，命令行字符串不能有任何换行。

红绿灯和停车牌建模完成！！！

ROS2+Gazebo简易自动驾驶（4）

第四步：红绿灯、停车牌建模

停车牌建模

红绿灯建模

赵为之

引用和评论

[OpenVLA] All attempts to get a Google ... token failed

高阶智驾“破壁人”来了：13万标配激光雷达，还能「车位到车位」

YOLOv5 的量化及部署 - RGB 专题

地平线轨迹预测 QCNet 参考算法 - V2.0

智能驾驶中的感知模块介绍

地平线 3D 目标检测 bev_sparse 参考算法 - V2.0

征程 6 相比征程 5 对算子支持扩展的具体案例讲解

ROS2+Gazebo简易自动驾驶（4）

第四步：红绿灯、停车牌建模

停车牌建模

红绿灯建模

赵为之

引用和评论

[OpenVLA] All attempts to get a Google ... token failed

高阶智驾“破壁人”来了：13万标配激光雷达，还能「车位到车位」

YOLOv5 的量化及部署 - RGB 专题

地平线轨迹预测 QCNet 参考算法 - V2.0

智能驾驶中的 感知 模块介绍

地平线 3D 目标检测 bev_sparse 参考算法 - V2.0

征程 6 相比征程 5 对算子支持扩展的具体案例讲解

智能驾驶中的感知模块介绍