Up:Tutorial?　　 　Previous：車両のダイナミクス（ダンベルモデル）　　

距離センサ †

ここでは、距離センサ機能を使ったサンプルを紹介します。エージェントに設置されたカメラの位置からそのカメラの視線方向におけるオブジェクトまでの距離を取得することができます。

↑

コントローラ作成 †

まずはコントローラを作成します。(captureView?のコントローラとほとんど同じです。）

$ cd sigverse-<version>/bin/NewWorld
$ emacs distanceSensor.cpp
#include <Controller.h>
#include <ControllerEvent.h>
#include <Logger.h>
#include <ViewImage.h>
#include <math.h>

#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )

class RobotController : public Controller
{
public:
  void onInit(InitEvent &evt);
  double onAction(ActionEvent &evt);
  void onRecvMessage(RecvMessageEvent &evt);
private:  

  //視線フラグ（視線が右に移動中か左に移動中か）
  bool move_eye;

  //視線ベクトルの変化
  double dvvx;

  //体を回転させるときの回転角
  int dy;
 
  //移動速度
  double vel;
};

void RobotController::onInit(InitEvent &evt)
{
  //パラメータの設定
  move_eye = false;
  dvvx     =  0.1;
  dy       = 45;
  vel      = 10.0;
} 

//定期的に呼び出される関数
double RobotController::onAction(ActionEvent &evt)
{ 

  SimObj *my = getObj(myname());

  //視線ベクトルのx成分を取得します。
  double vvx = my->evx1(); 

  double dvvx_tmp;
  //視線が右に移動中の場合
  if (move_eye == false)
    {
      dvvx_tmp = -1 * dvvx;
    }
  //左に移動中の場合
  else dvvx_tmp = dvvx;

  //視線を変化させます。
  vvx = vvx + dvvx_tmp;
  my->evx1(vvx); 


  //視線ベクトルのx成分を-1.0から1.0まで移動させます。
  if(vvx >= 1.0)
    {
      move_eye = false;
    }
  if( vvx <= -1.0)
    {
      move_eye = true;
    } 

  return 0.5; 

}

//メッセージ受信時に呼び出される関数
void RobotController::onRecvMessage(RecvMessageEvent &evt)
{
  int n = evt.getSize();
  static int iImage = 0;
  if (n>0)
    {  

      //取得したメッセージを表示します。
      std::string msg = evt.getString(0);
      LOG_MSG(("msg : %s", msg.c_str()));


      //メッセージ"distance"を受信すると距離センサで距離を測定します。
      if (strcmp(msg.c_str(), "distance") == 0)
        {
          //距離を取得します。
          unsigned char dis = distanceSensor();
          LOG_MSG(("distance = %d", dis));
        } 

      //メッセージ"rotation"を受信すると回転します。
      if (strcmp(msg.c_str(), "rotation") == 0){

        SimObj *my = getObj(myname());

        // 自分のy軸周りの回転を得ます(クオータニオン)
        double qy = my->qy();

        //くオータニオンから回転角(ラジアン)を導出します
        double theta = 2*asin(qy);

        //体全体を回転させます。
        double y = theta + DEG2RAD(45);
        if( y >= PI)
         {
           y = y - 2 * PI;
         }
        my->setAxisAndAngle(0, 1.0, 0, y);

      }

      //メッセージ"move"を受信すると自分の向いている方向に進みます。
      if (strcmp(msg.c_str(), "move") == 0){

        SimObj *my = getObj(myname());

        //自分の位置を得ます。
        double x = my->x();
        double y = my->y();
        double z = my->z();

         //y軸周りの自分の回転を得ます。（クオータニオン）

        double qy = my->qy();

        //クオータニオンから回転角を導出します。
        double theta = 2*asin(qy);

        //移動距離を初期化します。
        double dx = 0.0;
        double dz = 0.0;
 
        //移動する方向を決定します。
        dx = sin(theta) * vel;
        dz = cos(theta) * vel;

        //移動します。
        my->setPosition( x + dx, y , z + dz );

      }

   }
}  

extern "C" Controller * createController ()
{
  return new RobotController;
}

このエージェントは視線ベクトルを常に左右に動かし、"rotation"や"move"といったメッセージを送信して操作します。 操作方法はcaptureView?を参考にしてください。このエージェントにメッセージ"distance"を送信するとdistanceSensor()を使ってカメラから視線方向へのオブジェクトの位置を測定します。

Front page   New List of pages Search Recent changes   Help   RSS of recent changes