ROSBasePlugin.hh

Go to the documentation of this file.

// Copyright (c) 2016 The UUV Simulator Authors.
// All rights reserved.
//
// 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 __ROS_BASE_PLUGIN_HH__
#define __ROS_BASE_PLUGIN_HH__

#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <uuv_sensor_ros_plugins/Common.hh>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <uuv_sensor_ros_plugins_msgs/ChangeSensorState.h>
#include <geometry_msgs/TransformStamped.h>
#include <gazebo/sensors/Noise.hh>
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>
#include <tf/tfMessage.h>
#include <tf/tf.h>
#include <chrono>
#include <random>
#include <string>
#include <map>


namespace gazebo
{
  class ROSBasePlugin
  {
    public: ROSBasePlugin();

    public: virtual ~ROSBasePlugin();

    public: bool InitBasePlugin(sdf::ElementPtr _sdf);

    public: virtual bool OnUpdate(const common::UpdateInfo&) = 0;

    public: bool AddNoiseModel(std::string _name, double _sigma);

    protected: std::string robotNamespace;

    protected: std::string sensorOutputTopic;

    protected: physics::WorldPtr world;

    protected: event::ConnectionPtr updateConnection;

    protected: common::Time lastMeasurementTime;

    protected: double updateRate;

    protected: double noiseSigma;

    protected: double noiseAmp;

    protected: bool gazeboMsgEnabled;

    protected: std::default_random_engine rndGen;

    protected: std::map<std::string, std::normal_distribution<double>>
      noiseModels;

    protected: std_msgs::Bool isOn;

    protected: boost::shared_ptr<ros::NodeHandle> rosNode;

    protected: transport::NodePtr gazeboNode;

    protected: ros::Publisher rosSensorOutputPub;

    protected: transport::PublisherPtr gazeboSensorOutputPub;

    protected: ros::ServiceServer changeSensorSrv;

    protected: ros::Publisher pluginStatePub;

    protected: ignition::math::Pose3d referenceFrame;

    protected: ros::Subscriber tfStaticSub;

    protected: std::string referenceFrameID;

    protected: bool isReferenceInit;

    protected: physics::LinkPtr referenceLink;

    protected: bool IsOn();

    protected: void PublishState();

    protected: bool ChangeSensorState(
        uuv_sensor_ros_plugins_msgs::ChangeSensorState::Request& _req,
        uuv_sensor_ros_plugins_msgs::ChangeSensorState::Response& _res);

    protected: void GetTFMessage(const tf::tfMessage::ConstPtr &_msg);

    protected: double GetGaussianNoise(double _amp);

    protected: double GetGaussianNoise(std::string _name, double _amp);

    protected: bool EnableMeasurement(const common::UpdateInfo& _info) const;

    protected: void UpdateReferenceFramePose();
  };
}

#endif // __ROS_BASE_PLUGIN_HH__