ModelState.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 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_PHYSICS_MODELSTATE_HH_
#define GAZEBO_PHYSICS_MODELSTATE_HH_

#include <vector>
#include <string>
#include <boost/regex.hpp>

#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>

#include "gazebo/physics/State.hh"
#include "gazebo/physics/LinkState.hh"
#include "gazebo/physics/JointState.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace physics
  {

    class GZ_PHYSICS_VISIBLE ModelState : public State
    {
      public: ModelState();

      public: ModelState(const ModelPtr _model, const common::Time &_realTime,
                  const common::Time &_simTime, const uint64_t _iterations);

      public: explicit ModelState(const ModelPtr _model);

      public: explicit ModelState(const sdf::ElementPtr _sdf);

      public: virtual ~ModelState();

      public: void Load(const ModelPtr _model, const common::Time &_realTime,
                  const common::Time &_simTime, const uint64_t _iterations);

      public: virtual void Load(const sdf::ElementPtr _elem);

      public: const ignition::math::Pose3d &Pose() const;

      public: const ignition::math::Vector3d &Scale() const;

      public: bool IsZero() const;

      public: unsigned int GetLinkStateCount() const;

      public: LinkState_M GetLinkStates(const boost::regex &_regex) const;

      public: JointState_M GetJointStates(const boost::regex &_regex) const;

      public: LinkState GetLinkState(const std::string &_linkName) const;

      public: bool HasLinkState(const std::string &_linkName) const;

      public: const LinkState_M &GetLinkStates() const;

      public: unsigned int GetJointStateCount() const;

      public: JointState GetJointState(unsigned int _index) const;

      public: JointState GetJointState(const std::string &_jointName) const;

      public: const JointState_M &GetJointStates() const;

      public: bool HasJointState(const std::string &_jointName) const;

      public: unsigned int NestedModelStateCount() const;

      public: ModelState NestedModelState(const std::string &_modelName) const;

      public: bool HasNestedModelState(const std::string &_modelName) const;

      public: const ModelState_M &NestedModelStates() const;

      public: void FillSDF(sdf::ElementPtr _sdf);

      public: virtual void SetWallTime(const common::Time &_time);

      public: virtual void SetRealTime(const common::Time &_time);

      public: virtual void SetSimTime(const common::Time &_time);

      public: virtual void SetIterations(const uint64_t _iterations);

      public: ModelState &operator=(const ModelState &_state);

      public: ModelState operator-(const ModelState &_state) const;

      public: ModelState operator+(const ModelState &_state) const;

      public: inline friend std::ostream &operator<<(std::ostream &_out,
                  const gazebo::physics::ModelState &_state)
      {
        ignition::math::Vector3d euler(_state.pose.Rot().Euler());
        _out.unsetf(std::ios_base::floatfield);
        _out << std::setprecision(3)
          << "<model name='" << _state.GetName() << "'>"
          << "<pose>"
          << ignition::math::precision(_state.pose.Pos().X(), 4) << " "
          << ignition::math::precision(_state.pose.Pos().Y(), 4) << " "
          << ignition::math::precision(_state.pose.Pos().Z(), 4) << " "
          << ignition::math::precision(euler.X(), 4) << " "
          << ignition::math::precision(euler.Y(), 4) << " "
          << ignition::math::precision(euler.Z(), 4) << " "
          << "</pose>";

        // Only record scale if it is not the default value of [1, 1, 1].
        if (_state.scale != ignition::math::Vector3d::One)
          _out << "<scale>" << _state.scale << "</scale>";

        for (LinkState_M::const_iterator iter =
            _state.linkStates.begin(); iter != _state.linkStates.end();
            ++iter)
        {
          _out << iter->second;
        }

        for (const auto &ms : _state.modelStates)
        {
          _out << ms.second;
        }

        // Output the joint information
        // for (JointState_M::const_iterator iter =
        //     _state.jointStates.begin(); iter != _state.jointStates.end();
        //     ++iter)
        // {
        //   _out << iter->second;
        // }

        _out << "</model>";

        return _out;
      }

      private: ignition::math::Pose3d pose;

      private: ignition::math::Vector3d scale;

      private: LinkState_M linkStates;

      private: JointState_M jointStates;

      private: ModelState_M modelStates;
    };
  }
}
#endif