ModelState.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef _GAZEBO_MODELSTATE_HH_
19 #define _GAZEBO_MODELSTATE_HH_
20 
21 #include <vector>
22 #include <string>
23 #include <boost/regex.hpp>
24 
25 #include <ignition/math/Vector3.hh>
26 #include "gazebo/math/Pose.hh"
27 
28 #include "gazebo/physics/State.hh"
31 #include "gazebo/util/system.hh"
32 
33 namespace gazebo
34 {
35  namespace physics
36  {
39 
48  class GZ_PHYSICS_VISIBLE ModelState : public State
49  {
51  public: ModelState();
52 
61  public: ModelState(const ModelPtr _model, const common::Time &_realTime,
62  const common::Time &_simTime, const uint64_t _iterations);
63 
69  public: explicit ModelState(const ModelPtr _model);
70 
75  public: explicit ModelState(const sdf::ElementPtr _sdf);
76 
78  public: virtual ~ModelState();
79 
88  public: void Load(const ModelPtr _model, const common::Time &_realTime,
89  const common::Time &_simTime, const uint64_t _iterations);
90 
95  public: virtual void Load(const sdf::ElementPtr _elem);
96 
99  public: const math::Pose &GetPose() const;
100 
103  public: const ignition::math::Vector3d &Scale() const;
104 
107  public: bool IsZero() const;
108 
113  public: unsigned int GetLinkStateCount() const;
114 
119  public: LinkState_M GetLinkStates(const boost::regex &_regex) const;
120 
125  public: JointState_M GetJointStates(const boost::regex &_regex) const;
126 
134  public: LinkState GetLinkState(const std::string &_linkName) const;
135 
139  public: bool HasLinkState(const std::string &_linkName) const;
140 
143  public: const LinkState_M &GetLinkStates() const;
144 
149  public: unsigned int GetJointStateCount() const;
150 
158  public: JointState GetJointState(unsigned int _index) const;
159 
167  public: JointState GetJointState(const std::string &_jointName) const;
168 
171  public: const JointState_M &GetJointStates() const;
172 
176  public: bool HasJointState(const std::string &_jointName) const;
177 
182  public: unsigned int NestedModelStateCount() const;
183 
191  public: ModelState NestedModelState(const std::string &_modelName) const;
192 
196  public: bool HasNestedModelState(const std::string &_modelName) const;
197 
200  public: const ModelState_M &NestedModelStates() const;
201 
204  public: void FillSDF(sdf::ElementPtr _sdf);
205 
209  public: virtual void SetWallTime(const common::Time &_time);
210 
213  public: virtual void SetRealTime(const common::Time &_time);
214 
217  public: virtual void SetSimTime(const common::Time &_time);
218 
222  public: virtual void SetIterations(const uint64_t _iterations);
223 
227  public: ModelState &operator=(const ModelState &_state);
228 
232  public: ModelState operator-(const ModelState &_state) const;
233 
237  public: ModelState operator+(const ModelState &_state) const;
238 
243  public: inline friend std::ostream &operator<<(std::ostream &_out,
244  const gazebo::physics::ModelState &_state)
245  {
246  math::Vector3 q(_state.pose.rot.GetAsEuler());
247  _out << std::fixed <<std::setprecision(3)
248  << "<model name='" << _state.GetName() << "'>"
249  << "<pose>"
250  << _state.pose.pos.x << " "
251  << _state.pose.pos.y << " "
252  << _state.pose.pos.z << " "
253  << q.x << " "
254  << q.y << " "
255  << q.z << " "
256  << "</pose>"
257  << "<scale>"
258  << _state.scale
259  << "</scale>";
260 
261  for (LinkState_M::const_iterator iter =
262  _state.linkStates.begin(); iter != _state.linkStates.end();
263  ++iter)
264  {
265  _out << iter->second;
266  }
267 
268  for (const auto &ms : _state.modelStates)
269  {
270  _out << ms.second;
271  }
272 
273  // Output the joint information
274  // for (JointState_M::const_iterator iter =
275  // _state.jointStates.begin(); iter != _state.jointStates.end();
276  // ++iter)
277  // {
278  // _out << iter->second;
279  // }
280 
281  _out << "</model>";
282 
283  return _out;
284  }
285 
287  private: math::Pose pose;
288 
290  private: ignition::math::Vector3d scale;
291 
293  private: LinkState_M linkStates;
294 
296  private: JointState_M jointStates;
297 
299  private: ModelState_M modelStates;
300  };
302  }
303 }
304 #endif
double x
X location.
Definition: Vector3.hh:311
Quaternion rot
The rotation.
Definition: Pose.hh:255
double y
Y location.
Definition: Vector3.hh:314
Forward declarations for the common classes.
Definition: Animation.hh:33
std::map< std::string, ModelState > ModelState_M
Definition: PhysicsTypes.hh:224
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Vector3 GetAsEuler() const
Return the rotation in Euler angles.
std::map< std::string, JointState > JointState_M
Definition: PhysicsTypes.hh:236
State of an entity.
Definition: State.hh:49
double z
Z location.
Definition: Vector3.hh:317
std::string GetName() const
Get the name associated with this State.
keeps track of state of a physics::Joint
Definition: JointState.hh:46
Vector3 pos
The position.
Definition: Pose.hh:252
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::ModelState &_state)
Stream insertion operator.
Definition: ModelState.hh:243
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:88
std::map< std::string, LinkState > LinkState_M
Definition: PhysicsTypes.hh:232
Store state information of a physics::Model object.
Definition: ModelState.hh:48
Store state information of a physics::Link object.
Definition: LinkState.hh:49
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44