移動ロボットのナビゲーション

前回のページでは，ジョイスティックなどでシミュレータ上の移動ロボットを動かすことができるまでを紹介した．

今回は，ジョイスティックなどで移動した経路を保存し，

保存した経路の情報に沿って自律的に移動するプログラムを紹介する．

１．移動した軌跡を保存しよう！

まず，いろいろなナビゲーションのやり方が本などで紹介されていますが，

rvizを使ったゴール位置の指定の方法ばかり紹介されているので，

つくばチャレンジ組がよく使う，「一度走ったところは障害物が無いはず理論」を使って

移動経路を保存しておき，同じ環境で自律走行するときに，以前の経路を追従する

プログラムを作ってみたいと思います．

移動経路の取得方法は，移動ロボットの車輪の回転を計測するセンサから

「オドメトリ」情報を取得するのが一般的です．

実機で車輪の回転角度からオドメトリ情報をpublishするには

実際の回転角センサの仕様を理解してプログラムする必要があるのですが

今回はGazebo上の移動ロボットを想定して話を進めます．

前回作った移動ロボットはurdfでロボットの仕様を決定した時，

最後のほうに記載した<gazebo>タグの中にodometryに関する記載がしてあります．

この記載をしておくだけで，/odomトピックとしてgazeboのシミュレータノードから

オドメトリ情報がpublishされてきます．

設定されたタイヤの半径などの情報を利用して移動量が算出されているので，実機を作るよりも簡単です．

オドメトリの定義は下記の通り．

ーーーーーーーーーーーーーーーーーーーーーーー

std_msgs/Header header

string child_frame_id

geometry_msgs/PoseWithCovariance pose

geometry_msgs/TwistWithCovariance twist

ーーーーーーーーーーーーーーーーーーーーーーー

poseは，child_frame_idに登録されているフレームの位置と姿勢を，headerの中で定義されているframe_idに対して示すようになっている。

今回の例ではframe_idはmap，child_frame_idはbase_footprintになっています．

poseはPoseWithCovariance型の中のPose型により原点位置 Point（x，y）と姿勢をQuaternionで示しています．

twistは，移動ロボットの速度となります．

どうなっているのか実際に知るためコマンドラインでトピック情報を確認してみる．

前ページの移動ロボットのシミュレーションが立ち上がっている状態で下記のコマンドでトピック情報を表示．

$ rostopic echo /odom

下記のようにodomのトピック情報が確認できる．

このオドメトリ情報を一定時間間隔で保存していけば，移動した軌跡として後で利用することができます．

一定時間間隔で保存するとずっと止まっている時も位置情報を保存してしまうので，

今回は，一定間隔＋以前保存した場所から0.5m以上離れた時に保存するという方法で軌跡を保存していきたいと思います．

まず，odometryのトピックをノードで受信できるか確認するプログラムを作ってみます．

$ cd ~/catkin_ws/src

$ catkin_create_pkg odom_listener roscpp std_msgs geometry_msgs

$ cd odom_listener/src && gedit odom_listener.cpp

odom_listener.cppの内容は下記の通り．

odom_listener.cpp

#include <ros/ros.h>

#include <sensor_msgs/Joy.h>

#include <geometry_msgs/Twist.h>

#include <nav_msgs/Odometry.h>

class odom_listener{

private :

ros::Subscriber sub;

nav_msgs::Odometry odom;

public :

odom_listener();

void odom_loop();

private :

void odomCallback(const nav_msgs::Odometry &odom_msg);

};

odom_listener::odom_listener(){

ros::NodeHandle n;

sub = n.subscribe("odom", 1, &odom_listener::odomCallback, this);

}

void odom_listener::odom_loop() {

ros::Rate loop_rate(20); //20Hzのループ

while(ros::ok()) {

ros::spinOnce();

loop_rate.sleep();

}

return;

}

void odom_listener::odomCallback(const nav_msgs::Odometry &odom_msg) {

ROS_INFO("odom : x %lf : y %lf\n", odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);

return;

}

int main(int argc, char **argv) {

ros::init(argc, argv, "odom_listener");

odom_listener odometry;

odometry.odom_loop();

return (0);

}

CMakeLists.txtも下記のように修正

$ cd ~/catkin_ws/src/odom_listener && gedit CMakeLists.txt

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(odom_listener)

find_package(catkin REQUIRED COMPONENTS

geometry_msgs

roscpp

std_msgs

)

catkin_package(

INCLUDE_DIRS include

LIBRARIES odom_listener

CATKIN_DEPENDS geometry_msgs roscpp std_msgs

DEPENDS system_lib

)

include_directories(

${catkin_INCLUDE_DIRS}

)

add_executable(odom_listener src/odom_listener.cpp)

target_link_libraries(odom_listener

${catkin_LIBRARIES}

)

それではコンパイルして，実行してみましょう．

$ cd ~/catkin_ws && catkin_make

$ rosrun odom_listener odom_listener

前のページにある移動ロボットのシミュレーションを立ち上げると，オドメトリのデータが下記のように表示されることが確認できます．

オドメトリのデータが取得できることがわかったので，移動した軌跡をファイルに保存していくプログラムを作る．

他の人が作っている移動軌跡に追従するノードを利用（手抜きをする）ため，

オドメトリのデータはyamlファイルにPose型で保存していくことにする．

yamlファイルの一例は下記の通り．

（書いた後で，finish pose以外Pose型になっていなかったので，直した方が良いと思っています．が，このまま続けます．）

waypoints.yaml

waypoints:

- position:

x: 4.23423

y: 0.164882

z: 0

qx: 0

qy: 0

qz: -0.000185805

qw: 1

- position:

x: 8.03098

y: 0.215329

z: 0

qx: 0

qy: 0

qz: 0.00539874

qw: 0.999985

・・・・ここはコードではありません．省略の意味

最後に下記の記載がある．

finish_pose:

header:

seq: 0

stamp: 1466584768.006917447

frame_id: map

pose:

position:

x: -0.0497315

y: -0.0498823

z: 0

orientation:

x: 0

y: 0

z: 0.0522905

w: 0.998632

ということで，上記のようなyamlファイルを保存するためにodom_listener.cppを下記のように修正します．

詳細は，省きます．C++がわかっていればすぐにわかると思います．

odom_listener.cpp

#include <ros/ros.h>

#include <sensor_msgs/Joy.h>

#include <geometry_msgs/Twist.h>

#include <geometry_msgs/PoseStamped.h>

#include <nav_msgs/Odometry.h>

#include <cmath>

#include <vector>

#include <fstream>

#include <string>

class odom_listener{

private :

ros::Subscriber sub;

nav_msgs::Odometry odom;

std::vector<geometry_msgs::Pose> waypoints;

geometry_msgs::PoseStamped finish_pose_;

geometry_msgs::Pose pre_pose;

std::string filename_;

std::string world_frame_;

int start;

public :

odom_listener();

void odom_loop();

void save();

private :

void odomCallback(const nav_msgs::Odometry &odom_msg);

};

odom_listener::odom_listener():filename_("waypoints.yaml"){

ros::NodeHandle n;

sub = n.subscribe("odom", 1, &odom_listener::odomCallback, this);

start=0;

pre_pose.position.x = 0.0;

pre_pose.position.y = 0.0;

pre_pose.position.z = 0.0;

pre_pose.orientation.x = 0.0;

pre_pose.orientation.y = 0.0;

pre_pose.orientation.z = 0.0;

pre_pose.orientation.w = 0.0;

}

void odom_listener::odom_loop() {

ros::Rate loop_rate(2); //2Hz

while(ros::ok()) {

ros::spinOnce();

loop_rate.sleep();

}

if(start != 0){ //save waypoints

ROS_INFO_STREAM("start write");

save();

}

return;

}

void odom_listener::odomCallback(const nav_msgs::Odometry &odom_msg) {

ROS_INFO("odom : x %lf : y %lf\n", odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);

if(pow(odom_msg.pose.pose.position.x - pre_pose.position.x, 2.0) + pow(odom_msg.pose.pose.position.y - pre_pose.position.y, 2.0) + pow(odom_msg.pose.pose.position.z - pre_pose.position.z, 2.0) >= 0.5) {// check distance

if(start==0) start=1;

waypoints.push_back(odom_msg.pose.pose);//collect position

ROS_INFO_STREAM("save current position");

pre_pose.position.x = odom_msg.pose.pose.position.x;

pre_pose.position.y = odom_msg.pose.pose.position.y;

pre_pose.position.z = odom_msg.pose.pose.position.z;

pre_pose.orientation.x = odom_msg.pose.pose.orientation.x;

pre_pose.orientation.y = odom_msg.pose.pose.orientation.y;

pre_pose.orientation.z = odom_msg.pose.pose.orientation.z;

pre_pose.orientation.w = odom_msg.pose.pose.orientation.w;

}

// update finish position

finish_pose_.header.seq = 0;

//finish_pose_.header.stamp = 0.0;//pre_timestamp;

finish_pose_.header.frame_id = world_frame_;

finish_pose_.pose.position.x = odom_msg.pose.pose.position.x;

finish_pose_.pose.position.y = odom_msg.pose.pose.position.y;

finish_pose_.pose.position.z = odom_msg.pose.pose.position.z;

finish_pose_.pose.orientation.x = odom_msg.pose.pose.orientation.x;

finish_pose_.pose.orientation.y = odom_msg.pose.pose.orientation.y;

finish_pose_.pose.orientation.z = odom_msg.pose.pose.orientation.z;

finish_pose_.pose.orientation.w = odom_msg.pose.pose.orientation.w;

return;

}

void odom_listener::save(){// save waypoints

std::ofstream ofs(filename_.c_str(), std::ios::out);

ofs << "waypoints:" << std::endl;

for(int i=0; i < waypoints.size(); i++){

ofs << " " << "- position:" << std::endl;

ofs << " x: " << waypoints[i].position.x << std::endl;

ofs << " y: " << waypoints[i].position.y << std::endl;

ofs << " z: " << waypoints[i].position.z << std::endl;

ofs << " qx: "<< waypoints[i].orientation.x << std::endl;

ofs << " qy: "<< waypoints[i].orientation.y << std::endl;

ofs << " qz: "<< waypoints[i].orientation.z << std::endl;

ofs << " qw: "<< waypoints[i].orientation.w << std::endl;

}

ofs << "finish_pose:" << std::endl;

ofs << " header:" << std::endl;

ofs << " seq: " << finish_pose_.header.seq << std::endl;

ofs << " stamp: " << finish_pose_.header.stamp << std::endl;

ofs << " frame_id: " << finish_pose_.header.frame_id << std::endl;;

ofs << " pose:" << std::endl;

ofs << " position:" << std::endl;

ofs << " x: " << finish_pose_.pose.position.x << std::endl;

ofs << " y: " << finish_pose_.pose.position.y << std::endl;

ofs << " z: " << finish_pose_.pose.position.z << std::endl;

ofs << " orientation:" << std::endl;

ofs << " x: " << finish_pose_.pose.orientation.x << std::endl;

ofs << " y: " << finish_pose_.pose.orientation.y << std::endl;

ofs << " z: " << finish_pose_.pose.orientation.z << std::endl;

ofs << " w: " << finish_pose_.pose.orientation.w << std::endl;

ofs.close();

std::cout << "write success"<<std::endl;

}

int main(int argc, char **argv) {

ros::init(argc, argv, "odom_listener");

odom_listener odometry;

odometry.odom_loop();

return (0);

}

下記コマンドで，コンパイル，実行してみましょう．

$ cd ~/catkin_ws && catkin_make

$ rosrun odom_listener odom_listener

上記のプログラムは，0.5秒ごとにオドメトリ位置をチェックして，以前保存した位置から0.5m以上離れていた時に，現在の位置をvector型に追加保存していきます．

プログラム終了時にsave()関数でファイル（waypoints.yaml）に保存します．

保存場所はプログラム実行したディレクトリになります．

下記に前回のページのシミュレータの立ち上げからwaypoints保存まで紹介します．

$ roslaunch wheel_robot wheel_robot_control.launch // シミュレータ立ち上げ

$ rosrun odom_listener odom_listener // 別ウィンドウでオドメトリ保存用のノード立ち上げ

$ rosrun wheel_robot_control key_teleop.py //別ウィンドウで移動ロボットを操作．この例はキーボードでのコントロール

上記の３つを実行後に，ロボットを所望する場所にコントロールして軌道を保存していきます．

所望する軌道通りに移動が終わったら，odom_listenerを実行した画面でctl+cを押してノードを終了させます．

終了したウィンドウに「write success」が表示されれば保存は終了です．

rosrun odom_listener odom_listener

を実行したディレクトリ内にwaypoints.yamlが保存されていることを確認できるはず．

次に，この保存したwaypoints.yamlをナビゲーション用のポイントとして利用する方法を紹介します．

２．保存した軌跡に追従させよう！

保存した軌跡に追従させるプログラムは，いろいろなところで紹介されているnavigationパッケージを使った誘導を行います．パッケージの性質上，適当に空の地図を用意したり色々面倒なことがあります．．．

また，保存したwaypoints.yamlファイルを読み込み，順次navigationパッケージのmove-baseに目標位置を送信するためのwaypoints_publishノードを自作して準備する必要があります．

（いろいろ使えそうなのがあったのですが，すぐに動かなかったりするのでmove_baseのページを参考に，下記の通り自作していきます）

２．１ waypoints_publishの作成

下記コマンドでwaypoints_publishノードを新規作成する．

(先に言っておくと，このwaypoints_publishでmove_baseに移動目標を与えているので，

計画済みの経路に従って動くのか，現在の状況に従って走行軌跡（目標位置）を変更するのかを

判断する頭脳の部分のノードになると思います．）

後，いつも事後ですが．．．このプログラムはwaypointをpublishしてませんね．．．

$ cd ~/catkin_ws/src

$ catkin_create_pkg waypoints_publish roscpp actionlib tf move_base_msgs

$ cd waypoints_publish/src && gedit waypoints_publish.cpp

waypoints_publish.cppの内容は下記の通り．

yamlファイルを読み込んで，vector型にデータを保存して，１つづつmovebaseのコマンドで移動先を指定していくようにしている．

waypoints_publish.cpp

#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>

#include <actionlib/client/simple_action_client.h>

#include <geometry_msgs/PointStamped.h>

#include <geometry_msgs/PoseStamped.h>

#include <tf/tf.h>

#include <yaml-cpp/yaml.h>

#include <vector>

#include <fstream>

#include <math.h>

#include <string>

#include <visualization_msgs/MarkerArray.h>//for visual marker on rviz

#define MAKE_ORIENTATION

template<typename T>

void operator >> (const YAML::Node& node, T& i)

{

i = node.as<T>();

}

class Waypoints_publish {

private :

actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;

std::vector<geometry_msgs::PoseStamped> waypoints_;

std::vector<geometry_msgs::PoseStamped>::iterator current_waypoint_;

geometry_msgs::Pose finish_pose_;

ros::Rate rate;

ros::Publisher marker_pub_;//marker for rviz

public :

Waypoints_publish() :

move_base_action_("move_base", true),

rate(10)

{

/* connect move_base */

while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))

{

ROS_INFO("Waiting for the move_base action");

}

ros::NodeHandle nh;

marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);

/* read .yaml */

ros::NodeHandle private_nh("~");

std::string filename = "";

private_nh.param("filename", filename, filename);

if(filename != ""){

ROS_INFO_STREAM("Read waypoints data from " << filename);

if(!readFile(filename)) {

ROS_ERROR("Failed loading waypoints file");

}

current_waypoint_ = waypoints_.begin();

} else {

ROS_ERROR("waypoints file doesn't have name");

}

void run() {

bool has_activate_ = true;

ros::Rate rate(10);

while(ros::ok() && has_activate_){

has_activate_ = sendPoint();

while(!navigationFinished() && ros::ok()) rate.sleep();

publishMarkers();

}

return;

}

private :

bool readFile(const std::string &filename){

waypoints_.clear();

try{

std::ifstream ifs(filename.c_str(), std::ifstream::in);

if(ifs.good() == false){

return false;

}

YAML::Node node;

node = YAML::Load(ifs);

const YAML::Node &wp_node_tmp = node["waypoints"];

const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;

if(wp_node != NULL){

for(int i=0; i < wp_node->size(); i++){

geometry_msgs::PoseStamped pose;

(*wp_node)[i]["position"]["x"] >> pose.pose.position.x;

(*wp_node)[i]["position"]["y"] >> pose.pose.position.y;

(*wp_node)[i]["position"]["z"] >> pose.pose.position.z;

(*wp_node)[i]["position"]["qx"] >> pose.pose.orientation.x;

(*wp_node)[i]["position"]["qy"] >> pose.pose.orientation.y;

(*wp_node)[i]["position"]["qz"] >> pose.pose.orientation.z;

(*wp_node)[i]["position"]["qw"] >> pose.pose.orientation.w;

waypoints_.push_back(pose);

}

}else{

return false;

}

const YAML::Node &fp_node_tmp = node["finish_pose"];

const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;

if(fp_node != NULL){

(*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;

(*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;

(*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

(*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;

(*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;

(*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;

(*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;

}else{

return false;

}

}catch(YAML::ParserException &e){

return false;

}catch(YAML::RepresentationException &e){

return false;

}

return true;

}

bool sendPoint() {

move_base_msgs::MoveBaseGoal move_base_goal;

move_base_goal.target_pose.header.stamp = ros::Time::now();

move_base_goal.target_pose.header.frame_id = "map";

if(current_waypoint_ == waypoints_.end()-1) {

#ifdef MAKE_ORIENTATION

double goal_direction = atan2(finish_pose_.position.y - current_waypoint_->pose.position.y,

finish_pose_.position.x - current_waypoint_->pose.position.x);

move_base_goal.target_pose.pose = current_waypoint_->pose;

move_base_goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);

#else

move_base_goal.target_pose.pose = current_waypoint_->pose;

#endif

current_waypoint_++;

ROS_INFO("Sending goal");

move_base_action_.sendGoal(move_base_goal);

} else if (current_waypoint_ < waypoints_.end() - 1) {

#ifdef MAKE_ORIENTATION

double goal_direction = atan2((current_waypoint_+1)->pose.position.y - current_waypoint_->pose.position.y,

(current_waypoint_+1)->pose.position.x - current_waypoint_->pose.position.x);

move_base_goal.target_pose.pose = current_waypoint_->pose;

move_base_goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(goal_direction);

#else

move_base_goal.target_pose.pose = current_waypoint_->pose;

#endif

current_waypoint_++;

ROS_INFO("Sending goal");

move_base_action_.sendGoal(move_base_goal);

} else {

move_base_goal.target_pose.pose = finish_pose_;

ROS_INFO("Sending goal");

move_base_action_.sendGoal(move_base_goal);

return false;

}

return true;

}

bool navigationFinished(){

return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;

}

void publishMarkers(){

visualization_msgs::MarkerArray markers_array;

for(int i=0; i < waypoints_.size(); i++){

visualization_msgs::Marker marker, label;

marker.header.frame_id = "map";

marker.header.stamp = ros::Time::now();

marker.scale.x = 0.2;

marker.scale.y = 0.2;

marker.scale.z = 0.2;

marker.pose.position.z = marker.scale.z / 2.0;

marker.color.r = 0.8f;

marker.color.g = 0.2f;

marker.color.b = 0.2f;

std::stringstream name;

name << "waypoint " << i;

marker.ns = name.str();

marker.id = i;

marker.pose.position.x = waypoints_[i].pose.position.x;

marker.pose.position.y = waypoints_[i].pose.position.y;

marker.type = visualization_msgs::Marker::SPHERE;

marker.action = visualization_msgs::Marker::ADD;

marker.color.a = 1.0f;

markers_array.markers.push_back(marker);

//ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);

}

marker_pub_.publish(markers_array);

}

};

int main(int argc, char** argv){

ros::init(argc, argv, "waypoints_publish_tf");

Waypoints_publish waypoints_publish;

waypoints_publish.run();

return 0;

}

CMakeLists.txtも下記のように修正

$ cd ~/catkin_ws/src/waypoint_publish && gedit CMakeLists.txt

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)

project(waypoints_publish)

find_package(catkin REQUIRED COMPONENTS

actionlib

move_base_msgs

roscpp

tf

)

find_package(PkgConfig)

pkg_search_module(yaml-cpp REQUIRED yaml-cpp)

catkin_package(

INCLUDE_DIRS include

LIBRARIES waypoints_publish

CATKIN_DEPENDS actionlib move_base_msgs roscpp tf

DEPENDS system_lib

)

include_directories(

${catkin_INCLUDE_DIRS}

${yaml-cpp_INCLUDE_DIRS}

)

add_executable(waypoints_publish src/waypoints_publish.cpp)

target_link_libraries(waypoints_publish

${catkin_LIBRARIES}

${yaml-cpp_LIBRARIES}

)

下記コマンドで，コンパイルして間違いがないか確認しておきましょう．

$ cd ~/catkin_ws && catkin_make

コンパイルできてもwaypointを目標位置としてい移動ロボットの移動を指示するmove-baseの準備ができていないので，

次にmove-baseの利用方法を説明する．

２．２ move-baseの準備

ナビゲーション用のパッケージをわかりやすいように作成しておく．

$ cd ~/catkin_ws/src && catkin_create_pkg wheel_robot_nav

今回は適当にwheel_robot_navという名前にしておく．

launchディレクトリを作成して，wheel_robot_nav.launchファイルを作成する．

$ cd ~/catkin_ws/src/wheel_robot_nav && mkdir launch

$ cd launch

$ gedit wheel_robot_nav.launch

今回は適当にwheel_robot_navという名前にしておく．

wheel_robot_nav.launchの内容は下記のようにする．

launchファイル中のparamのディレクトリは後で作成する．

4行目のマップのyamlファイルの名前は，生成されたマップ画像のファイル名と同じにしておくこと．

後ほど修正．現在の例はblank_map.yamlを記載している．

wheel_robot_nav.launch

</node>

</node>

</node>

</launch>

次にnavigation用のparamディレクトリの作成と7つのパラメータファイルを保存する．

$ cd ~/catkin_ws/src/wheel_robot_nav && mkdir param

$ cd param

$ gedit base_local_planner_params.yaml

$ gedit global_costmap_params.yaml

$ gedit local_costmap_params.yaml

$ gedit costmap_common_params.yaml

$ gedit move_base_params.yaml

$ gedit dwa_local_planner_params.yaml

$ gedit smoother.yaml

７つのファイルは下記の通り．

方向は移動時の保存した姿勢をあまり考慮しないようなナビゲーション設定として，角度の許容を±180度（±π=3.14）としておき，許容距離を0.3mとしておいた．

（が，すぐにwaypointが切り替わらない問題が発生． 1.0mに拡大しても現在未解決）

base_local_planner_params.yaml

TrajectoryPlannerROS:

# Robot Configuration Parameters

max_vel_x: 0.3

min_vel_x: 0.1

max_vel_theta: 1.0

min_vel_theta: -1.0

min_in_place_vel_theta: 0.6

acc_lim_x: 0.5

acc_lim_theta: 1.0

# Goal Tolerance Parameters

yaw_goal_tolerance: 3.14 #0.3

xy_goal_tolerance: 0.3#0.15

# Forward Simulation Parameters

sim_time: 3.0

vx_samples: 6

vtheta_samples: 20

# Trajectory Scoring Parameters

meter_scoring: true

pdist_scale: 0.6

gdist_scale: 0.8

occdist_scale: 0.01

heading_lookahead: 0.325

dwa: true

# Oscillation Prevention Parameters

oscillation_reset_dist: 0.05

# Differential-drive robot configuration

holonomic_robot: false

max_vel_y: 0.0

min_vel_y: 0.0

acc_lim_y: 0.0

vy_samples: 0

global_costmap_params.yaml

global_costmap:

global_frame: map

robot_base_frame: base_footprint

update_frequency: 1.0

publish_frequency: 0.5

static_map: true

transform_tolerance: 0.5

local_costmap_params.yaml

local_costmap:

global_frame: map

robot_base_frame: base_footprint

update_frequency: 5.0

publish_frequency: 2.0

static_map: false

rolling_window: true

width: 10.0

height: 10.0

resolution: 0.05

transform_tolerance: 0.5

costmap_common_params.yaml

obstacle_range: 5.0

raytrace_range: 6.0

footprint: [[0.5, 0.25], [0.5, -0.25], [-0.5, 0.25], [-0.5, -0.25]]

inflation_radius: 0.55

map_type: voxel

origin_z: 0.0

z_resolution: 0.2

z_voxels: 2

publish_voxel_map: false

max_obstacle_height: 0.60

observation_sources: scan

scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}

move_base_params.yaml

shutdown_costmaps: false

controller_frequency: 5.0

controller_patience: 3.0

planner_frequency: 1.0

planner_patience: 5.0

oscillation_timeout: 10.0

oscillation_distance: 0.2

dwa_local_planner_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters - Dulcinea

max_vel_x: 0.6

min_vel_x: -0.1

max_vel_y: 0.0

min_vel_y: 0.0

max_trans_vel: 0.6

min_trans_vel: -0.1

trans_stopped_vel: 0.4#0.01

max_rot_vel: 0.4

min_rot_vel: -0.4

rot_stopped_vel: 0.2#0.05

acc_lim_x: 1.0

acc_lim_theta: 2.0

acc_lim_y: 0.0

# Goal Tolerance Parameters

yaw_goal_tolerance: 3.14 #0.3

xy_goal_tolerance: 0.3 #0.15

# Forward Simulation Parameters

sim_time: 1.0

vx_samples: 6

vy_samples: 1

vtheta_samples: 20

# Trajectory Scoring Parameters

path_distance_bias: 64.0

goal_distance_bias: 24.0

occdist_scale: 0.5

forward_point_distance: 0.325

stop_time_buffer: 0.5

scaling_speed: 0.25

max_scaling_factor: 0.2

# Oscillation Prevention Parameters

oscillation_reset_dist: 0.05

# Debugging

publish_traj_pc : true

publish_cost_grid_pc: true

global_frame_id: odom

smoother.yaml

# Default parameters used by the yocs_velocity_smoother module.

# This isn't used by minimal.launch per se, rather by everything

# which runs on top.

# Mandatory parameters

speed_lim_v: 0.8

speed_lim_w: 5.4

accel_lim_v: 1.0 # maximum is actually 2.0, but we push it down to be smooth

accel_lim_w: 2.0

# Optional parameters

frequency: 20.0

decel_factor: 1.0

# Robot velocity feedback type:

# 0 - none (default)

# 1 - odometry

# 2 - end robot commands

robot_feedback: 2

また，ダミーのマップを用意する必要があるので適当に真っ白なマップをgimpなどでpgmの画像ファイルとして用意する．

yamlファイルでpgmファイルを指定する必要があるので下記のようにmapディレクトリとblank_map.yamlファイルを作成する．

$ cd ~/catkin_ws/src/wheel_robot_nav && mkdir map

$ cd map

$ gedit blank_map.yaml

blank_map.yaml（注意：originで画像の中心まで移動させておくこと。マップ外への移動はmove_baseではできない。）

image: blank_map.pgm

resolution: 0.01

origin: [-5, -5, 0]

occupied_thresh: 0.65

free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package

negate: 0

blank_map.pgmファイルの作り方は，

$ gimp

で画像編集ソフトgimpを立ち上げる．

gimpがインストールされていなければ

$ sudo apt-get install gimp

でインストール．

gimpが立ち上がったら，「ファイル」→「新しい画像」で

適当なサイズの画像を作る．今回の場合は２０００x２０００pixelとした．

真っ白な画像が初期画像となるが，障害物のないマップはこれでOKなので，

このまま「ファイル」→「エクスポート」で画像を

pgm形式を選択してblank_map.pgmとしてASCII形式でmapディレクトリに保存．

最後に，保存したwaypointファイルを管理しておくディレクトリを作成しておき，そこに前節で保存したファイルを置いておく．

$ cd ~/catkin_ws/src/wheel_robot_nav && mkdir waypoints

これで保存したwaypointを追従するナビゲーションの準備が完了したので下記のコマンドで実行する．

まず、dwa（dynamic window approachのライブラリをインストールしておく）

$ sudo apt-get install ros-noetic-dwa-local-planner

次に，前回準備した移動ロボットのシミュレータを実行する．

$ roslaunch wheel_robot wheel_robot_control.launch

rvizとgazeboのシミュレータ画面が立ち上がるはず．

別ターミナルで下記のナビゲーションのlaunchファイルを立ち上げる．

$ roslaunch wheel_robot_nav wheel_robot_nav.launch

上記のlaunchファイルを実行すると，waypointに従って移動を始める．

サンプルのwaypoint_publish.cppはrviz上にwaypointを表示するように記載してあるので

waypointを表示するために，

rvizの画面で左のリストの「Add」を押して現れる画面で「vizualization_marker」トピックを選択してwaypointを表示させる．

「Add」で出てくる画面の例を下記に示す．

また，どのwaypointに向かって動いているのかわかりやすいように「current_goal」のトピックも追加する．

Addの画面は下記の例の様に指定できる．

また，ゴールまでの計画経路がわかる様に「plan」を追加する．

上記のトピックを追加したときの表示例を下記に示す．

これで，一度動かした場所と同じ経路を追従可能な移動ロボットのナビゲーションが可能となった．

はじめにも書いたが，細かいwaypointにいちいち追従しては止まってという動作を繰り返すため，とても移動が遅くなることと，途中の中継点での位置決めがかなり下手くそ？なのか，判定がうまくいっていないのかわからないが，なかなか目標地点にたどり着かずに，次のwaypointへの移動指示に切り替わらないなどの問題が残っている．

初心者向けにodomを使い続けているためなのか，４輪の移動ロボットで対向２輪作動式の移動ロボットをモデルとした移動を仮定しているために誤差が発生してこのようになっているためなのかはまだ不明．

（というか，初めから自己位置も計測しつつ，モデルも対向２輪でやれという話なのかもしれないが．．．）

早いうちに，上記の自己位置認識機能を組み合わせ，モデルを対向２輪に変更して確認する予定です．

また，waypoint_publishを修正して，自己位置を確認しつつ，柔軟にwaypointの位置を早めに次のポイントへ切り替えるなどをやってみる予定．