Klamp't  0.8.1
urdf_model_state.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: John Hsu */
36 
37 #ifndef URDF_MODEL_STATE_H
38 #define URDF_MODEL_STATE_H
39 
40 #include <string>
41 #include <vector>
42 #include <map>
43 #include <memory>
44 
45 #include "urdf_pose.h"
46 #include "urdf_twist.h"
47 
48 
49 namespace urdf{
50 
51 static int my_round (double x) {
52  int i = (int) x;
53  if (x >= 0.0) {
54  return ((x-i) >= 0.5) ? (i + 1) : (i);
55  } else {
56  return (-x+i >= 0.5) ? (i - 1) : (i);
57  }
58 }
59 
60 class Time
61 {
62 public:
63  Time() { this->clear(); };
64 
65  void set(double _seconds)
66  {
67  this->sec = (int32_t)(floor(_seconds));
68  this->nsec = (int32_t)(my_round((_seconds - this->sec) * 1e9));
69  this->Correct();
70  };
71 
72  operator double ()
73  {
74  return (static_cast<double>(this->sec) +
75  static_cast<double>(this->nsec)*1e-9);
76  };
77 
78  int32_t sec;
79  int32_t nsec;
80 
81  void clear()
82  {
83  this->sec = 0;
84  this->nsec = 0;
85  };
86 private:
87  void Correct()
88  {
89  // Make any corrections
90  if (this->nsec >= 1e9)
91  {
92  this->sec++;
93  this->nsec = (int32_t)(this->nsec - 1e9);
94  }
95  else if (this->nsec < 0)
96  {
97  this->sec--;
98  this->nsec = (int32_t)(this->nsec + 1e9);
99  }
100  };
101 };
102 
103 
105 {
106 public:
107  JointState() { this->clear(); };
108 
110  std::string joint;
111 
112  std::vector<double> position;
113  std::vector<double> velocity;
114  std::vector<double> effort;
115 
116  void clear()
117  {
118  this->joint.clear();
119  this->position.clear();
120  this->velocity.clear();
121  this->effort.clear();
122  }
123 };
124 
126 {
127 public:
128  ModelState() { this->clear(); };
129 
131  std::string name;
132 
133  Time time_stamp;
134 
135  void clear()
136  {
137  this->name.clear();
138  this->time_stamp.set(0);
139  this->joint_states.clear();
140  };
141 
142  std::vector<std::shared_ptr<JointState> > joint_states;
143 
144 };
145 
146 }
147 
148 #endif
149 
std::string joint
joint name
Definition: urdf_model_state.h:107
Definition: urdf_model_state.h:60
Definition: urdf_model_state.h:104
Definition: urdf_color.h:46
Definition: urdf_model_state.h:125
std::string name
state name must be unique
Definition: urdf_model_state.h:128