state.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
31 
32 #include <ros/console.h>
33 
34 namespace hector_pose_estimation {
35 
37 {
38  orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>("orientation");
39  rate_ = addSubState<RateStateType::VectorDimension,RateStateType::CovarianceDimension>("rate");
40  position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>("position");
41  velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>("velocity");
42  construct();
43 }
44 
46 
48 {
49  orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>("orientation");
50  position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>("position");
51  velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>("velocity");
52  construct();
53 }
54 
56 
58 {
59  orientation_ = addSubState<OrientationStateType::VectorDimension,OrientationStateType::CovarianceDimension>("orientation");
60  construct();
61 }
62 
64 
66 {
67  position_ = addSubState<PositionStateType::VectorDimension,PositionStateType::CovarianceDimension>("position");
68  velocity_ = addSubState<VelocityStateType::VectorDimension,VelocityStateType::CovarianceDimension>("velocity");
69  construct();
70 }
71 
73 
75 
77 
79 {
81  reset();
82 }
83 
85 {
86  // reset status flags
87  system_status_ = 0;
89 
90  // reset pseudo-states
91  fake_rate_ = Vector::Zero(3,1);
92  fake_orientation_ = Vector::Zero(4,1);
93  fake_position_ = Vector::Zero(3,1);
94  fake_velocity_ = Vector::Zero(3,1);
95  fake_acceleration_ = Vector::Zero(3,1);
96 
97  // reset state
98  vector_.setZero();
99  covariance_.setZero();
100  fake_orientation_.w() = 1.0;
101  if (orientation()) orientation()->vector().w() = 1.0;
102 
103  R_valid_ = false;
104 }
105 
106 bool State::valid() const {
107  return (vector_ == vector_);
108 }
109 
111 {
112  normalize();
113  R_valid_ = false;
114 }
115 
121 
123 {
125  R = q.toRotationMatrix();
126 // R << (q.w()*q.w()+q.x()*q.x()-q.y()*q.y()-q.z()*q.z()), (2.0*q.x()*q.y()-2.0*q.w()*q.z()), (2.0*q.x()*q.z()+2.0*q.w()*q.y()),
127 // (2.0*q.x()*q.y()+2.0*q.w()*q.z()), (q.w()*q.w()-q.x()*q.x()+q.y()*q.y()-q.z()*q.z()), (2.0*q.y()*q.z()-2.0*q.w()*q.x()),
128 // (2.0*q.x()*q.z()-2.0*q.w()*q.y()), (2.0*q.y()*q.z()+2.0*q.w()*q.x()), (q.w()*q.w()-q.x()*q.x()-q.y()*q.y()+q.z()*q.z());
129 }
130 
132  if (!R_valid_) {
134  R_valid_ = true;
135  }
136  return R_;
137 }
138 
139 double State::getYaw() const
140 {
142  return atan2(2*q.x()*q.y() + 2*q.w()*q.z(), q.x()*q.x() + q.w()*q.w() - q.z()*q.z() - q.y()*q.y());
143 }
144 
145 void State::getEuler(double &roll, double &pitch, double &yaw) const
146 {
148  roll = atan2(2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
149  pitch = -asin(2*(q.x()*q.z() - q.w()*q.y()));
150  yaw = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
151 }
152 
154 {
155  ColumnVector3 euler;
156  getEuler(euler(0), euler(1), euler(2));
157  return euler;
158 }
159 
160 void State::update(const Vector &vector_update)
161 {
162  if (orientation()) {
163  // if vector_update has only n - 1 elements, we have to use covariance indices
164  int orientation_index, orientation_size;
165  if (vector_update.size() == getVectorDimension() - 1) {
166  orientation_index = orientation()->getCovarianceIndex();
167  orientation_size = orientation()->getCovarianceDimension();
168  } else {
169  assert(vector_update.size() == getVectorDimension());
170  orientation_index = orientation()->getVectorIndex();
171  orientation_size = orientation()->getVectorDimension();
172  }
173 
174  // add everything before orientation part
175  if (orientation_index > 0) {
176  int length = orientation_index;
177  x().head(length) += vector_update.head(length);
178  }
179 
180  // add everything after orientation part
181  if (orientation_index + orientation_size < vector_update.size()) {
182  int length = vector_update.size() - orientation_index - orientation_size;
183  x().tail(length) += vector_update.tail(length);
184  }
185 
186  // update orientation
187  updateOrientation(vector_update.segment<3>(orientation_index));
188 
189  } else {
190  // simply add vectors
191  x() += vector_update;
192  }
193 }
194 
195 void State::updateOrientation(const ColumnVector3 &rotation_vector)
196 {
197  if (!orientation()) return;
198 
199 // Eigen::Quaterniond q(orientation()->vector().data());
200 // q = Eigen::Quaterniond(1.0, 0.5 * rotation_vector.x(), 0.5 * rotation_vector.y(), 0.5 * rotation_vector.z()) * q;
201 // orientation()->vector() = q.normalized().coeffs();
202 
203  // Eigen::Map<Eigen::Quaterniond> q(orientation()->vector().data());
204  Eigen::Quaterniond q(orientation()->vector().data());
205  q = Eigen::Quaterniond().fromRotationVector(rotation_vector) * q;
206  orientation()->vector() = q.coeffs();
207 
208  R_valid_ = false;
209 }
210 
211 bool State::inSystemStatus(SystemStatus test_status) const {
212  return (getSystemStatus() & test_status) == test_status;
213 }
214 
216  if (new_status == system_status_) return true;
217 
218  // iterate through StatusCallbacks
219  for(std::vector<SystemStatusCallback>::const_iterator it = status_callbacks_.begin(); it != status_callbacks_.end(); ++it)
220  if (!(*it)(new_status)) return false;
221 
222  SystemStatus set = new_status & ~system_status_;
223  SystemStatus cleared = system_status_ & ~new_status;
224  if (set) ROS_INFO_STREAM("Set system status " << getSystemStatusString(new_status, set));
225  if (cleared) ROS_INFO_STREAM("Cleared system status " << getSystemStatusString(cleared, cleared));
226 
227  system_status_ = new_status;
228  return true;
229 }
230 
231 bool State::setMeasurementStatus(SystemStatus new_measurement_status) {
232  SystemStatus set = new_measurement_status & ~measurement_status_;
233  SystemStatus cleared = measurement_status_ & ~new_measurement_status;
234  if (set) ROS_INFO_STREAM("Set measurement status " << getSystemStatusString(new_measurement_status, set));
235  if (cleared) ROS_INFO_STREAM("Cleared measurement status " << getSystemStatusString(cleared, cleared));
236 
237  measurement_status_ = new_measurement_status;
238  return true;
239 }
240 
242  return setSystemStatus((system_status_ & ~clear) | set);
243 }
244 
246  return setMeasurementStatus((measurement_status_ & ~clear) | set);
247 }
248 
250 // for(std::vector<SystemStatusCallback>::const_iterator it = status_callbacks_.begin(); it != status_callbacks_.end(); ++it)
251 // if (*it == callback) return;
252  status_callbacks_.push_back(callback);
253 }
254 
256  if (orientation()) {
257  double s = 1.0 / orientation()->vector().norm();
258  orientation()->vector() = orientation()->vector() * s;
259  }
260 }
261 
263 {
264  setOrientation(orientation.coeffs());
265 }
266 
268 {
269  ScalarType roll, pitch;
270  roll = atan2(2*(q.y()*q.z() + q.w()*q.x()), q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
271  pitch = -asin(2*(q.x()*q.z() - q.w()*q.y()));
272  setRollPitch(roll, pitch);
273 }
274 
276 {
277  ScalarType yaw = getYaw();
278  fake_orientation_ = Quaternion(Eigen::AngleAxis<ScalarType>(yaw, ColumnVector3::UnitZ()) * Eigen::AngleAxis<ScalarType>(pitch, ColumnVector3::UnitY()) * Eigen::AngleAxis<ScalarType>(roll, ColumnVector3::UnitX())).coeffs();
279 }
280 
282 {
283  const Quaternion &q = orientation;
284  double yaw = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
285  setYaw(yaw);
286 }
287 
289 {
290  ColumnVector3 euler = getEuler();
291  fake_orientation_ = Quaternion(Eigen::AngleAxis<ScalarType>(yaw, ColumnVector3::UnitZ()) * Eigen::AngleAxis<ScalarType>(euler(1), ColumnVector3::UnitY()) * Eigen::AngleAxis<ScalarType>(euler(2), ColumnVector3::UnitX())).coeffs();
292 }
293 
295 template class SubState_<Dynamic,Dynamic>;
296 
297 } // namespace hector_pose_estimation
virtual void updated()
Definition: state.cpp:110
void setOrientation(const Quaternion &orientation)
Definition: state.cpp:262
virtual const boost::shared_ptr< OrientationStateType > & orientation() const
Definition: state.h:112
std::string getSystemStatusString(const SystemStatus &status, const SystemStatus &asterisk_status=0)
Definition: types.cpp:33
Matrix_< 3, 3 >::type RotationMatrix
Definition: state.h:71
VectorBlock< const Vector, 3 > ConstAccelerationType
Definition: state.h:67
boost::function< bool(SystemStatus &)> SystemStatusCallback
Definition: state.h:109
XmlRpcServer s
virtual ConstRateType getRate() const
Definition: state.cpp:117
void setYaw(const Quaternion &orientation)
Definition: state.cpp:281
virtual void normalize()
Definition: state.cpp:255
const State::RotationMatrix & R() const
Definition: state.cpp:131
virtual ConstVelocityType getVelocity() const
Definition: state.cpp:119
SystemStatus measurement_status_
Definition: state.h:173
virtual const boost::shared_ptr< PositionStateType > & position() const
Definition: state.h:114
virtual ConstOrientationType getOrientation() const
Definition: state.cpp:116
virtual void updateOrientation(const ColumnVector3 &rotation_vector)
Definition: state.cpp:195
ColumnVector3 getEuler() const
Definition: state.cpp:153
boost::shared_ptr< PositionStateType > position_
Definition: state.h:184
Eigen::Quaternion< ScalarType > Quaternion
Definition: matrix.h:49
virtual void reset()
Definition: state.cpp:84
boost::shared_ptr< BaseState > base_
Definition: state.h:181
virtual IndexType getCovarianceDimension() const
Definition: state.h:77
virtual const Vector & getVector() const
Definition: state.h:88
boost::shared_ptr< VelocityStateType > velocity_
Definition: state.h:185
std::vector< SystemStatusCallback > status_callbacks_
Definition: state.h:175
double getYaw() const
Definition: state.cpp:139
virtual bool inSystemStatus(SystemStatus test_status) const
Definition: state.cpp:211
unsigned int SystemStatus
Definition: types.h:70
boost::shared_ptr< RateStateType > rate_
Definition: state.h:183
virtual const boost::shared_ptr< VelocityStateType > & velocity() const
Definition: state.h:115
virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear)
Definition: state.cpp:241
SystemStatus system_status_
Definition: state.h:172
virtual IndexType getVectorDimension() const
Definition: state.h:76
virtual SystemStatus getSystemStatus() const
Definition: state.h:100
virtual bool setMeasurementStatus(SystemStatus new_status)
Definition: state.cpp:231
virtual bool valid() const
Definition: state.cpp:106
virtual ConstAccelerationType getAcceleration() const
Definition: state.cpp:120
VectorBlock< const Vector, 4 > ConstOrientationType
Definition: state.h:55
void getRotationMatrix(RotationMatrix &R) const
Definition: state.cpp:122
#define ROS_INFO_STREAM(args)
virtual Vector & x()
Definition: state.h:92
virtual bool setSystemStatus(SystemStatus new_status)
Definition: state.cpp:215
virtual const boost::shared_ptr< RateStateType > & rate() const
Definition: state.h:113
VectorBlock< const Vector, 3 > ConstRateType
Definition: state.h:58
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void setRollPitch(const Quaternion &orientation)
Definition: state.cpp:267
VectorBlock< const Vector, 3 > ConstPositionType
Definition: state.h:61
virtual void update(const Vector &vector_update)
Definition: state.cpp:160
virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear)
Definition: state.cpp:245
ColumnVector_< 3 >::type ColumnVector3
Definition: matrix.h:59
boost::shared_ptr< OrientationStateType > orientation_
Definition: state.h:182
virtual ConstPositionType getPosition() const
Definition: state.cpp:118
VectorBlock< const Vector, 3 > ConstVelocityType
Definition: state.h:64
virtual const boost::shared_ptr< AccelerationStateType > & acceleration() const
Definition: state.h:116
virtual void addSystemStatusCallback(const SystemStatusCallback &callback)
Definition: state.cpp:249


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Thu Feb 18 2021 03:29:30