导航

    蓝鲸ROS机器人论坛

    • 注册
    • 登录
    • 搜索
    • 版块
    • 话题
    • 热门
    ROS交流群
    ROS Group
    产品服务
    Product Service
    开源代码库
    Github
    官网
    Official website
    技术交流
    Technological exchanges
    激光雷达
    LIDAR
    ROS教程
    ROS Tourials
    深度学习
    Deep Learning
    机器视觉
    Computer Vision

    小强ROS机器人教程(27)___bw_auto_dock自动充电功能包的使用和实现原理

    产品服务
    最新动态
    33
    128
    309709
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • xiaoqiang
      xiaoqiang 最后由 xiaoqiang 编辑

      点击此处购买自动充电模块套件

      1.jpg
      尺寸图:
      (尾部两个4mm通孔用于固定小车底板尾部,5.5mmDC头接车载电池)
      2.jpg
      图形1.png

      自动充电模块串口通信协议v1.2.pdf,2020年新版

      自动充电模块串口通信协议V1.0.pdf,2020年之前老版本

      自动充电串口质检工具.exe

      ROS驱动包

      【使用与实现原理】

      2018年12月之后购买的用户,充电模块全部升级到了2代,2代相比一代使用超声波替换了碰撞开关,减少了模块机械损坏机率。2代支持同时测量电池电压和充电输入电压,1代只支持测量充电电压。电压测量值以ros话题形式发布,具体请看驱动包readme文件。

      1. 将自动充电模块安装在小车尾部,把充电桩固定好,接好电源。小车充电过程会接触充电桩,因此充电桩需要固定不能左右、前后移动,最好是靠墙放置。充电桩前面需要至少预留1米乘以1.5米的自由活动空间。

      0_1533288521912_充电桩放置示意图.png

      2. 在主机ros catkin工作空间中下载安装自动充电模块的ros驱动包

      如果是已经配置好的小强用户,可以直接跳过本步骤。

      #假设catkin工作空间在~/Documents/ros
      cd  ~/Documents/ros
      cd src/
      git clone http://git.bwbot.org/publish/bw_auto_dock.git
      #切换到对应分支,1代切换到master,2代切换到version2分支,如果ros版本是melodic以上版本用noetic-version2分支
      git checkout noetic-version2 #语法“git checkout 分支名字”
      cd ..
      catkin_make  
      
      

      3.将模块通过usb转串口模块接入主机usb,添加udev规制,将串口号映射为/dev/ttyUSB004,同时根据插入的usb端口号进行端口绑定。参考:这里

      如果是已经配置好的小强用户,可以直接跳过本步骤。

      #先安装串口设置包
      sudo apt-get install setserial
      #将当前计算机用户名添加进dailout用户组,以xiaoqiang为例
      sudo adduser xiaoqiang dialout
      
      #查看串口所在usb端口,即下条命令输出结果中/ttyUSBXXX字符前面的数字例如2-2:1.0,/ttyUSBXXX需要替换成实际端口号
      udevadm info --name=/dev/ttyUSBXXX --attribute-walk
      
      #根据这个端口号和映射信息/dev/ttyUSB004建立udev规则文件/etc/udev/rules.d/95-persistent-serial.rules,下面内容可供参考。对于非小强用户,xiaoqiang需要更换成自己的计算机用户名
      ACTION!="add", GOTO="persistent_serial_end"
      SUBSYSTEM!="tty", GOTO="persistent_serial_end"
      KERNEL!="ttyUSB[0-9]*", GOTO="persistent_serial_end"
      
      # This is old 11.10 style: IMPORT="usb_id --export %p"
      IMPORT{builtin}="path_id"
      KERNELS=="2-2:1.0"         ,SYMLINK+="ttyUSB004" , OWNER="xiaoqiang" ,RUN+="/bin/setserial /dev/ttyUSB004 low_latency"
      LABEL="persistent_serial_end"
      
      
      #最后重新加载udev规则
      udevadm control --reload-rules
      #重新插拔u转串模块 如果 ls /dev可以看到ttyUSB004设备,说明操作成功。
      

      4.启动ros驱动节点,开始测试自动充电功能

      对于小强用户,可以直接启动xiaoqiang_local.launch进行测试。
      对于其它平台的用户,请根据xiaoqiang_local.launch里面的参数注释,把里程计和安装参数修改一下。

      roslaunch bw_auto_dock xiaoqiang_local.launch
      

      手动设置充电桩位置:节点启动成功后,先遥控小车到充电桩附近,小车可以不用接触上充电桩,保证充电桩已经很接近小车充电部位(相差10cm左右),现在可以通过运行下列命令手动发布充电桩位置保存指令,这样小车会把当前位置记录为充电桩位置。

      rostopic pub  /bw_auto_dock/dockposition_save   std_msgs/Bool   '{data: true}'  -1
      #保存成功后会在launch文件设置的目录里面生成一个文件,里面有位置数据。
      #只有在红外传感器收到数据而且里程计消息有效时才会保存数据
      

      遥控小车到一个随机位置,运行下列命令后,小车会自动进入自动充电模式,然后尝试自动对准充电桩。

      rostopic pub  /bw_auto_dock/EnableCharge   std_msgs/Bool   '{data: true}'  -1
      

      运行下列命令可以使小车退出自动充电模式,现在又可以返回到上一步,继续发布自动充电命令。

      rostopic pub  /bw_auto_dock/EnableCharge   std_msgs/Bool   '{data: false}'  -1
      

      5.自动充电原理介绍

      充电桩会广播一组红外信号,小车上的自动充电模块有4个红外接收器。通过分析这四个红外接收器接收的红外信号,小车可以确定自己相对充电桩的位置。

      0_1533288551997_自动充电原理介绍.png

      安装在小车上的自动充电模块有充电电压、充电电流检测功能,还有两个碰撞检测开关。
      整个自动充电控制逻辑是这样的:

      a. 设置充电桩位置后,bw_auto_dock会自动设定两个参考点,充电桩位于这两个参考点连线一侧的中间位置。
      b. 每次接收到自动充电指令后,小车会自动在这两个参考点之间循环运动。
      c. 当L1或者R1接收器接收到充电桩的中间位置信号后,小车会判断已经到达充电桩正前方,会先后退一段距离(back_distance长度),然后原地旋转使R2、L2接收器正对充电桩。
      d. 小车开始直线后退对准充电桩,期间根据R2、L2接收的信号进行pid对准控制。
      e. 如果侦测到充电电压、电流,就停止移动,开始充电。如果触发了碰撞开关,同时还没有侦测到电压和电流就回到步骤b。

      6.常见问题

      a.back_distance该如何设置?
         back_distance对应的是超声波模块到base_link的距离,这个距离决定了上文5.c步骤小车后退距离。如果发现小车每次5.c旋转后都没有正对充电桩,说明这个参数设置的过大或过小,手动调一下back_distance参数(一般是减去过调的距离)。
      
      b.在步骤5.e中机器人经常没有碰到充电桩就不后退了反而立即离开,即crash_distance该如何设置?
         crash_distance是一个碰撞检测阈值,在5.e步骤中,如果超声波探测的值小于crash_distance就认为是触发了碰撞开关。
         如果5.e中机器人经常没有碰到充电桩就不后退了反而立即离开,说明crash_distance值设置的太大了,尝试把它改小一些(每次减少10)。
         如果5.e中机器人已经碰到墙了,还在不断后退,说明crash_distance值设置的太小了,尝试把它改大一些(每次增加10)。
         在rqt_console里面打开dock_driver节点的debug输出,可以查看超声波模块的当前测量值。
      

      超声波距离.png

      7.升级控制算法

      xq5分支代码是控制算法升级后的版本(ros melodic以上版本需要用noetic分支代码),同时整合提供了service和action接口,还支持手动对接充电,具体请阅读代码了解。

      #编译xq5分支代码(或者noetic分支),所依赖的ros包从这里git clone下载后catkin_make编译
      http://git.bwbot.org/publish/bw_auto_dock.git  #noetic分支
      https://gitee.com/BluewhaleRobot/galileo_serial_server.git
      https://gitee.com/BluewhaleRobot/galileo_msg.git
      
      HarryLee 1 条回复 最后回复 回复 引用 0
      • caom26
        caom26 最后由 编辑

        此回复已被删除!
        1 条回复 最后回复 回复 引用 0
        • 强哥
          强哥 最后由 编辑

          请看上面的驱动包,都有的😁

          1 条回复 最后回复 回复 引用 0
          • caom26
            caom26 最后由 编辑

            您好 我执行到第二步的catkin_make就报错了 请问您能告知一下怎么解决吗
            0_1535428420033_341ebe01-6a57-43e5-ba50-502168f9760c-image.png

            bot weijiz 2 条回复 最后回复 回复 引用 0
            • bot
              bot @caom26 最后由 编辑

              @caom26 装的是小强的镜像么

              切克闹

              caom26 1 条回复 最后回复 回复 引用 0
              • weijiz
                weijiz @caom26 最后由 编辑

                @caom26 这是由于缺少geometry_msgs包

                sudo apt-get install ros-kinetic-geometry-msgs
                

                这个是kinetic版本的指令,如果不是kinetic版本需要根据版本进行修改。
                如果有其他提示缺少软件包的情况也要对应安装。
                也可以加入qq群538456117,交流

                caom26 1 条回复 最后回复 回复 引用 0
                • caom26
                  caom26 @weijiz 最后由 编辑

                  @weijiz 谢谢您!我试了一下 还是不行 可能不是这个版本的 请问如何修改呢 我是按照上面的步骤一步一步执行的

                  weijiz 1 条回复 最后回复 回复 引用 0
                  • caom26
                    caom26 @bot 最后由 编辑

                    @bot 2. 在主机ros catkin工作空间中下载安装自动充电模块的ros驱动包
                    如果是已经配置好的小强用户,可以直接跳过本步骤。

                    #假设catkin工作空间在~/Documents/ros
                    cd ~/Documents/ros
                    cd src/
                    git clone https://github.com/BlueWhaleRobot/bw_auto_dock.git
                    cd …
                    catkin_make

                    我就是按照这个装的

                    1 条回复 最后回复 回复 引用 0
                    • weijiz
                      weijiz @caom26 最后由 编辑

                      @caom26 如果是melodic版本需要执行

                      sudo apt-get install ros-melodic-geometry-msg
                      

                      首先要确定自己安装的是什么版本的ROS。

                      caom26 1 条回复 最后回复 回复 引用 0
                      • caom26
                        caom26 @weijiz 最后由 编辑

                        @weijiz 谢谢谢谢 我已经解决了

                        1 条回复 最后回复 回复 引用 0
                        • kevin
                          kevin 最后由 编辑

                          请问下,我按照教程上运行

                          roslaunch bw_auto_dock xiaoqiang_local.launch
                          

                          后发布

                          rostopic pub  /bw_auto_dock/dockposition_save   std_msgs/Bool   '{data: true}'  -1
                          

                          接着再发布

                          rostopic pub  /bw_auto_dock/EnableCharge   std_msgs/Bool   '{data: true}'  -1
                          

                          小车没有反应?(使用的自己搭建的平台,树莓派3B主机indigo版本,arduino开发板,小车控制是cmd_val)

                          xiaoqiang 1 条回复 最后回复 回复 引用 0
                          • xiaoqiang
                            xiaoqiang @kevin 最后由 xiaoqiang 编辑

                            @kevin launch里面的里程计话题有没有改成自己小车的里程计名字,还有slamdb文件夹创建一下,发布dockposition_save命令后看一下里面是否有一个txt文件dock_station.txt。

                            mkdir  ~/slamdb
                            

                            dock_station.txt文件里面第三行,存储了当前充电桩位置,这行三个数据依次为x、y、角度。

                            kevin 1 条回复 最后回复 回复 引用 0
                            • kevin
                              kevin @xiaoqiang 最后由 编辑

                              @xiaoqiang 用的是ros_arduino_bridge,默认也是odom

                              xiaoqiang 1 条回复 最后回复 回复 引用 0
                              • xiaoqiang
                                xiaoqiang @kevin 最后由 编辑

                                launch文件贴一下

                                kevin 1 条回复 最后回复 回复 引用 0
                                • kevin
                                  kevin @xiaoqiang 最后由 编辑

                                  @xiaoqiang

                                  <launch>
                                    <node pkg="bw_auto_dock" type="bw_auto_dock" name="dock_driver">
                                      <!-- 充电部位到base_link位置的距离,base_link一般位于两个主动轮轴中间-->
                                      <param name="back_distance" value="0.40"/>
                                      <!-- 自动充电对准过程中的最大直线速度-->
                                      <param name="max_linearspeed" value="0.20"/>
                                      <!-- 自动充电对准过程中的最大旋转速度-->
                                      <param name="max_rotspeed" value="1.0"/>
                                      <!-- 自动充电直线运动对准阶段中的pid参数-->
                                      <param name="back_dock_kp" value="0.20"/>
                                      <param name="back_dock_ki" value="0.04"/>
                                      <param name="back_dock_kd" value="0.0"/>
                                      <!-- 自动充电设备串口号-->
                                      <param name="port" value="/dev/ttyUSB004"/>
                                      <!-- 里程计所在坐标系名字-->
                                      <param name="odom_frame_id" value="odom"/>
                                      <!-- 里程计话题名字-->
                                      <remap from="/odom" to="/xqserial_server/Odom" />
                                      <!-- 保存有充电桩位置的文件名,全路径在"~/slamdb/" -->
                                      <param name="station_filename" value="dock_station.txt"/>
                                      <param name="grid_length" value="4.0"/>
                                    </node>
                                    <!-- 设为0关闭底盘红外,设为1启动底盘红外-->
                                    <!-- <node pkg="nav_test" type="barDetectChanger.py" name="dock_barDetectChanger_node">
                                      <param name="barDetectFlag" value="0"/>
                                    </node> -->
                                  </launch>
                                  
                                  xiaoqiang 1 条回复 最后回复 回复 引用 0
                                  • xiaoqiang
                                    xiaoqiang @kevin 最后由 xiaoqiang 编辑

                                    @kevin 上面launch没有remap那个里程计话题名字,{remap from=“/odom” to=“/xqserial_server/Odom” }改成{remap from=“/odom” to=“你的里程计话题名字” }

                                    kevin 2 条回复 最后回复 回复 引用 0
                                    • kevin
                                      kevin @xiaoqiang 最后由 编辑

                                      @xiaoqiang 谢谢!小车可以行走了。

                                      1 条回复 最后回复 回复 引用 0
                                      • kevin
                                        kevin @xiaoqiang 最后由 编辑

                                        @xiaoqiang 还有个问题:模块一定要装在尾部吗?如果装在头部或者其他位置,应该做什么改变?

                                        xiaoqiang 1 条回复 最后回复 回复 引用 0
                                        • xiaoqiang
                                          xiaoqiang @kevin 最后由 编辑

                                          @kevin 暂时只支持尾部

                                          1 条回复 最后回复 回复 引用 0
                                          • Leslie
                                            Leslie 最后由 编辑

                                            1 条回复 最后回复 回复 引用 0
                                            • 1
                                            • 2
                                            • 3
                                            • 4
                                            • 5
                                            • 6
                                            • 7
                                            • 1 / 7
                                            • First post
                                              Last post
                                            Copyright © 2015-2023 BlueWhale community