SLAMで作成したMapでNavigation2 -自律移動修正編- (ROS2-Foxy)

はじめに
前提条件
/cmd_vel の転送プログラムを作成
Path-Planning でロボットを動かしてみよう
1. 自己位置推定
2. 自律移動
おわりに

はじめに

前回の記事では、Navigation2 パッケージを用いて

amcl パッケージで自己位置推定
Path-Planning で経路計画

を実行しました。

今回の記事では、/cmd_vel を自作ロボットに転送できるようにします。

前提条件

前提条件は、以下の通りです。

前回までの記事が完了している
Path-Planning が起動すると、/cmd_vel がパブリッシュされている

/cmd_vel の転送プログラムを作成

以前の記事ですが、こちらで translate_cmd_vel.py を作成したと思います。

translate_cmdvel.py を以下のように変更してください。

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class cmdvel2Rover(Node):
    def __init__(self):
        # wake up node name
        super().__init__("Translate_cmdvel2_node")
        # declare publisher
        self.publisher = self.create_publisher(Twist, "/first_robot/cmd_vel", 40)
        # declare subscriber
        self.subscription = self.create_subscription(Twist, "/cmd_vel", self.get_scan_values, 40)
        # period publisher
        timer_period = 0.2
        self.timer = self.create_timer(timer_period, self.send_cmd_vel)
        # creating a message object to fit new velocities and publish them
        self.velocity = Twist()

        self.linear_x = 0.0
        self.linear_y = 0.0
        self.linear_z = 0.0
        self.angle_x = 0.0
        self.angle_y = 0.0
        self.angle_z = 0.0

    def get_scan_values(self, scan_data):
        # separate scan data
        self.linear_x = scan_data.linear.x
        self.linear_y = scan_data.linear.y
        self.linear_z = scan_data.linear.z
        self.angle_x = scan_data.angular.x
        self.angle_y = scan_data.angular.y
        self.angle_z = scan_data.angular.z

    def send_cmd_vel(self):
        # setting linear velocity
        self.velocity.linear.x = self.linear_x
        self.velocity.linear.y = self.linear_y
        self.velocity.linear.z = self.linear_z
        self.velocity.angular.x = -self.angle_x/2
    　  self.velocity.angular.y = -self.angle_y/2
        self.velocity.angular.z = -self.angle_z/2

        self.publisher.publish(self.velocity)


def main(args=None):
    rclpy.init(args=args)
    oab = cmdvel2Rover()
    rclpy.spin(oab)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

ここまできたら、説明しなくてもわかるかとは思いますが、説明しておきます。

# declare publisher
self.publisher = self.create_publisher(Twist, "/first_robot/cmd_vel", 40)
# declare subscriber
self.subscription = self.create_subscription(Twist, "/cmd_vel", self.get_scan_values, 40)
# period publisher
timer_period = 0.02
self.timer = self.create_timer(timer_period, self.send_cmd_vel)

self.pubisher は、”/first_robot/cmd_vel” へパブリッシュするインスタンスです。
self.subscription は、 “/cmd_vel” をサブスクライブするインスタンスです。
一定時間ごとに self.get_scan_values 関数を起動します。

self.create_timer で 0.02s 毎に self.send_cmd_vel 関数を起動します。

def get_scan_values(self, scan_data):
    # separate scan data
    self.linear_x = scan_data.linear.x
    self.linear_y = scan_data.linear.y
    self.linear_z = scan_data.linear.z
    self.angle_x = scan_data.angular.x
    self.angle_y = scan_data.angular.y
    self.angle_z = scan_data.angular.z

単純に、scan した Twist型のデータをTwist型の変数へ代入するのみです。

def send_cmd_vel(self):
    # setting linear velocity
    self.velocity.linear.x = self.linear_x
    self.velocity.linear.y = self.linear_y
    self.velocity.linear.z = self.linear_z
    self.velocity.angular.x = -self.angle_x/2
    self.velocity.angular.y = -self.angle_y/2
    self.velocity.angular.z = -self.angle_z/2

    self.publisher.publish(self.velocity)

こちらも同様に、Twist型のデータを /first_robot/cmd_vel にパブリッシュします。

angle をなぜ負に反転させ、1/2 しているかは、実際に動かした時に説明します。
ここまでやってきてあれなのですが、凡ミスです。

translate_cmd_vel.py は既に setup.py に登録されているので、launch ファイルで同時起動できるようにしておきます。

first_navigation.launch.py を以下のように変更してください。

Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    arguments=['-d', rviz],
    parameters=[{'use_sim_time': use_sim_time}],
    output='screen'),
↓
Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    arguments=['-d', rviz],
    parameters=[{'use_sim_time': use_sim_time}],
    output='screen'),
Node(
    package='ros2_first_test',
    executable='translate_cmdvel',
    name='translate_cmdvel',
    output='screen'),

これで完了です！

Path-Planning でロボットを動かしてみよう

ここまできたら、準備は完了です。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_navigation.launch.py

自己位置推定は以下のGIFをご確認ください。

自己位置推定

ここで一つ問題があります。

amcl で推定されたロボット位置を拡大して確認してみます。

そうです。向きが逆です。
URDF の向きが違うので逆になっています。。。(凡ミスです。。。)

なので矢印の向きを逆にして 2D Pose Estimate を実行してください。

自律移動

上記の自己位置推定までを実行していただければ、以下のようにロボットが動くと思います。

そうなんです、バック走しています…。

自律走行後は、以下のようになっています。

完成です！

おわりに

今回は Navigation2 パッケージを用いて自律走行をシミュレートしました。

次回は、URDF ファイルと world ファイルを編集して、バック走ではなく前向きに走行するようにします。