カムプログラムロボットをhector_slamとmove_baseで自律移動させる

TOP > てきとうにこらむ > ゲーム作りとプログラミング日記 > カムプログラムロボットをhector_slamとmove_baseで自律移動させる

カムプログラムロボットをhector_slamとmove_baseで自律制御させる

ようやくLiDARで自己推定しながらナビゲーションができるようになった、すごく大変だった pic.twitter.com/GOtczK1QOR
— てきめん (@youkidearitai) November 16, 2022

ROS(Robot Operating System)の理解が雑魚すぎるので、色々ググったりDuckDuckGoったりしてネットを漁ってきて動かせるようになった。しかしながら、一点指定したら他の場所へ行けなくなったりするなど色々あるみたいで、パラメーターのチューニングが必要っぽい。

試行錯誤しながらの実現なので、再現性はないかもしれない。ご了承下さい。

もっているもの

Raspberry Pi 3A+(2022年現在本当に手に入らないね、余ってた)
モータードライバーDRV8830を2つつけたカムプログラムロボット制御用のRaspberry Piの自作HAT
LD06 LiDARモジュール
USBシリアル通信モジュール(LD06とつなげる)
Ubuntu PC(ROSをインストール済)

ほかにはEntaniya VR220カメラがありますが、今回は関係ないので省きます。

設定すること

基本的な構成として、Raspberry Piにroscoreを置くことにしている。なぜかというと、ラジコンとしてUnityでビルドしたコントローラーを操作できるようにしているため。

Ubuntu PC

hector_slamとmove_baseをこちらで設定する。

hector_slam

https://github.com/tu-darmstadt-ros-pkg/hector_slamをインストール。
https://github.com/DaikiMaekawa/navigation2d_exampleをインストール。

ros-navigation2d-example/launch/move_base.launchの以下をコメントアウト

<!-- node pkg="roomba_500_series" type="roomba500_light_node" respawn="false" name="roomba500_light_node" output="screen"/-->

ros-navigation2d-example/launch/hector_hokuyo.launchの以下をコメントアウト

<!-- node pkg="hokuyo_node" type="hokuyo_node" name="hokuyo_node"/-->

includeの部分をちょっと修正、hector_geotiff_launchとなっているので修正

<include file="$(find hector_geotiff_launch)/launch/geotiff_mapper.launch">

default_mappingの部分も修正

<include file="$(find navigation2d_example)/launch/default_mapping.launch">
  <arg name="base_frame" value="laser"/>
  <arg name="odom_frame" value="laser"/>
</include>

default_mapping.launchを修正

<param name="base_frame" value="laser" />
<param name="odom_frame" value="laser" />

paramの部分も修正

param/global_costmap_params.yaml

global_frame: map
robot_base_frame: scanmatcher_frame

param/local_costmap_params.yaml

global_frame: map
robot_base_frame: scanmatcher_frame

あとはexportで環境変数を調節する

export ROS_HOSTNAME=192.168.111.12 # 手元のHOSTNAME
export ROS_MASTER_URI=http://ubuntu.local:11311 # MASTERのURI

実行、まとめてrvizなども実行される

$ roslaunch navigation2d_example move_base.launch

Raspberry Pi

LD06をこちらで設定する。~/catkin_ws/src/hector_slam/hector_slam_launch/hector_slam.launchというファイル名になってた。（そもそもRaspberry Pi側にもhector_slam必要なのだろうか？）

<launch>
  <arg name="topic_name" default="scan" />
  <!-- X2L -->
  <node name="LD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_node" output="screen" respawn="false" >
    <param name="product_name" value="LDLiDAR_LD06" />
    <param name="port_name"         type="string" value="/dev/ttyUSB0"/>
    <!-- param name="baudrate"         type="int" value="115200"/ -->
    <param name="inverted"            type="bool"   value="false"/>
    <param name="laser_scan_dir" type="bool" value="false" />
    <param name="angle_compensate"    type="bool"   value="true"/>
    <param name="topic_name" value="$(arg topic_name)" />
    <param name="frame_id"     type="string" value="laser"/>
    <param name="resolution_fixed"    type="bool"   value="true"/>
    <param name="auto_reconnect"    type="bool"   value="true"/>
    <param name="reversion"    type="bool"   value="false"/>
    <param name="angle_min"    type="double" value="-180" />
    <param name="angle_max"    type="double" value="180" />
    <param name="range_min"    type="double" value="0.1" />
    <param name="range_max"    type="double" value="12.0" />
    <param name="ignore_array" type="string" value="" />
    <param name="frequency"    type="double" value="8"/>
    <param name="samp_rate"    type="int"    value="3"/>
    <param name="isSingleChannel"    type="bool"   value="true"/>
  </node>

  <!-- tf -->
  <node pkg ="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0.0 0.0 0.0 /map /nav 5"/>
  <node pkg ="tf" type="static_transform_publisher" name="odom_to_base_link" args="0.0 0.0 0.0 0.0 0.0 0.0 /nav /base_footprint 5"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.2245 0.0 0.2 0.0 0.0 0.0 /base_footprint /base_link 5" />
</launch>

cmd_velを受信するサブスクライバーを書く、motorというパッケージを作成してcmd_vel.pyというファイル名にしておいた。

#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistStamped, Twist, Vector3

from drv8830 import DRV8830, I2C_ADDR2, I2C_ADDR4
import time

one = DRV8830(i2c_addr=I2C_ADDR2)
two = DRV8830(i2c_addr=I2C_ADDR4)

one.set_voltage(0.7)
two.set_voltage(0.7)

before = None

def callback(msg):
    global before

    v_r = (-1*msg.angular.z + msg.linear.x)
    v_l = (msg.angular.z + msg.linear.x)

    if v_r > 0 and v_l > 0 and before != "forward":
        rospy.loginfo("forward")
        before = "forward"
        one.forward()
        two.forward()
    elif v_r > 0 and v_l < 0 and before != "right":
        rospy.loginfo("right")
        before = "right"
        two.forward()
        one.reverse()
    elif v_r < 0 and v_l > 0 and before != "left":
        rospy.loginfo("left")
        before = "left"
        two.reverse()
        one.forward()
    elif v_r < 0 and v_l < 0 and before != "back":
        rospy.loginfo("back")
        before = "back"
        one.reverse()
        two.reverse()

    if v_r == 0 and v_l == 0 and before != "stop":
        rospy.loginfo("stop")
        before = "stop"
        one.coast()
        two.coast()


NODE_NAME_STR = "automotor"
CMD_VEL_STR = "cmd_vel"

rospy.init_node(NODE_NAME_STR, anonymous=False)

try:
    rospy.Subscriber(CMD_VEL_STR, Twist, callback)
except:
    import traceback
    traceback.print_exc()

rospy.spin()

それで両方とも起動する

$ roslaunch hector_slam_launch hector_slam.launch &
$ rosrun motor cmd_vel.py &

参考サイト

参考サイトがなかったら多分実現してなかったと思います。ありがとうございます。

2022/11/18 1:39