Backup of Image capture from agent's perspective(No. 4)

List of Backups
View the diff.
View the diff current.
View the source.
Go to Image capture from agent's perspective.
- 1 (2013-02-22 (Fri) 17:25:01)
- 2 (2013-02-22 (Fri) 17:35:46)
- 3 (2013-02-22 (Fri) 17:35:46)
- 4 (2013-12-04 (Wed) 00:56:27)
- 5 (2014-01-13 (Mon) 02:46:44)

Up:Tutorial?　　　　Previous：Use of service provider?　　　　　Next：Collision between agents?

Image capture from agent's perspective

Image capture from agent's perspective †

(※This tutorial is valid only for later than v2.1.0)

This tutorial explains how to capture an image from the agent's perspective and how to use it in a controller based on service provider function.

↑

Creating a controller †

$ emacs captureView.cpp

captureView.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 onRecvMsg(RecvMsgEvent &evt);

private:
  ViewService* m_view;
  double vel;
};

void RobotController::onInit(InitEvent &evt)
{
  // Setting the velocity
  vel = 10.0;
  // Connecting service
  m_view = (ViewService*)connectToService("SIGViewer");
}

// Callback function which is called frequently
double RobotController::onAction(ActionEvent &evt)
{
  return 10.0;
}

// Callback function which is called when a message comes
void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
  static int iImage = 0;

// Display the received message
  std::string msg = evt.getMsg();
  LOG_MSG(("msg : %s", msg.c_str()));

// Execute captureView when a message "capture" comes
  if (msg == "capture") {
    if(m_view != NULL) {

ViewImage *img = m_view->captureView(2, COLORBIT_24, IMAGE_320X240);
      if (img != NULL) {

// Receiving an image data
        char *buf = img->getBuffer();

// Save in Windows BMP format
        char fname[256];
        sprintf(fname, "view%03d.bmp", iImage++);
        img->saveAsWindowsBMP(fname);

// Release the memory
        delete img;
      }
    }
  }

// Rorate the agent when a message "rotation" comes
  if (msg == "rotation"){

SimObj *my = getObj(myname());

// Reference of quaternion
    double qy = my->qy();

// Calculate Euler angle from the quaternion
    double theta = 2*asin(qy);

// Rorate the body
    double y = theta + DEG2RAD(45);
    if( y >= PI)
      y = y - 2 * PI;

my->setAxisAndAngle(0, 1.0, 0, y);
   }

// Going forward when a message "move" comes
  if (msg == "move"){

SimObj *my = getObj(myname());

// Get the current position
    Vector3d pos;
    my->getPosition(pos);

double qy = my->qy();

// Calculate Euler angle from the quaternion
    double theta = 2*asin(qy);

// Movement distance
    double dx = 0.0;
    double dz = 0.0;

// Calculate the movement orientation
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;

// Move
    my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );
  }
}

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

↑

captureView †

Receving an image data using captureView in 50th line.

The first argument of the captureView(); is the ID number of the target camera. The camera ID 2 was used in this example. The second and third arguments are corresponded to bit depth and image size. (Only 24bits and 320x240 can be choosen in v2.1.0)

Use the function getBuffer() to extract the image data from the ViewImage.

In this example, the agent will

go forward when "move" is received.
rorate when "rotation" is received.
capture an image when "capture" is received.

Image files such as view000.bmp, view001.bmp... in bmp format will be created in a starting directory.

↑

Compilation †

$ ./sigmake.sh captureView.cpp

↑

Creating a world file †

$ emacs captureView.xml

The following world file is almost the same as the world file used in the Vision sensor.

captureView.xml

<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

<instanciate class="Robot-nii.xml">

<set-attr-value name="name" value="robot_000"/>

<set-attr-value name="language" value="c++"/>

<set-attr-value name="implementation" value="./captureView.so"/>

<set-attr-value name="dynamics" value="false"/>

<set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

<camera id="1"
            link="HEAD_LINK"
            direction="0.1 0 1"
            position="-2.0 0.0 7.0"/>
    <camera id="2"
            link="HEAD_LINK"
            direction="-0.1 0 1"
            position="2.0 0.0 7.0"/>
  </instanciate>

<instanciate class="seTV.xml">
    <set-attr-value name="name" value="TV_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="87.5"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="visStateAttrName" value="switch"/>
    <set-attr-value name="switch" value="on"/>
  </instanciate>

</world>

The difference between the Vision sensor is number of cameras. The agent has two cameras. the location and orientation of the cameras are modified.

The right camera has ID 1. The left camera has ID 2.

↑

Execution of the image capture †

$ sigserver.sh -w ./captureView.xml

You can see a humanoid robot agent in a living room.

You can send "rotation" and "move" to the agent to control the position and orientation of the agent.

Please send a message "capture" when you want to capture an image.

An image file captured by the camera ID 2 will be created in the starting directory