Diff of Image capture from agent's perspective

The added line is THIS COLOR.
The deleted line is THIS COLOR.
Go to Image capture from agent's perspective.
Deleting diff of Image capture from agent's perspective
Up:[[Tutorial]]　　 　　Previous：[[Vision sensor]]　　　　　Next：[[Collision detection by agents]]
----
#contents

* Image capture from agent's perspective [#b858da08]

(※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 [#xdb3ed39]

 $ emacs captureView.cpp

captureView.cpp

#highlight(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 [#aabb1384]

Receving an image data using captureView in 50th line.
#highlight(cpp:firstline[50]){{
 ViewImage *img = m_view->captureView(2, COLORBIT_24, IMAGE_320X240);
}}

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 [#qa8cd0af]

 $ ./sigmake.sh captureView.cpp


** Creating a world file [#e7dad340]

 $ emacs captureView.xml

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

captureView.xml
#highlight(xml){{

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

  <gravity x="0.0" y="-980.7" z="0.0"/>

  <!-- Set a humanoid robot agent Robot-nii -->
  <instanciate class="Robot-nii.xml">

    <!-- Set name of a humanoid robot agent -->
    <set-attr-value name="name" value="robot_000"/>

    <!-- Set language type as C++ for the controller -->
    <set-attr-value name="language" value="c++"/>

    <!-- Set a file name of the controller -->
    <set-attr-value name="implementation" value="./captureView.so"/>

    <!-- put the flag of dynamics to false -->
    <set-attr-value name="dynamics" value="false"/>

    <!-- Set the initial position of agent (x,y,z) -->
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

    <!-- Set the Camera ID, name of link on which the camera is attached, direction and position -->
    <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>

  <!-- Construction of a living room -->
  <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>

  <instanciate class="seDoll_Bear.xml">
    <set-attr-value name="name" value="kuma_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="9.9"/>
    <set-attr-value name="z" value="0.0"/>
  </instanciate>

  <instanciate class="seToy_D.xml">
    <set-attr-value name="name" value="penguin_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="50.0"/>
    <set-attr-value name="y" value="6.15"/>
    <set-attr-value name="z" value="-40.0"/>
  </instanciate>

  <instanciate class="seSofa_2seater.xml">
    <set-attr-value name="name" value="sofa_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-200.0"/>
    <set-attr-value name="y" value="31.85"/>
    <set-attr-value name="z" value="-100.0"/>
    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qx" value="0.0"/>
    <set-attr-value name="qy" value="0.707"/>
    <set-attr-value name="qz" value="0.0"/>
  </instanciate>

  <instanciate class="seTVbass_B.xml">
    <set-attr-value name="name" value="TVdai_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="25.1"/>
    <set-attr-value name="z" value="-250.0"/>
  </instanciate>

  <instanciate class="sePlant_B.xml">
    <set-attr-value name="name" value="ki_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="100.0"/>
    <set-attr-value name="y" value="56.5"/>
    <set-attr-value name="z" value="-250.0"/>
  </instanciate>

  <instanciate class="seSidetable_B.xml">
    <set-attr-value name="name" value="sidetable_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-100.0"/>
    <set-attr-value name="y" value="16.0"/>
    <set-attr-value name="z" value="-100.0"/>
    <set-attr-value name="qw" value="0.707"/>
    <set-attr-value name="qx" value="0.0"/>
    <set-attr-value name="qy" value="0.707"/>
    <set-attr-value name="qz" value="0.0"/>
  </instanciate>
  <instanciate class="seApple.xml">
    <set-attr-value name="name" value="apple_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-50.0"/>
    <set-attr-value name="y" value="3.875"/>
    <set-attr-value name="z" value="30.0"/>
  </instanciate>

  <instanciate class="seOrange.xml">
    <set-attr-value name="name" value="orange_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="70.0"/>
    <set-attr-value name="y" value="2.215"/>
    <set-attr-value name="z" value="-30.0"/>
  </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 [#e0ef2571]

 $ 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.

#ref(./capture_1.PNG,40%)

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


#highlight(end)

Up:[[Tutorial]]　　 　　Previous：[[Vision sensor]]　　　　　Next：[[Collision detection by agents]]


----
Up:[[Tutorial]]　　　 　Previous：[[Vision sensor]]　　　　　Next：[[Collision detection by agents]]


#counter