135 lines
4.5 KiB
C++
135 lines
4.5 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_PLUGINS_HARNESSPLUGIN_HH_
|
|
#define GAZEBO_PLUGINS_HARNESSPLUGIN_HH_
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "gazebo/transport/TransportTypes.hh"
|
|
#include "gazebo/common/PID.hh"
|
|
#include "gazebo/common/Plugin.hh"
|
|
|
|
namespace gazebo
|
|
{
|
|
/// \brief This plugin is designed to lower a model at a controlled rate.
|
|
/// Joints between a harness model and a model to lower are created
|
|
/// according to SDF provided to this plugin.
|
|
///
|
|
/// A winch joint and detach joint can be specified. The winch joint,
|
|
/// ideally a prismatic joint, has a PID controller. The detach joint,
|
|
/// which can be the same joint as the winch joint, is detached on a given
|
|
/// signal.
|
|
///
|
|
/// Two topics are created:
|
|
///
|
|
/// 1. ~/<plugin_model_name>/harness/velocity
|
|
/// - Message Type: GzString, expected to be a float
|
|
/// - Purpose: Set target winch velocity
|
|
///
|
|
/// 2. ~/<plugin_model_name>/harness/detach
|
|
/// - Message Type: GzString, expected to be a bool ("true")
|
|
/// - Purpose: Detach the <detach> joint.
|
|
///
|
|
/// For an example refer to:
|
|
/// - World file: worlds/harness.world
|
|
/// - Code: examples/stand_alone/harness
|
|
class GAZEBO_VISIBLE HarnessPlugin : public ModelPlugin
|
|
{
|
|
/// \brief Constructor.
|
|
public: HarnessPlugin();
|
|
|
|
/// \brief Destructor.
|
|
public: virtual ~HarnessPlugin();
|
|
|
|
// Documentation Inherited.
|
|
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
|
|
|
|
// Documentation Inherited.
|
|
public: virtual void Init();
|
|
|
|
/// \brief Set the target winch velocity
|
|
/// \param[in] _value Target winch velocity.
|
|
public: void SetWinchVelocity(const float _value);
|
|
|
|
/// \brief Get the current winch velocity
|
|
/// \return Velocity of the winch joint
|
|
public: double WinchVelocity() const;
|
|
|
|
/// \brief Detach the <detach> joint. Once the joint is detached, it
|
|
/// cannot be reattached.
|
|
public: void Detach();
|
|
|
|
/// \brief Callback for World Update events.
|
|
/// \param[in] _info Update information
|
|
private: void OnUpdate(const common::UpdateInfo &_info);
|
|
|
|
/// \brief Velocity control callback.
|
|
/// \param[in] _msg Message data, interpreted as a float
|
|
private: void OnVelocity(ConstGzStringPtr &_msg);
|
|
|
|
/// \brief Detach control callback.
|
|
/// \param[in] _msg Message data, interpreted as a bool
|
|
private: void OnDetach(ConstGzStringPtr &_msg);
|
|
|
|
/// \brief Get the index of a joint with the given name.
|
|
/// \param[in] _name Name of the joint to find.
|
|
/// \return Index into this->jointsto
|
|
private: int JointIndex(const std::string &_name) const;
|
|
|
|
/// \brief Vector of joints
|
|
private: std::vector<physics::JointPtr> joints;
|
|
|
|
/// \brief Index into the joints vector that specifies the winch joint.
|
|
private: int winchIndex = 0;
|
|
|
|
/// \brief Index into the joints vector that specifies the joint to detach.
|
|
private: int detachIndex = 0;
|
|
|
|
/// \brief Position PID controller for the winch
|
|
private: common::PID winchPosPID;
|
|
|
|
/// \brief Velocity PID controller for the winch
|
|
private: common::PID winchVelPID;
|
|
|
|
/// \brief Target winch position
|
|
private: float winchTargetPos = 0.0;
|
|
|
|
/// \brief Target winch velocity
|
|
private: float winchTargetVel = 0.0;
|
|
|
|
/// \brief Previous simulation time
|
|
private: common::Time prevSimTime = common::Time::Zero;
|
|
|
|
/// \brief Communication node
|
|
/// \todo: Transition to ignition-transport in gazebo8
|
|
private: transport::NodePtr node;
|
|
|
|
/// \brief Velocity control subscriber
|
|
/// \todo: Transition to ignition-transport in gazebo8
|
|
private: transport::SubscriberPtr velocitySub;
|
|
|
|
/// \brief Detach control subscriber
|
|
/// \todo: Transition to ignition-transport in gazebo8
|
|
private: transport::SubscriberPtr detachSub;
|
|
|
|
/// \brief Connection to World Update events.
|
|
private: event::ConnectionPtr updateConnection;
|
|
};
|
|
}
|
|
#endif
|