quadrotor_interface.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 
30 
31 #include <cmath>
32 
34 
36 {}
37 
39 {}
40 
41 bool QuadrotorInterface::enabled(const CommandHandle *handle) const
42 {
43  if (!handle || !handle->connected()) return false;
44  std::string resource = handle->getName();
45  return enabled_.count(resource) > 0;
46 }
47 
49 {
50  if (!handle || !handle->connected()) return false;
51  if (enabled(handle)) return true;
52  std::string resource = handle->getName();
53  enabled_[resource] = handle;
54  ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str());
55  return true;
56 }
57 
59 {
60  if (!handle) return;
61  if (!enabled(handle)) return;
62  std::string resource = handle->getName();
63  std::map<std::string, const CommandHandle *>::iterator it = enabled_.lower_bound(resource);
64  if (it != enabled_.end() && it->second == handle) enabled_.erase(it);
65  ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str());
66 }
67 
69 {
70  if (!handle) return;
71  std::string resource = handle->getName();
72  if (inputs_.count(resource)) {
73  const CommandHandlePtr& input = inputs_.at(resource);
74  if (input.get() != handle) input->reset();
75  }
76  if (outputs_.count(resource)) {
77  const CommandHandlePtr& output = outputs_.at(resource);
78  if (output.get() != handle) output->reset();
79  }
80 }
81 
82 const Pose *QuadrotorInterface::getPoseCommand() const { return getCommand<PoseCommandHandle>("pose"); }
83 const Twist *QuadrotorInterface::getTwistCommand() const { return getCommand<TwistCommandHandle>("twist"); }
84 const Wrench *QuadrotorInterface::getWrenchCommand() const { return getCommand<WrenchCommandHandle>("wrench"); }
85 const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand<MotorCommandHandle>("motor"); }
86 
87 void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const
88 {
89  const Quaternion::_w_type& w = pose().orientation.w;
90  const Quaternion::_x_type& x = pose().orientation.x;
91  const Quaternion::_y_type& y = pose().orientation.y;
92  const Quaternion::_z_type& z = pose().orientation.z;
93  roll = atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w);
94  pitch = -asin(2.*x*z - 2.*w*y);
95  yaw = atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
96 }
97 
98 double PoseHandle::getYaw() const
99 {
100  const Quaternion::_w_type& w = pose().orientation.w;
101  const Quaternion::_x_type& x = pose().orientation.x;
102  const Quaternion::_y_type& y = pose().orientation.y;
103  const Quaternion::_z_type& z = pose().orientation.z;
104  return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
105 }
106 
107 Vector3 PoseHandle::toBody(const Vector3& nav) const
108 {
109  const Quaternion::_w_type& w = pose().orientation.w;
110  const Quaternion::_x_type& x = pose().orientation.x;
111  const Quaternion::_y_type& y = pose().orientation.y;
112  const Quaternion::_z_type& z = pose().orientation.z;
113  Vector3 body;
114  body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z;
115  body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z;
116  body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z;
117  return body;
118 }
119 
120 Vector3 PoseHandle::fromBody(const Vector3& body) const
121 {
122  const Quaternion::_w_type& w = pose().orientation.w;
123  const Quaternion::_x_type& x = pose().orientation.x;
124  const Quaternion::_y_type& y = pose().orientation.y;
125  const Quaternion::_z_type& z = pose().orientation.z;
126  Vector3 nav;
127  nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z;
128  nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z;
129  nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z;
130  return nav;
131 }
132 
133 void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const
134 {
135  getCommand(x, y);
136  x -= pose.get()->position.x;
137  y -= pose.get()->position.y;
138 }
139 
140 double HeightCommandHandle::getError(const PoseHandle &pose) const
141 {
142  return getCommand() - pose.get()->position.z;
143 }
144 
146 {
147  if (get()) {
148  get()->x = 0.0;
149  get()->y = 0.0;
150  get()->z = sin(command / 2.);
151  get()->w = cos(command / 2.);
152  }
153 
154  if (scalar_) {
155  *scalar_ = command;
156  }
157 }
158 
160  if (scalar_) return *scalar_;
161  const Quaternion::_w_type& w = get()->w;
162  const Quaternion::_x_type& x = get()->x;
163  const Quaternion::_y_type& y = get()->y;
164  const Quaternion::_z_type& z = get()->z;
165  return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
166 }
167 
168 bool HeadingCommandHandle::update(Pose& command) const {
169  if (get()) {
170  command.orientation = *get();
171  return true;
172  }
173  if (scalar_) {
174  command.orientation.x = 0.0;
175  command.orientation.y = 0.0;
176  command.orientation.z = sin(*scalar_ / 2.);
177  command.orientation.x = cos(*scalar_ / 2.);
178  return true;
179  }
180  return false;
181 }
182 
183 double HeadingCommandHandle::getError(const PoseHandle &pose) const {
184  static const double M_2PI = 2.0 * M_PI;
185  double error = getCommand() - pose.getYaw() + M_PI;
186  error -= floor(error / M_2PI) * M_2PI;
187  return error - M_PI;
188 }
189 
190 bool CommandHandle::enabled() { return interface_->enabled(this); }
191 bool CommandHandle::start() { return interface_->start(this); }
192 void CommandHandle::stop() { interface_->stop(this); }
193 void CommandHandle::disconnect() { interface_->disconnect(this); }
194 
195 } // namespace hector_quadrotor_controller
const ValueType * get() const
Definition: handles.h:78
bool enabled(const CommandHandle *handle) const
HandleType::ValueType const * getCommand(const std::string &name) const
std::map< std::string, CommandHandlePtr > outputs_
std::map< std::string, const CommandHandle * > enabled_
#define ROS_DEBUG_NAMED(name,...)
virtual const std::string & getName() const
Definition: handles.h:177
virtual const MotorCommand * getMotorCommand() const
Vector3 toBody(const Vector3 &nav) const
double getError(const PoseHandle &pose) const
std::map< std::string, CommandHandlePtr > inputs_
double getError(const PoseHandle &pose) const
Vector3 fromBody(const Vector3 &nav) const
void getError(const PoseHandle &pose, double &x, double &y) const
void getEulerRPY(double &roll, double &pitch, double &yaw) const


hector_quadrotor_controller
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:30:48