ThrusterROSPlugin.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 __THRUSTER_ROS_PLUGIN_HH__
#define __THRUSTER_ROS_PLUGIN_HH__

#include <map>
#include <string>
#include <vector>

#include <uuv_gazebo_plugins/ThrusterPlugin.hh>

#include <boost/scoped_ptr.hpp>
#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include <uuv_gazebo_ros_plugins_msgs/FloatStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>

#include <uuv_gazebo_ros_plugins_msgs/SetThrusterState.h>
#include <uuv_gazebo_ros_plugins_msgs/GetThrusterState.h>
#include <uuv_gazebo_ros_plugins_msgs/SetThrusterEfficiency.h>
#include <uuv_gazebo_ros_plugins_msgs/GetThrusterEfficiency.h>
#include <uuv_gazebo_ros_plugins_msgs/GetThrusterConversionFcn.h>

namespace uuv_simulator_ros
{
  class ThrusterROSPlugin : public gazebo::ThrusterPlugin
  {
    public: ThrusterROSPlugin();

    public: ~ThrusterROSPlugin();

    public: void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf);

    public: void RosPublishStates();

    public: void SetThrustReference(
        const uuv_gazebo_ros_plugins_msgs::FloatStamped::ConstPtr &_msg);

    public: gazebo::common::Time  GetRosPublishPeriod();

    public: void SetRosPublishRate(double _hz);

    public: virtual void Init();

    public: virtual void Reset();

    public: bool SetThrustForceEfficiency(
      uuv_gazebo_ros_plugins_msgs::SetThrusterEfficiency::Request& _req,
      uuv_gazebo_ros_plugins_msgs::SetThrusterEfficiency::Response& _res);

    public: bool GetThrustForceEfficiency(
      uuv_gazebo_ros_plugins_msgs::GetThrusterEfficiency::Request& _req,
      uuv_gazebo_ros_plugins_msgs::GetThrusterEfficiency::Response& _res);

    public: bool SetDynamicStateEfficiency(
      uuv_gazebo_ros_plugins_msgs::SetThrusterEfficiency::Request& _req,
      uuv_gazebo_ros_plugins_msgs::SetThrusterEfficiency::Response& _res);

    public: bool GetDynamicStateEfficiency(
        uuv_gazebo_ros_plugins_msgs::GetThrusterEfficiency::Request& _req,
        uuv_gazebo_ros_plugins_msgs::GetThrusterEfficiency::Response& _res);

    public: bool SetThrusterState(
      uuv_gazebo_ros_plugins_msgs::SetThrusterState::Request& _req,
      uuv_gazebo_ros_plugins_msgs::SetThrusterState::Response& _res);

    public: bool GetThrusterState(
      uuv_gazebo_ros_plugins_msgs::GetThrusterState::Request& _req,
      uuv_gazebo_ros_plugins_msgs::GetThrusterState::Response& _res);

    public: bool GetThrusterConversionFcn(
      uuv_gazebo_ros_plugins_msgs::GetThrusterConversionFcn::Request& _req,
      uuv_gazebo_ros_plugins_msgs::GetThrusterConversionFcn::Response& _res);

    private: std::map<std::string, ros::ServiceServer> services;

    private: boost::scoped_ptr<ros::NodeHandle> rosNode;

    private: ros::Subscriber subThrustReference;

    private: ros::Publisher pubThrust;

    private: ros::Publisher pubThrustWrench;

    private: ros::Publisher pubThrusterState;

    private: ros::Publisher pubThrustForceEff;

    private: ros::Publisher pubDynamicStateEff;

    private: gazebo::event::ConnectionPtr rosPublishConnection;

    private: gazebo::common::Time rosPublishPeriod;

    private: gazebo::common::Time lastRosPublishTime;
  };
}

#endif  // __THRUSTER_ROS_PLUGIN_HH__