1. 运动控制

键盘控制

启动驱动roslaunch pibot_bringup bringup.launch,下同

roslaunch pibot keyboard_teleop.launch

根据提示可以控制全向的Zues和差分的Apollo小车

手柄控制

连接手柄

连接手柄至电脑或者Raspberry Pi 3
ls /dev/input/js* -l

joystick

测试手柄

sudo jstest /dev/input/js0

手柄控制

Zeus小车
roslaunch pibot joystick.launch
Apollo小车
roslaunch pibot joystick-holonomic.launch

joystick.launch

<launch>
  <arg name="joy_config" default="joystick" />
  <arg name="joy_dev" default="/dev/input/js0" />
  <arg name="config_filepath" default="$(find pibot)/config/$(arg joy_config).config.yaml" />
  
  <node pkg="joy" type="joy_node" name="joy_node">
    <param name="dev" value="$(arg joy_dev)" />
    <param name="deadzone" value="0.3" />
    <param name="autorepeat_rate" value="20" />
  </node>

  <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node" output="screen">
    <rosparam command="load" file="$(arg config_filepath)" />
  </node>
</launch>

joystick.config.yaml

axis_linear: 1  # Left thumb stick vertical
scale_linear: 0.2
scale_linear_turbo: 1.5

axis_angular: 0  # Left thumb stick horizontal
scale_angular: 1.0

enable_button: 6  # Left trigger button
enable_turbo_button: -1

App控制

2. 里程校准

linear_calibrate

启动校准

rosrun pibot calibrate_linear.py

按照提示启动rqt_reconfigure

切换到calibrate_linear选项， 勾选start_test即可开始测试。小车按照设置的速度(speed),向前运动设定的距离(test_distance),误差不超过设定值(tolerance)

odom_linear_scale_correction为比例参数，设为默认1即可

调整参数

用尺子测量小车实际行径距离，如果与test_distance相差较大，则需要调整相关参数
对于2轮差分Apollo
differential.h

    void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
    {
        float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
        float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
        float vxy = 1000 * dxy_ave / interval;
        float vth = 1000 * dth / interval;

        odom->vel_x = vxy;
        odom->vel_y = 0;
        odom->vel_z = vth;
        float dx = 0, dy = 0;
        if (motor_dis[0] != motor_dis[1])
        {
            dx = cos(dth) * dxy_ave;
            dy = -sin(dth) * dxy_ave;
            odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
            odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
        }

        if (motor_dis[0] + motor_dis[1] != 0)
            odom->z += dth;
    }

单独向前是motor_dis[0] + motor_dis[1]应该为0

左轮向后motor_dis[0]为正，右轮向前为正

容易得到odom->x因为(-motor_dis[0] + motor_dis[1]) / 2.0，而motor_dis[0], motor_dis[1]跟一周编码器个数和轮子的直接相关，在假定一周编码器个数恒定去看下，即只与轮子直接相关

这也是为什么先进行linear_calibrate的原因

如果实际测量值<test_distance，应该如何调整轮子直径，调大？调小？

即例如实际行走了0.8m，计算出来的为1m，odom->x大了，即用来计算直径的参数大了，应该减小直径。

angular_calibrate

启动校准

rosrun pibot calibrate_angular.py

出现ImportError: No module named PyKDL错误需要 sudo apt-get install ros-kinetic-kdl-parser-py

按照提示启动rqt_reconfigure

切换到calibrate_angular选项， 勾选start_test即可开始测试。小车按照设置的速度(speed),旋转设定的角度(test_angle),误差不超过设定值(tolerance)

odom_linear_scale_correction为比例参数，设为默认1即可

调整参数

观察设计旋转的角度，如果与test_angle相差较大，则需要调整相关参数
对于2轮差分Apollo
differential.h

    void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
    {
        float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
        float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
        float vxy = 1000 * dxy_ave / interval;
        float vth = 1000 * dth / interval;

        odom->vel_x = vxy;
        odom->vel_y = 0;
        odom->vel_z = vth;
        float dx = 0, dy = 0;
        if (motor_dis[0] != motor_dis[1])
        {
            dx = cos(dth) * dxy_ave;
            dy = -sin(dth) * dxy_ave;
            odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
            odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
        }

        if (motor_dis[0] + motor_dis[1] != 0)
            odom->z += dth;
    }

旋转是odom->z为dth累加即 (motor_dis[0] + motor_dis[1]) / (2* body_radius)

先前完成了linear_calibrate，(motor_dis[0] + motor_dis[1])就固定了，现在odom->z就只与body_radius相关，且为反比关系

如果实际观察角度<test_angle，应该如何调整轮子间距，调大？调小？

即例如实际行走了345°，计算出来的为360°，odom->z大了即body_radius小了(反比)，应该增加body_radius。

备注

上述为差分轮Apollo的参数调整，全向轮Zues也类似

如果实在搞不清楚应该调大参数还是调小，那就调整参数直接测试，观察结果，这样直接也同样高效！