UnderwaterObjectROSPlugin.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 __UNDERWATER_OBJECT_ROS_PLUGIN_HH__
#define __UNDERWATER_OBJECT_ROS_PLUGIN_HH__

#include <string>

#include <uuv_gazebo_plugins/UnderwaterObjectPlugin.hh>

#include <boost/scoped_ptr.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Bool.h>
#include <visualization_msgs/Marker.h>
#include <uuv_gazebo_ros_plugins_msgs/SetUseGlobalCurrentVel.h>
#include <uuv_gazebo_ros_plugins_msgs/UnderwaterObjectModel.h>
#include <uuv_gazebo_ros_plugins_msgs/GetModelProperties.h>
#include <uuv_gazebo_ros_plugins_msgs/SetFloat.h>
#include <uuv_gazebo_ros_plugins_msgs/GetFloat.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

#include <map>

namespace uuv_simulator_ros
{
  class UnderwaterObjectROSPlugin : public gazebo::UnderwaterObjectPlugin
  {
    public: UnderwaterObjectROSPlugin();

    public: virtual ~UnderwaterObjectROSPlugin();

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

    public: virtual void Init();

    public: virtual void Reset();

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

    public: void UpdateLocalCurrentVelocity(
      const geometry_msgs::Vector3::ConstPtr &_msg);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    protected: virtual void PublishRestoringForce(
      gazebo::physics::LinkPtr _link);

    protected: virtual void PublishHydrodynamicWrenches(
      gazebo::physics::LinkPtr _link);

    protected: virtual void GenWrenchMsg(
      ignition::math::Vector3d _force, ignition::math::Vector3d _torque,
      geometry_msgs::WrenchStamped &_output);

    protected: virtual void InitDebug(gazebo::physics::LinkPtr _link,
      gazebo::HydrodynamicModelPtr _hydro);

    protected: virtual void PublishCurrentVelocityMarker();

    protected: virtual void PublishIsSubmerged();

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

    private: ros::Subscriber subLocalCurVel;

    private: std::map<std::string, ros::Publisher> rosHydroPub;

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

    private: geometry_msgs::TransformStamped nedTransform;

    private: tf2_ros::TransformBroadcaster tfBroadcaster;
  };
}

#endif  // __UNDERWATER_OBJECT_ROS_PLUGIN_HH__