5月 27th, 2015

移動ロボットのBumperからの入力を制御するRTコンポーネント

技術, by nishimura.

こんにちは!学部4年生の西村です。
私たちが研究室で行っていることをこれから紹介していきます。

最初なので軽く説明をしますが、知っている人は読み飛ばしてくださいね。
私たちは、ロボットを動かしていく上でRT(ロボット)ミドルウェアを利用しています。
RTミドルウェアとは、産業技術総合研究所が開発しているロボットシステム構築のためのプラットフォームのことです。
顔認識、音声合成、移動など、ロボットの機能要素ごとにプログラムをつくり(RTコンポーネントと呼びます)、それを繋ぎ合わせることでシステムを構築していきます。
他の人がつくったコンポーネントを再利用することが可能なので、システムの開発効率を向上させることができると考えられています。
さらに詳しい説明は産総研のホームページに載っているので、ここでは割愛します。

さて、本題に入りますが、私たちが研究室で動かしているロボットは、移動ロボットKobukiです。

Kobuki

今回は、この移動ロボットKobukiを制御するコンポーネントを紹介します。
実はだいぶ前につくっていたのですが、ホームページでの更新は遅くなってしまいました、、

【準備】

RTミドルウェアのインストール
本モジュール内のコンポーネントを利用するにはOpenRTM-aistのC++版をインストールしておく必要があります。
産総研のホームページからダウンロード可能です。
現在の最新版はOpenRTM-aist C++ 1.1.0-RELEASEです。

【本コンポーネントの概要】

自動車などと同じように、移動ロボットにも衝撃を和らげるためのバンパーが取り付けられています。
このRTコンポーネントは、バンパーから入力された値によって、移動ロボットを制御するものです。
移動ロボットKobukiには、前、右、左にバンパーがあり、このコンポーネントでは、障害物にあたると一旦後ろに下がり、それを避ける方向に進むようにプログラムされています。

【インターフェース】

《DataInPorts》

ポート名: bumper
データ型: RTC.TimedBooleanSeq型
バンパーからの入力値です。

《DataOutPorts》

ポート名: targetVelocity
データ型: RTC.TimedVelocity2D型
移動ロボットに出力する速度です。

【コンフィグレーション】

initialVelocityX
アクティベート時の初速です。初期値は0.0です。

avoidVelocityX
障害物にぶつかったときに後退する速度です。初期値は-0.1です。

【ソースコード】

モードが4つ準備されていて、最初のSearchModeのときは与えられた初速で前進するだけです。
バンパーは前、右、左に分かれており、どのバンパーに入力があったかで、FrontAvoidMode, RightAvoidMode, LeftAvoidModeにモードが切り替わります。
バンパーからの入力や、そのときの速度などは、随時コンソールに値が出力されます。

こちらからダウンロードできます。

《BumperController.cpp》

実行したときに呼ばれるonExecute()という関数の中身はこんな感じです。

RTC::ReturnCode_t BumperController::onExecute(RTC::UniqueId ec_id)
{
	if(this ->m_bumperIn.isNew()) {
		m_bumperIn.read();
 
		int bumper_r = m_bumper.data[0];
		int bumper_f = m_bumper.data[1];
		int bumper_l = m_bumper.data[2];
 
		std::cout <<  "right = " << bumper_r << ", " ;
		std::cout <<  "front = " << bumper_f << ", " ;
		std::cout <<   "left = " << bumper_l << std::endl;
 
		if(m_Mode == SEARCH_MODE){
			m_targetVelocity.data.vx = m_initialVelocityX;
			m_targetVelocity.data.vy = 0;
			m_targetVelocity.data.va = 0;
 
			if(bumper_f){
				m_Mode = FRONT_AVOID_MODE;
				m_StartTime = coil::gettimeofday();
			}
			if(bumper_r){
				m_Mode = RIGHT_AVOID_MODE;
				m_StartTime = coil::gettimeofday();
			}
			if(bumper_l){
				m_Mode = LEFT_AVOID_MODE;
				m_StartTime = coil::gettimeofday();
			}
		}
 
		if(m_Mode == FRONT_AVOID_MODE){
			coil::TimeValue now = coil::gettimeofday();
			double durationSec = now - m_StartTime;
 
			m_targetVelocity.data.vx = m_avoidVelocityX;
			m_targetVelocity.data.vy = 0;
			m_targetVelocity.data.va = 0.5;
 
			if(durationSec > 2.0){
				m_Mode = SEARCH_MODE;
			}
		}
		if(m_Mode == RIGHT_AVOID_MODE){
			coil::TimeValue now = coil::gettimeofday();
			double durationSec = now - m_StartTime;
 
			m_targetVelocity.data.vx = m_avoidVelocityX;
			m_targetVelocity.data.vy = 0;
			m_targetVelocity.data.va = 0.5;
 
			if(durationSec > 2.0){
				m_Mode = SEARCH_MODE;
			}
		}
		if(m_Mode == LEFT_AVOID_MODE){
			coil::TimeValue now = coil::gettimeofday();
			double durationSec = now - m_StartTime;
 
			m_targetVelocity.data.vx = m_avoidVelocityX;
			m_targetVelocity.data.vy = 0;
			m_targetVelocity.data.va = -0.5;
 
			if(durationSec > 2.0){
				m_Mode = SEARCH_MODE;
			}
		}
 
		std::cout << "Mode = " << m_Mode << std::endl;
 
		std::cout <<  "vx = " << m_targetVelocity.data.vx << ", ";
		std::cout <<  "vy = " << m_targetVelocity.data.vy << ", ";
		std::cout <<  "va = " << m_targetVelocity.data.va << std::endl;
 
		m_targetVelocityOut.write();
	}
 
	return RTC::RTC_OK;
}

《BumperController.h》

ヘッダーファイルには、プログラムで使う変数の宣言などが書かれています。

class BumperController
  : public RTC::DataFlowComponentBase
{
 private:
     int m_Mode;
     coil::TimeMeasure m_Timer;
     coil::TimeValue m_StartTime;
};
 
const int SEARCH_MODE = 0;
const int RIGHT_AVOID_MODE = 1;
const int FRONT_AVOID_MODE = 2;
const int LEFT_AVOID_MODE = 3;

【システム図】

下図のように、KobukiRTCとBumperControllerを接続し、実行します。
kobukiRTCのソースコードはこちらからダウンロードできます。
初速の初期値は0.0になっているので、何もしなければ動きません。(KobukiのBumperに触れると後退はします。)
コンフィグレーションで、初速を変えてKobukiを動かしてみてください。

KobukiBumperControll

【実行時】

コンソールにはこのように出力されます。
前のバンパーに入力があり、front=1と表示され、速度vxが-0.1に変わっているのがわかりますね。

KobukiBumperControll_Console

次回は、音声で移動ロボットを操作するコンポーネントを紹介します!

Back Top

Responses to “移動ロボットのBumperからの入力を制御するRTコンポーネント”

  1. No comments yet.

コメントを残す