この記事の目的

この記事の目的

　ROSで移動ロボットの経路を設定する方法についてまとめる

1. move_baseにおけるロボットの移動経路

前回の投稿でROSシミュレーション上で手動で設定した速度・角速度でロボットを動かすことができたので、次はロボットが指定した経路を追従するシミュレーションについて検討する。ROSのmove_baseでは、ロボットが走行する経路はglobal_plannerを用いて計算する。

wiki.ros.org

global_plannerは地図データから障害物のある確率を『コストマップ』として認識し、通行可能な領域からゴールまでの移動コストが小さくなる経路を探索的に求める。アルゴリズムとしては、A* search、ダイクストラ法などが用いられる。

wiki.ros.org

得られた経路は『Path』というデータ構造でROS Topicとしてpublishされる。保持している情報は下記のようになる。

docs.ros.org

要素	説明
header	タイムスタンプ、座標系を保持する
PoseStamped	header情報とpose情報

前回のシミュレーション環境で地図データにglobal_plannerを用いて経路を求めることもできるが、直線的な経路が選ばれるなどユーザーが狙った経路をうまく導出できない場合が多い。今回はユーザーが手動で操作した結果をそのまま走行経路として設定する方法について検討する。

2. ロボットの走行結果を保存する

前回の投稿で作成したユーザーがシミュレーション上で車両を動かすシミュレーションの結果をcsvファイルに保存する機能を追加する。もともと各時刻における位置座標・速度・角速度を計算しているのでcsvに保存する処理を下記のように加えるだけで対応できる。

class Simple_odom_simulator():

    ##################
    # Initialization #
    ##################
    def __init__(self):

    #中略
        if self.save_path_as_csv == True:
            self.path_dict = {}
    #中略

    def update_odom(self):
    #中略
        #Save path_plan
        if self.save_path_as_csv == True:
            addRow = [0,self.sim_pose.position.x,self.sim_pose.position.y,0,updated_quaternion[0],updated_quaternion[1],updated_quaternion[2],updated_quaternion[3],
                      self.sim_twist.linear.x,self.sim_twist.linear.y,self.sim_twist.linear.z,0,0,self.sim_twist.angular.z]
            self.path_dict[len(self.path_dict)] = addRow
            pass

    #中略
    def save_csv(self):
        # Save CSV path file
        cols = ["time", "x", "y", "z", "w0", "w1", "w2", "w3", "vx", "vy", "vz", "roll", "pitch", "yaw"]
        df = pd.DataFrame.from_dict(self.path_dict, orient='index',columns=cols)
        df.to_csv("path_data.csv", index=False)

上記では各時刻のposeの値をdict型データに保存し、ユーザー操作終了時にpandasの関数でcsvファイルに保存している。

3. ロボットの走行結果を走行経路(Path)としてPublishする

次に、保存したcsvファイルを読み込みPathを出力するスクリプトは下記のように記述できる。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import pandas as pd

# import for ros function
import rospy
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
from nav_msgs.msg import Path

#########################
# Simple Path publisher #
#########################
class Simple_path_simulator():

    ##################
    # Initialization #
    ##################
    def __init__(self):

        rospy.init_node('Simple_Path_Publisher', anonymous=True)
        self.r = rospy.Rate(50)  # 50hz
        #Initialize odometry header
        self.path_header = Header()
        self.path_header.seq = 0
        self.path_header.stamp = rospy.Time.now()
        self.path_header.frame_id = "map"

        self.path = Path()
        self.path.header = self.path_header

        #get pose data from csv
        self.csv_path_data = pd.read_csv("path_data.csv")
        pose_list = self.get_poses_from_csvdata()
        self.path.poses =pose_list

        #initialize publisher
        self.path_pub = rospy.Publisher("/path", Path, queue_size=50)

    #############################################
    # Update odometry form User request cmd_vel #
    #############################################
    def get_poses_from_csvdata(self):
        #Get poses from csv data
        poses_list = []
        for indx in range(len(self.csv_path_data)):
            temp_pose = PoseStamped()
            temp_pose.pose.position.x = self.csv_path_data["x"][indx]
            temp_pose.pose.position.y = self.csv_path_data["y"][indx]
            temp_pose.pose.position.z = self.csv_path_data["z"][indx]
            temp_pose.pose.orientation.x = self.csv_path_data["w0"][indx]
            temp_pose.pose.orientation.y = self.csv_path_data["w1"][indx]
            temp_pose.pose.orientation.z = self.csv_path_data["w2"][indx]
            temp_pose.pose.orientation.w = self.csv_path_data["w3"][indx]
            temp_pose.pose.orientation.w = self.csv_path_data["w3"][indx]
            temp_pose.header = self.path_header
            temp_pose.header.seq = indx
            poses_list.append(temp_pose)
        return poses_list

    def publish_path_topic(self):
        self.path_pub.publish(self.path)
        self.r.sleep()


if __name__ == '__main__':
    print('Path Publisher is Started...')
    test = Simple_path_simulator()
    try:
        while not rospy.is_shutdown():
            test.publish_path_topic()
    except KeyboardInterrupt:
        print("finished!")