底盘驱动（tbase_slot）

ancientcc

代码参考：小程序Basic(aplt.leagor.basic)有一个底盘驱动实例tleagor_base。

底盘硬件规范，最小线速度、角速度等更多信息，参考：硬件规范。

 

一、输出规范

1.1 settings.cfg

base_driver = true

根下需存在键“base_driver”，值是bool类型的“true”。

1.2 libroseaplt.so需输出函数：aplt_create_base_slot

void* aplt_create_base_slot();

返回值是个指向aplt::tbase_slot对象的指针。

 

二、导航

导航是个很广话题，涉及众多算法，具体到底盘驱动，任务可归结为一个。

  • 订阅话题：/cmd_vel，消息类型：geometry_msgs::Twist，然后把这速度发向驱动板。

和处理速度相关，底盘驱动须重载数个api。

图1 导航相关api
  1. (主线程)会在多种场合调用get_serial_path，从而得到底盘串口路径和波特率。
  2. (主线程)会在多种场合调用tros_base_node::start，它会创建并运行线程：base_drver_node，线程函数是底盘驱动须重载的start_ros_node。为什么没进入导航就要启动节点线程？——为了读电池电压。做到机器人运行着，就要能读电压。
  3. (节点线程)start_ros_node()。这个函数行为非常像ros中已有的底盘节点，所以线程名用了“_node”字样。它一般要做几件事。1）打开/关闭串口；2）订阅话题：/cmd_vel，消息类型：geometry_msgs::Twist。收到话题后向驱动板发速度命令；3）读取电池电压。4）如果是内置imu，要读取实时欧拉角。
  4. (主线程)要开始导航了，会调用ros_instance::start_navigation_node，参数buildmap表示此次是否是建图。laser_tf、dcamera_tf是两个类型是2D位姿的输出参数。必须填写laser_tf，用于指示激光雷达到base_footprint的tf。如果有深度相机，还需要填写dcamera_tf，指示深度相机到base_footprint的tf。
  5. (start_navigation_node)调用驱动enter_navigation方法。这可分为三个操作。1）给各ros底盘变量赋默认值。2）调用aplt_enter_navigation，通过重载这方法，驱动改掉认为不符要求的默认值。3）根据robot_legnth、robot_width计算footprint。
  6. (start_navigation_node)创建数个坐标系，并发布静态tf。坐标系中有以机器人中心为原点的base_footprint，以雷达中心为原点的laser，以深度相机中心为原点的dcamera。之前得到的laser_tf、dcamera_tf用于发布laser、dcamera的静态tf。

start_ros_node是底盘节点的线程函数，带三个参数。

virtual void start_ros_node(bool& exit, const std::string& serial_path, int baudrate) = 0;
  • exit[IN]。表示是否要退出节点线程。平时false，一旦launcher希望退出线程了，会置为true。一旦检测到true，start_navigation需立即退出。
  • serial_path[IN]。可用于作为open参数的串口路径。在android，类似“/dev/ttyUSB0”。
  • baudrate[IN]。串口通信时要使用的波特率。示例：115200。

机械臂节点和雷达节点也有这么个线程函数，三个参数意义和这里的一样。

 

三、footprint、雷达和底盘中心相对位置

virtual void aplt_enter_navigation(bool buildmap, ros::tbase_cfg& base_cfg, 
    double& max_mode_vel_x, double& max_mode_vel_theta) = 0;

这个footprint就是ros中的footprint（足迹）。在ros，是用数个顶点表示footprint。在这里只让开发者给出机器人长度、宽度，rose会由它们算出ros需要的顶点。在驱动代码，让在aplt_enter_navigation中的base_cfg.robot_width、base_cfg.robot_height返回这两个值。

图2 robot_length、robot_width和footprint

图2显示了机器人长度（robot_length）、宽度（robot_width）含义，单位都是米。开发者定义robot_length=0.26、robot_width=0.28，经由这值会算出“[[-0.13, -0.14], [-0.13, 0.14], [0.13, 0.14], [0.13, -0.14]]”，把它做为ros的footprint。

在ros，至少会存在两个坐标系，以机器人底盘中心为原点的base_footprint坐标系，以雷达中心为原点的laser坐标系。可能的话，还会有以深度相机中心为原点的dcamera坐标系。enter_navigation的输出参数laser_tf返回这个laser到base_footprint的tf，dcamera_tf返回这个dcamera到base_footprint的tf。对tf值类型tpose2d，x、y、yaw分别是tf中的position.x、position.y和旋转中的yaw。底盘驱动则是在aplt_enter_navigation时填写这两个tf。

有什么方法可较快验证laser_tf？——在机器人前面横一块障碍物，在rviz，测量base_footprint到障碍物距离是真实世界距离。如果不方便测dcamera到base_footprint的tf，依旧是横一块障碍物，rviz上显示的障碍物，能够让相机、雷达近乎接近的那个x，就是要得到的pose2d.x。

 

四、imu

底盘驱动要用imu读取实时欧拉角。

virtual bool has_magnetometer() const { return false; }
virtual bool start_calib_magnetometer() { return false; }
virtual bool stop_calib_magnetometer() { return false; }

virtual bool has_angular_velocity() const { return false; }

virtual bool use_external_imu() const { return false; }
virtual void open_external_imu(const std::string& serial_path, int baudrate, bool use_magnetometer) {}
texternal_imu* external_imu() const { return external_imu_; }
virtual void close_external_imu();

virtual bool imu_can_read() const = 0;

按imu是否使用独立串口，分为内置imu和外置imu。驱动板自带、从驱动板串口读的称内置，用独立串口接入的称为外置。use_external_imu()返回值指示用的是内置(false)还是外置(true)。

有的imu支持输出融和磁力计后的欧拉角，如果这底盘能融和磁力计，让has_magnetometer()返回true。但是，对融和磁力计的，一般需要个称为校准的操作，start_calib_magnetometer、stop_calib_magnetometer对应开始、结束校准。

如果用外置imu，launcher接下会调用open_external_imu(...)打开这个独立串口，参数分别是串口路径、波特率，以及欧拉角是否要融合磁力计。打开后的外置imu存储在tbase_slot成员变量external_imu_。当要销毁时，launcher会调用close_external_imu()。

对不少imu设备，打开后，并不意味着立即可以读欧拉角。像IM600，得先发SET_CONFIG(0x12)等命令。确定欧拉角可读了，让imu_can_read()返回true。

namespace aplt {
class tvaluex
{
	double battery_level; // voltage
	double euler[3]; // radian
	double angular_vel[3]; // radian/s
};
tvaluex valuex;
}

读出的实时欧拉角存储在aplt::tvaluex::euler[3]，0、1、2分别是roll、pitch、yaw。对这三种，目前只在用yaw。

如果底盘支持输出实时角速度，那让has_angular_velocity()返回值true，并把3个角度速填写在aplt::tvaluex::angular_vel[3]。0、1、2分别是x、y、z三个方向。本计划把这三个用在cartographer的imu角速度，目前这方面还在测试，也就不会生效。现在就让has_angular_velocity()返回false。

 

五：电池电压

底盘驱动往往在非主线程读出电压，意味着须要和主线程同步。主线程可能会非常频繁地使用电池电压，如果每次读取都要同步，有点太耗资源。采取方案：底盘驱动在非主线程设置时要带同步，然后主线程隔5秒，把这个非主线程读到的电压赋值给主线程可自由访问的电压battery_level。

  1. (主线程)要开始读电压了，调用nposm_NMTHREAD_battery_level。
  2. (非主线程)[底盘驱动]读到电压了，调用set_NMTHREAD_battery_level(double level)。
  3. (主线程)调用flip_battery_level，把非主线程得到的电压设置到battery_level。因为电压不会变得非常快，5秒才调一次。
  4. (主线程)可不必同步地使用battery_level。

对小程序，如果是在非主线程读到电压，那用set_NMTHREAD_battery_level设置。如果是主线程，直接修改battery_level。如果想使用battery_level，尽量在主线程。非要在非主线程，那要把主线读到的battery_level传递过去。

 

六、和机械臂共用一个串口

机器人如何安装机械臂，这里存在两种方法。一种是底盘和机械臂共用一个串口，这可能是底盘和机械臂就同一个产家，或虽然不同产家，但底盘知道控制机械臂协议。一种是机械臂单独安装，双方有着独立串口。对编写驱动来说，第二种情况就是各写各的。以下说第一种会遇到的额外问题。

存在机械臂时，机器人须要底盘和机械臂这两个独立驱动。因为串口是同一个，它们的串口node线程运行的其实是同样逻辑，只是函数名不同罢了，底盘驱动叫tbase_slot::start_navigation，机械臂驱动叫::start_moveit。可以肯定，机械臂、底盘同时要操作时，只能运行当中一个node线程。这里就存在个问题，运行哪一个？——是先启动机械臂，还是先启动底盘导航，这是没法确定的。launcher用着以下逻辑启动底盘、机械臂。

void tros_instance::start_base_or_moveit_serial(bool base, bool buildmap)
{
    tdrivers::tvars vars = drivers_.curvars(true);
    bool is_same = vars.base.node == vars.moveit.node;

    tdrivers::tserialp* seiralp = nullptr;
    if (base) {
        if (!is_same || !moveit_driver_.started()) {
            base_driver_.start_navigation(buildmap, vars.base.node, vars.base.baudrate);
        } else {
            SDL_Log("base and moveit use same serial, and moveit has started, so base run fake");
            base_driver_.set_thread(buildmap, moveit_driver_.get_thread());
        }

    } else {
        if (!is_same || !base_driver_.started()) {
            moveit_driver_.start_moveit(vars.moveit.node, vars.moveit.baudrate);
        } else {
            SDL_Log("base and moveit use same serial, and base has started, so moveit run fake");
            moveit_driver_.set_thread(base_driver_.get_thread());
        }
    }
}

如果底盘、机械臂是两个串口(is_same=false)，那启动各自节点线程。

如果是同一个串口，谁先启动就运行谁的节点线程，后启动那个采用方法是增加该节点线程的引用计数。因为用了引用计数，即使两个都在操作，那运行的只一个节点线程。

void tbase_driver::set_thread(bool buildmap, net::tshared_worker& thread)
{
	VALIDATE_IN_MAIN_THREAD();
	VALIDATE(slot != nullptr, null_str);
	VALIDATE(thread_.get() == nullptr, null_str);
	VALIDATE(thread.get() != nullptr, null_str);

	slot->pre_start_navigation(buildmap);

	thread_ = thread;
}

以上是set_thread代码，依旧会调用底盘驱动的pre_start_navigation方法，但不再创建节点线程，只是增加个已有节点线程的引用计数。

和机械臂共用一个串口有关，探讨一个问题：为什么底盘驱动要新建一个线程去运行start_navigation。想想看，打开串口，接收/cmd_vel话题，向底盘发速度命令，这些都不是费时操作。如果能放在主线程，那读出的电压，欧拉角，因为写操作是在主线程，和主线程其它模块读它们，天然没有了同步读写问题。

  1. 在执行机械臂操作时，可能会是个阻塞式任务，像tros_instance::do_pickplace()。在这过程中，要操作机械臂了，moveit模块会通过非主线程发出/joint_states话题。可若是把接收“/joint_states”话题放在主线程，此时主线程还在阻塞中，导致这些/joint_states没法变成命令发到机械臂。当然，可以修改moveit模块，让要操作机械臂时，不再通过/joint_states话题机制，而是直接把sensor_msgs::JointState发向串口。这理论上可以用到，可这需要修改太多moveit代码。
  2. 既然机械臂节点需要新线程，机械臂极可能是和底盘同用一个串口，底盘节点也只能用新建线程。
发布于 昨天

全部评论: 0 条

    写评论: