RobotPlugin.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2015 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 __SWARM_ROBOT_PLUGIN_HH__
#define __SWARM_ROBOT_PLUGIN_HH__

#include <functional>
#include <map>
#include <mutex>
#include <string>
#include <vector>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/common/Console.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/UpdateInfo.hh>
#include <gazebo/physics/PhysicsTypes.hh>
#include <gazebo/sensors/sensors.hh>
#include <ignition/math/Angle.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/transport.hh>
#include <sdf/sdf.hh>

#include "msgs/datagram.pb.h"
#include "msgs/neighbor_v.pb.h"

namespace swarm
{
  typedef std::map<std::string, ignition::math::Pose3d> ObjPose_M;

  class ImageData
  {
    public: ObjPose_M objects;
  };


  class IGNITION_VISIBLE RobotPlugin : public gazebo::ModelPlugin
  {
    public: enum VehicleType
            {
              GROUND = 0,

              ROTOR = 1,

              FIXED_WING = 2
            };

    public: RobotPlugin();

    public: virtual ~RobotPlugin();

    protected: virtual void Load(sdf::ElementPtr _sdf);

    protected: virtual void Update(const gazebo::common::UpdateInfo &_info);

    protected: template<typename C>
    bool Bind(void(C::*_cb)(const std::string &_srcAddress,
                            const std::string &_dstAddress,
                            const uint32_t _dstPort,
                            const std::string &_data),
              C *_obj,
              const std::string &_address,
              const int _port = kDefaultPort)
    {
      // Sanity check: Make sure that you use your local address or multicast.
      if ((_address != this->kMulticast) && (_address != this->Host()))
      {
        gzerr << "[" << this->Host() << "] Bind() error: Address ["
              << _address << "] is not your local address" << std::endl;
        return false;
      }

      // Mapping the "unicast socket" to a topic name.
      const std::string unicastTopic =
        "/swarm/" + _address + "/" + std::to_string(_port);

      if (!this->node.Subscribe(unicastTopic,
          &RobotPlugin::OnMsgReceived, this))
      {
        gzerr << "RobotPlugin::Bind() error: Subscribe() returned an "
              << "error while subscribing the unicast/multicast address"
              << std::endl;
        return false;
      }

      // Register the user callback using the topic name as the key.
      this->callbacks[unicastTopic] = std::bind(_cb, _obj,
          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
          std::placeholders::_4);

      // Only enable broadcast if the address is a regular unicast address.
      if (_address != this->kMulticast)
      {
        const std::string bcastTopic =
          "/swarm/broadcast/" + std::to_string(_port);

        if (!this->node.Subscribe(bcastTopic,
            &RobotPlugin::OnMsgReceived, this))
        {
          gzerr << "RobotPlugin::Bind() error: Subscribe() returned an "
                << "error while subscribing the broadcast address"
                << std::endl;
          return false;
        }

        // Register the user callback using the broadcast topic as the key.
        this->callbacks[bcastTopic] = std::bind(_cb, _obj,
            std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
            std::placeholders::_4);
      }

      return true;
    }

    protected: bool SendTo(const std::string &_data,
                           const std::string &_dstAddress,
                           const uint32_t _port = kDefaultPort);

    protected: std::string Host() const;

    protected: std::vector<std::string> Neighbors() const;

    protected: VehicleType Type() const;

    protected: std::string Name() const;

    protected: bool SetLinearVelocity(
                   const ignition::math::Vector3d &_velocity);

    protected: bool SetLinearVelocity(const double _x,
                   const double _y, const double _z);

    protected: bool SetAngularVelocity(
                   const ignition::math::Vector3d &_velocity);

    protected: bool SetAngularVelocity(const double _x, const double _y,
                   const double _z);

    protected: bool Imu(ignition::math::Vector3d &_linVel,
                        ignition::math::Vector3d &_angVel,
                        ignition::math::Quaterniond &_orient) const;

    protected: bool Bearing(ignition::math::Angle &_bearing) const;

    protected: bool Pose(double &_latitude,
                         double &_longitude,
                         double &_altitude) const;

    protected: bool Image(ImageData &_img) const;

    protected: void SearchArea(double &_minLatitude,
                               double &_maxLatitude,
                               double &_minLongitude,
                               double &_maxLongitude);

    protected: double BatteryStartCapacity() const;

    protected: double BatteryCapacity() const;

    protected: double BatteryConsumption() const;

    protected: double BatteryConsumptionFactor() const;

    protected: double ExpectedBatteryLife() const;

    protected: ignition::math::Pose3d CameraToWorld(
      const ignition::math::Pose3d &_poseinCamera) const;

    private: virtual void Loop(const gazebo::common::UpdateInfo &_info);

    // Documentation Inherited.
    private: virtual void Load(gazebo::physics::ModelPtr _model,
                               sdf::ElementPtr _sdf);

    private: void OnMsgReceived(const std::string &_topic,
                                const msgs::Datagram &_msg);

    private: void OnNeighborsReceived(const std::string &_topic,
                                      const msgs::Neighbor_V &_msg);

    private: void AdjustPose();

    private: void TerrainLookup(const ignition::math::Vector3d &_pos,
                                ignition::math::Vector3d &_terrainPos,
                                ignition::math::Vector3d &_norm) const;

    private: void UpdateSensors();

    private: void UpdateBattery();

    using Callback_t =
    std::function<void(const std::string &_srcAddress,
                       const std::string &_dstAddress,
                       const uint32_t _dstPort,
                       const std::string &_data)>;

    protected: const std::string kBroadcast = "broadcast";

    protected: const std::string kMulticast = "multicast";

    protected: const std::string kBoo       = "boo";

    protected: static const uint32_t kDefaultPort = 4100;

    protected: static const uint32_t kBooPort     = 4200;

    protected: static const uint32_t kMtu = 1500;

    private: std::vector<std::string> neighbors;

    private: std::vector<double> neighborProbabilities;

    private: ignition::transport::Node node;

    // The gazebo transport node. Used for debugging, see source.
    // private: gazebo::transport::NodePtr gzNode;

    // Used to publish markers, Used for debugging, see source.
    // private: gazebo::transport::PublisherPtr markerPub;

    private: std::map<std::string, Callback_t> callbacks;

    private: gazebo::physics::ModelPtr model;

    private: std::string address;

    private: gazebo::event::ConnectionPtr updateConnection;

    private: VehicleType type;

    private: gazebo::sensors::GpsSensorPtr gps;

    private: gazebo::sensors::ImuSensorPtr imu;

    private: gazebo::sensors::LogicalCameraSensorPtr camera;

    private: double searchMinLatitude, searchMaxLatitude,
                    searchMinLongitude, searchMaxLongitude;

    private: mutable std::mutex mutex;

    private: gazebo::physics::HeightmapShapePtr terrain;

    private: ignition::math::Vector2d terrainScaling;

    private: ignition::math::Vector3d terrainSize;

    private: double modelHeight2;

    private: ignition::math::Vector3d linearVelocity;

    private: ignition::math::Vector3d angularVelocity;

    private: ignition::math::Quaterniond orientation;

    private: ignition::math::Angle bearing;

    private: double startCapacity;

    private: double capacity;

    private: double consumption;

    private: double consumptionFactor;

    private: gazebo::physics::WorldPtr world;

    friend class BooPlugin;
  };
}
#endif