Gazebo入門4
さて,今回はせっかくなので,ちょっと実戦向きなことをしてみましょう.
今回行うことは
- プラグインの作成
- シミュレーター内の写真を保存
メインはプラグインの作成になります.プラグインの作成については
を参考にしています.
さて、まずはプラグインの作成ということで,Cameraのプラグインを作りましょう.
ソースコードは以下となります.
以下に表示されるCameraPlugin.cc, CameraPlugin.hhはApache License, version 2.0 (http://www.apache.org/licenses/LICENSE-2.0) の元で公開されています.
CameraPlugin.cc
/*
* Copyright (C) 2012-2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
#include "gazebo/sensors/DepthCameraSensor.hh"
#include "CameraPlugin.hh"
using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CameraPlugin)
/////////////////////////////////////////////////
CameraPlugin::CameraPlugin()
: SensorPlugin(), width(0), height(0), depth(0)
{
}
/////////////////////////////////////////////////
CameraPlugin::~CameraPlugin()
{
this->parentSensor.reset();
this->camera.reset();
}
/////////////////////////////////////////////////
void CameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr /*_sdf*/)
{
if (!_sensor)
gzerr << "Invalid sensor pointer.\n";
this->parentSensor =
std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
if (!this->parentSensor)
{
gzerr << "CameraPlugin requires a CameraSensor.\n";
if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
gzmsg << "It is a depth camera sensor\n";
}
this->camera = this->parentSensor->Camera();
if (!this->parentSensor)
{
gzerr << "CameraPlugin not attached to a camera sensor\n";
return;
}
this->width = this->camera->ImageWidth();
this->height = this->camera->ImageHeight();
this->depth = this->camera->ImageDepth();
this->format = this->camera->ImageFormat();
this->newFrameConnection = this->camera->ConnectNewImageFrame(
std::bind(&CameraPlugin::OnNewFrame, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5));
this->parentSensor->SetActive(true);
}
/////////////////////////////////////////////////
void CameraPlugin::OnNewFrame(const unsigned char * _image,
unsigned int /*width*/,
unsigned int /*_height*/,
unsigned int /*_depth*/,
const std::string &/*_format*/)
{
rendering::Camera::SaveFrame(_image, this->width,
this->height, this->depth, this->format,
"me.jpg");
}
CameraPlugin.hh
/*
* Copyright (C) 2012-2016 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef _GAZEBO_CAMERA_PLUGIN_HH_
#define _GAZEBO_CAMERA_PLUGIN_HH_
#include <string>
#include "gazebo/common/Plugin.hh"
#include "gazebo/sensors/CameraSensor.hh"
#include "gazebo/rendering/Camera.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
class GAZEBO_VISIBLE CameraPlugin : public SensorPlugin
{
public: CameraPlugin();
/// \brief Destructor
public: virtual ~CameraPlugin();
public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
public: virtual void OnNewFrame(const unsigned char *_image,
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &_format);
protected: unsigned int width, height, depth;
protected: std::string format;
protected: sensors::CameraSensorPtr parentSensor;
protected: rendering::CameraPtr camera;
private: event::ConnectionPtr newFrameConnection;
};
}
#endif
さて,このプログラムはxml(あるいはworld)で定義されたCameraのフレームごとにカメラが見ている画像をカレントディレクトリにme.jpgとして保存します.
ソースコードの解説の前に動かしてみましょう.
コンパイルするために以下のCMakeLists.txtを用意してください.
CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(CameraPlugin SHARED CameraPlugin.cc)
target_link_libraries(CameraPlugin ${GAZEBO_LIBRARIES})
そして次に以下のコマンドでコンパイルをします.
mkdir build cd build cmake ../ make
コンパイルに成功するとbuildの中にlibCameraPlugin.soファイルが出来上がっていると思います.
そして次のxmlファイルを用意します.名前は何でも良いです.
<sdf version='1.6'>
<world name='default'>
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<model name="camera">
<link name="link">
<gravity>false</gravity>
<pose>0.05 0.05 0.05 0 0 0</pose>
<inertial>
<mass>0.1</mass>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="camera" type="camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<!-- プラグインの挿入 -->
<plugin name="CameraPlugin" filename="libCameraPlugin.so"/>
</sensor>
</link>
</model>
</world>
</sdf>
ここまで用意をして,GAZEBO_PLUGIN_PATHの環境変数をlibCameraPlugin.soがあるディレクトリに設定をして,
gazebo <world file name> --verbose
そうすると
こんな感じの画面が起動します.
カレントディレクトリを見てみるとme.jpgというのがあると思います.
カメラの前にモデルを置くと画像も変わるかと思います.
