84 lines
2.6 KiB
C++
84 lines
2.6 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 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_FOLLOWER_PLUGIN_HH_
|
||
|
|
#define _GAZEBO_FOLLOWER_PLUGIN_HH_
|
||
|
|
|
||
|
|
#include <memory>
|
||
|
|
#include <string>
|
||
|
|
|
||
|
|
#include <sdf/sdf.hh>
|
||
|
|
|
||
|
|
#include <gazebo/common/Plugin.hh>
|
||
|
|
#include <gazebo/physics/PhysicsTypes.hh>
|
||
|
|
|
||
|
|
namespace gazebo
|
||
|
|
{
|
||
|
|
// Forward declare private class
|
||
|
|
struct FollowerPluginPrivate;
|
||
|
|
|
||
|
|
/// \brief A simple object follower that finds the closest object in a
|
||
|
|
/// depth image and commands a differential drive vehicle to move towards the
|
||
|
|
/// object. The plugin is essentially an integration of the DiffDrivePlugin
|
||
|
|
/// and DepthCameraPlugin.
|
||
|
|
class GAZEBO_VISIBLE FollowerPlugin : public ModelPlugin
|
||
|
|
{
|
||
|
|
/// \brief Constructor.
|
||
|
|
public: FollowerPlugin();
|
||
|
|
|
||
|
|
/// \brief Destructor.
|
||
|
|
public: ~FollowerPlugin();
|
||
|
|
|
||
|
|
// Documentation Inherited.
|
||
|
|
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
|
||
|
|
|
||
|
|
// Documentation Inherited.
|
||
|
|
public: virtual void Init();
|
||
|
|
|
||
|
|
// Documentation Unherited.
|
||
|
|
public: virtual void Reset();
|
||
|
|
|
||
|
|
/// \brief Callback when a new depth camera frame is received.
|
||
|
|
/// \param[in] _image Depth image data buffer
|
||
|
|
/// \param[in] _width Image width
|
||
|
|
/// \param[in] _height Image height
|
||
|
|
/// \param[in] _depth Image depth
|
||
|
|
/// \param[in] _format Image format
|
||
|
|
private: void OnNewDepthFrame(const float *_image,
|
||
|
|
const unsigned int _width, const unsigned int _height,
|
||
|
|
const unsigned int _depth, const std::string &_format);
|
||
|
|
|
||
|
|
/// \brief Update the follower.
|
||
|
|
private: void OnUpdate();
|
||
|
|
|
||
|
|
/// \brief Find a depth sensor in the model.
|
||
|
|
/// \param[in] _model Model to search for depth sensor.
|
||
|
|
/// \return True if depth sensor is found.
|
||
|
|
private: bool FindSensor(const physics::ModelPtr &_model);
|
||
|
|
|
||
|
|
/// \brief Find revolute joints in the model.
|
||
|
|
private: void FindJoints();
|
||
|
|
|
||
|
|
/// \brief Update the follower.
|
||
|
|
private: void UpdateFollower();
|
||
|
|
|
||
|
|
/// \brief Pointer to private data
|
||
|
|
private: std::unique_ptr<FollowerPluginPrivate> dataPtr;
|
||
|
|
};
|
||
|
|
}
|
||
|
|
#endif
|