Carla + ros melodic仿真（一）：GNSS传感器配置

工作需要 + 理解能力检测，因此以后会推出Carla + ROS仿真系列文档

背景

在carla仿真中配置一个gnss传感器，可以正常的发出基于ros的传感器数据，用于ego_vehicle的定位。但是正常启动节点时，窗口刷新数据显示一直为0 。因此会基于解决问题的思路，分享一下解决过程

解决思路

1. 首先从启动carla-ros-bridge的自编辑launch文件分析:

roslaunch carla_ros_bridge traffic_light_demo.launch

可以得到一个加载自己编辑的opendrive格式地图的仿真界面，但是GNSS这一项是没有数据的，一直显示为(0, 0)。
(这里有数据是作者修改完以后的)

打开启动carla_ros_bridge这个node的traffic_light_demo.launch文件：
可以在launch文件末尾看到：启动carla_manual_control这个pkg的carla_manual_control.launch文件，这个启动的是用于手动控制ego_vehicle的节点，打开对应的节点代码，找到数据显示源，确定是显示的bug还是数据源的bug。

2. 打开carla_manual_control.py文件，可以看到相关的显示部分的data 来源。

可以从main方法中看到HUD类使用update_info_text成员方法，将从各个rostopic拿到的数据更新存放在成员变量self._info_tex list内，并且使用类内成员方法render(self, display)显示列表内的最新值，主要伪代码流程如下：

因此可以在update_info_text(self)成员方法内发现，GNSS显示数据来源是类内成员变量self.latitude, self.longitude：

self._info_text = [
            'Frame: % 22s' % self.carla_status.frame,
            'Simulation time: % 12s' % datetime.timedelta(
                seconds=int(rospy.get_rostime().to_sec())),
            'FPS: % 24.1f' % fps,
            '',
            'Vehicle: % 20s' % ' '.join(self.vehicle_info.type.title().split('.')[1:]),
            'Speed:   % 15.0f km/h' % (3.6 * self.vehicle_status.velocity),
            u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (yaw, heading),
            'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (x, y)),
            'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (self.latitude, self.longitude)),
            'Height:  % 18.0f m' % z,
            '']

搜索成员变量可以发现此变量在gnss_updated(self, data): 回调函数内被更新：

def gnss_updated(self, data):
        """
        Callback on gnss position updates
        """
        self.latitude = data.latitude
        self.longitude = data.longitude
        self.update_info_text()

然后可以看到此callback来源于topic: /carla/ego_vehicle/gnss1/fix，

self.gnss_subscriber = rospy.Subscriber(
            "/carla/{}/gnss/gnss1/fix".format(self.role_name), NavSatFix, self.gnss_updated)

正常启动节点，发现/carla/ego_vehicle/gnss/gnss1/fix并无数据，同时可以发现另一个关于gnss的topic:/carla/ego_vehicle/gnss正常发布数据，因此修改此处的topic即可正常显示出ego_vehicle当前位置的gnss数据。

ROS gnss数据pipeline

1. 这个虽然解决了问题，但是我们需要正向的理解这个topic数据是怎么产生的。因此打开前文所述的traffic_light_demo.launch文件，可以看到启动carla_ros_bridge节点的launch文件，打开此文件:在launch文件的末尾，可以发现通过运行bridge.py来启动节点，在main()方法内可以看到，此python文件主要是通过实例化一个类，调用其run()方法（封装了ros::spin()）来运行此节点。

carla_bridge = CarlaRosBridge(carla_client.get_world(), parameters)
carla_bridge.run()

2. 同时可以在CarlaRosBridge类内构造可以发现，其构造了一个rosservice的server端，用于生成提前定制好的object。

self.spawn_object_service = rospy.Service("/carla/spawn_object", SpawnObject, self.spawn_object)

在其self.spawn_object这个callback里可以看到：

def spawn_object(self, req):
        with self.actor_factory.spawn_lock:
            try:
                if "pseudo" in req.type:
                    id_ = self._spawn_pseudo_actor(req)
                else:
                    id_ = self._spawn_actor(req)

                self._registered_actors.append(id_)
                return SpawnObjectResponse(id_, "")

            except Exception as e:
                rospy.logwarn("Error spawning object '{}: {}".format(req.type, e))
                return SpawnObjectResponse(-1, str(e))

主要还是调用其类内成员方法self._spawn_actor(req):

def _spawn_actor(self, req):
        if "*" in req.type:
            blueprint = secure_random.choice(
                self.carla_world.get_blueprint_library().filter(req.type))
        else:
            blueprint = self.carla_world.get_blueprint_library().find(req.type)
        blueprint.set_attribute('role_name', req.id)
        for attribute in req.attributes:
            blueprint.set_attribute(attribute.key, attribute.value)
        if req.random_pose is False:
            transform = trans.ros_pose_to_carla_transform(req.transform)
        else:
            # get a random pose
            spawn_points = self.carla_world.get_map().get_spawn_points()
            transform = secure_random.choice(
                spawn_points) if spawn_points else carla.Transform()

        attach_to = None
        if req.attach_to != 0:
            attach_to = self.carla_world.get_actor(req.attach_to)
            if attach_to is None:
                raise IndexError("Parent actor {} not found".format(req.attach_to))

        carla_actor = self.carla_world.spawn_actor(blueprint, transform, attach_to)
        actor = self.actor_factory.create(
            req.type, req.id, req.attach_to, req.transform, carla_actor)
        return actor.uid

可以看到此方法是根据service传进来每一个actor的attribute，transform，attach_to属性来实例化出一个carla_actor对象，然后通过调用self.actor_factory.create()方法来生成我们所见到的actor。

3. 追本溯源，查看actor_factory类

def create(self, type_id, name, attach_to, spawn_pose, carla_actor=None):
        with self.lock:
            # check that the actor is not already created.
            if carla_actor is not None and carla_actor.id in self.actors:
                return None

            if attach_to != 0:
                if attach_to not in self.actors:
                    raise IndexError("Parent object {} not found".format(attach_to))
                parent = self.actors[attach_to]
            else:
                parent = None

            if carla_actor is not None:
                uid = carla_actor.id
            else:
                uid = next(self.id_gen)

            actor = self._create_object(uid, type_id, name, parent, spawn_pose, carla_actor)
            self.actors[actor.uid] = actor

            rospy.loginfo("Created {}(id={})".format(actor.__class__.__name__, actor.uid))

            return actor

可以看到前面都是一大串的check，实际就是调用了actor_factory类内的self.__create_object()方法来构造actor

def _create_object(self, uid, type_id, name, parent, spawn_pose, carla_actor=None):

其实我们可以看到，构造carla_actor对象的方法是：直接传入前面的uid序号，type_id即actor的类型，name名字，parent即为前面的attach_to属性表示这个actor的父关系是谁，spawn_pose为actor生成的地点，carla_actor填写none或者传入一个carla.actor对象

elif carla_actor.type_id.startswith("sensor.other.gnss"):
                actor = Gnss(uid, name, parent, spawn_pose, self.node, carla_actor, self.sync_mode)

这里是通过Gnss类来构造actor对象,打开gnss.py:

def __init__(self, uid, name, parent, relative_spawn_pose, node, carla_actor, synchronous_mode):
        """
        Constructor

        :param uid: unique identifier for this object
        :type uid: int
        :param name: name identiying this object
        :type name: string
        :param parent: the parent of this
        :type parent: carla_ros_bridge.Parent
        :param relative_spawn_pose: the relative spawn pose of this
        :type relative_spawn_pose: geometry_msgs.Pose
        :param node: node-handle
        :type node: carla_ros_bridge.CarlaRosBridge
        :param carla_actor: carla actor object
        :type carla_actor: carla.Actor
        :param synchronous_mode: use in synchronous mode?
        :type synchronous_mode: bool
        """
        super(Gnss, self).__init__(uid=uid,
                                   name=name,
                                   parent=parent,
                                   relative_spawn_pose=relative_spawn_pose,
                                   node=node,
                                   carla_actor=carla_actor,
                                   synchronous_mode=synchronous_mode)

        self.gnss_publisher = rospy.Publisher(self.get_topic_prefix(),
                                              NavSatFix,
                                              queue_size=10)
        self.listen()

    # pylint: disable=arguments-differ
def sensor_data_updated(self, carla_gnss_measurement):
        """
        Function to transform a received gnss event into a ROS NavSatFix message

        :param carla_gnss_measurement: carla gnss measurement object
        :type carla_gnss_measurement: carla.GnssMeasurement
        """
        navsatfix_msg = NavSatFix()
        navsatfix_msg.header = self.get_msg_header(timestamp=carla_gnss_measurement.timestamp)
        navsatfix_msg.latitude = carla_gnss_measurement.latitude
        navsatfix_msg.longitude = carla_gnss_measurement.longitude
        navsatfix_msg.altitude = carla_gnss_measurement.altitude
        self.gnss_publisher.publish(navsatfix_msg)

因此可以发现这里就是gnss传感器的根源，gnss类继承Sensor类，重新实现sensor_data_updated()方法，并且在方法里发布传感器数据，因此我们需要确认哪里会触发这个函数sensor_data_updated()。

4. 在父类Sensor()中我们可以看到，这是一个纯虚函数接口

 @abstractmethod
 def sensor_data_updated(self, carla_sensor_data):
        """
        Pure-virtual function to transform the received carla sensor data
        into a corresponding ROS message

        :param carla_sensor_data: carla sensor data object
        :type carla_sensor_data: carla.SensorData
        """
        raise NotImplementedError(
            "This function has to be implemented by the derived classes")

同时可以在父类Sensor()中可以看到，传感器数据分为两种更新方式，一种是和仿真时间同步更新，一种是事件同步更新。

def _update_synchronous_event_sensor(self, frame):
    self.sensor_data_updated(carla_sensor_data)
def _update_synchronous_sensor(self, frame, timestamp):
    self.sensor_data_updated(carla_sensor_data)

然后可以在下面的成员方法update()中发现，传感器通过哪种更新数据是根据传感器的类型来判断的

def update(self, frame, timestamp):
        if self.synchronous_mode:
            if self.is_event_sensor:
                self._update_synchronous_event_sensor(frame)
            else:
                self._update_synchronous_sensor(frame, timestamp)

        super(Sensor, self).update(frame, timestamp)

5. 现在问题是在主线程或者哪一个子线程会调用传感器的update方法，来实现数据的更新。让我们回到启动carla_ros_bridge节点的python文件：bridge.py。在此文件中，我们定义了CarlaRosBridge()类，同时可以在CarlaRosBridge()类内看到，仿真的更新分为两种，一种为sync_mode即同步模式(ros和carla server)，另一种为异步模式，两者存在时间差。

if self.sync_mode:
        self.carla_run_state = CarlaControl.PLAY

            self.carla_control_subscriber = \
                rospy.Subscriber("/carla/control", CarlaControl,
                                 lambda control: self.carla_control_queue.put(control.command))

            self.synchronous_mode_update_thread = Thread(
                target=self._synchronous_mode_update)
            self.synchronous_mode_update_thread.start()
        else:
            self.timestamp_last_run = 0.0

            self.actor_factory.start()

            # register callback to update actors
            self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)

同步模式是开了一个另外的线程用于更新：

self.synchronous_mode_update_thread = Thread( target=self._synchronous_mode_update)

异步模式则是直接根据carla的时间一段间隔更新一下，以此达到固定的频率

self.on_tick_id = self.carla_world.on_tick(self._carla_time_tick)

这里由于时间的原因，暂时不细致讲述两者的做法。直接看两者共同调用的_update()方法：

self._update(frame, world_snapshot.timestamp.elapsed_seconds)
self._update(carla_snapshot.frame, carla_snapshot.timestamp.elapsed_seconds)

因此查看类内成员_update方法

def _update(self, frame_id, timestamp):
        """
        update all actors
        :return:
        """
        # update world info
        self.world_info.update(frame_id, timestamp)

        # update all carla actors
        for actor_id in self.actor_factory.actors:
            try:
                self.actor_factory.actors[actor_id].update(frame_id, timestamp)
            except RuntimeError as e:
                rospy.logwarn("Update actor {}({}) failed: {}".format(
                    self.actor_factory.actors[actor_id].__class__.__name__, actor_id, e))
                continue

这里调用的其实就是actor_factory中的每一个actor的update方法。而我们在创建每一个actor的时候，actor_factory类其实就维护了一个actors的成员列表：

actor = self._create_object(uid, type_id, name, parent, spawn_pose, carla_actor)
self.actors[actor.uid] = actor

因此，也如前文代码缩写：每一个传感器也是一个actor，这里就可以回到传感器类Sensor()中的update成员方法：

def update(self, frame, timestamp):
        if self.synchronous_mode:
            if self.is_event_sensor:
                self._update_synchronous_event_sensor(frame)
            else:
                self._update_synchronous_sensor(frame, timestamp)

        super(Sensor, self).update(frame, timestamp)

时间更新和事件更新方式都是调用的子类的sensor_data_updated()方法：

self.sensor_data_updated(carla_sensor_data)

总结

综合以上的代码，我们就可以将首尾连接起来了，但是其实还差一个正向的代码流程图和仿真同步异步实现方式分析博客，因为时间关系以后会更新。