00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_quadrotor_controller/quadrotor_interface.h>
00030 #include <hector_quadrotor_controller/pid.h>
00031
00032 #include <controller_interface/controller.h>
00033
00034 #include <geometry_msgs/TwistStamped.h>
00035 #include <geometry_msgs/WrenchStamped.h>
00036 #include <std_srvs/Empty.h>
00037
00038 #include <ros/subscriber.h>
00039 #include <ros/callback_queue.h>
00040
00041 #include <boost/thread.hpp>
00042
00043 #include <limits>
00044
00045 namespace hector_quadrotor_controller {
00046
00047 using namespace controller_interface;
00048
00049 class TwistController : public controller_interface::Controller<QuadrotorInterface>
00050 {
00051 public:
00052 TwistController()
00053 {}
00054
00055 ~TwistController()
00056 {}
00057
00058 bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
00059 {
00060
00061 pose_ = interface->getPose();
00062 twist_ = interface->getTwist();
00063 acceleration_ = interface->getAcceleration();
00064 twist_input_ = interface->addInput<TwistCommandHandle>("twist");
00065 wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
00066 node_handle_ = root_nh;
00067
00068
00069 twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
00070 cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
00071
00072
00073 engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
00074 shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
00075
00076
00077 pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
00078 pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
00079 pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
00080 pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
00081 pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
00082 pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
00083
00084
00085 controller_nh.getParam("auto_engage", auto_engage_ = true);
00086 controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
00087 controller_nh.getParam("limits/force/z", limits_.force.z);
00088 controller_nh.getParam("limits/torque/xy", limits_.torque.x);
00089 controller_nh.getParam("limits/torque/xy", limits_.torque.y);
00090 controller_nh.getParam("limits/torque/z", limits_.torque.z);
00091 root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00092
00093
00094 interface->getMassAndInertia(mass_, inertia_);
00095
00096 command_given_in_stabilized_frame_ = false;
00097
00098 return true;
00099 }
00100
00101 void reset()
00102 {
00103 pid_.linear.x.reset();
00104 pid_.linear.y.reset();
00105 pid_.linear.z.reset();
00106 pid_.angular.x.reset();
00107 pid_.angular.y.reset();
00108 pid_.angular.z.reset();
00109
00110 wrench_.wrench.force.x = 0.0;
00111 wrench_.wrench.force.y = 0.0;
00112 wrench_.wrench.force.z = 0.0;
00113 wrench_.wrench.torque.x = 0.0;
00114 wrench_.wrench.torque.y = 0.0;
00115 wrench_.wrench.torque.z = 0.0;
00116
00117 linear_z_control_error_ = 0.0;
00118 motors_running_ = false;
00119 }
00120
00121 void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
00122 {
00123 boost::mutex::scoped_lock lock(command_mutex_);
00124
00125 command_ = *command;
00126 if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now();
00127 command_given_in_stabilized_frame_ = false;
00128
00129
00130 if (!isRunning()) this->startRequest(command_.header.stamp);
00131 }
00132
00133 void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command)
00134 {
00135 boost::mutex::scoped_lock lock(command_mutex_);
00136
00137 command_.twist = *command;
00138 command_.header.stamp = ros::Time::now();
00139 command_given_in_stabilized_frame_ = true;
00140
00141
00142 if (!isRunning()) this->startRequest(command_.header.stamp);
00143 }
00144
00145 bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00146 {
00147 boost::mutex::scoped_lock lock(command_mutex_);
00148
00149 ROS_INFO_NAMED("twist_controller", "Engaging motors!");
00150 motors_running_ = true;
00151 return true;
00152 }
00153
00154 bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00155 {
00156 boost::mutex::scoped_lock lock(command_mutex_);
00157
00158 ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
00159 motors_running_ = false;
00160 return true;
00161 }
00162
00163 void starting(const ros::Time &time)
00164 {
00165 reset();
00166 wrench_output_->start();
00167 }
00168
00169 void stopping(const ros::Time &time)
00170 {
00171 wrench_output_->stop();
00172 }
00173
00174 void update(const ros::Time& time, const ros::Duration& period)
00175 {
00176 boost::mutex::scoped_lock lock(command_mutex_);
00177
00178
00179 if (twist_input_->connected() && twist_input_->enabled()) {
00180 command_.twist = twist_input_->getCommand();
00181 command_given_in_stabilized_frame_ = false;
00182 }
00183
00184
00185 Twist command = command_.twist;
00186 Twist twist = twist_->twist();
00187 Twist twist_body;
00188 twist_body.linear = pose_->toBody(twist.linear);
00189 twist_body.angular = pose_->toBody(twist.angular);
00190
00191
00192 if (command_given_in_stabilized_frame_) {
00193 double yaw = pose_->getYaw();
00194 Twist transformed = command;
00195 transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
00196 transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
00197 transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
00198 transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
00199 command = transformed;
00200 }
00201
00202
00203 const double gravity = 9.8065;
00204 double load_factor = 1. / ( pose_->pose().orientation.w * pose_->pose().orientation.w
00205 - pose_->pose().orientation.x * pose_->pose().orientation.x
00206 - pose_->pose().orientation.y * pose_->pose().orientation.y
00207 + pose_->pose().orientation.z * pose_->pose().orientation.z );
00208
00209 if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit;
00210
00211
00212 if (auto_engage_) {
00213 if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) {
00214 motors_running_ = true;
00215 ROS_INFO_NAMED("twist_controller", "Engaging motors!");
00216 } else if (motors_running_ && command.linear.z < -0.1 ) {
00217 double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
00218 if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0;
00219 if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) {
00220 motors_running_ = false;
00221 ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
00222 } else {
00223 ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
00224 }
00225 } else {
00226 linear_z_control_error_ = 0.0;
00227 }
00228
00229
00230 if (motors_running_ && load_factor < 0.0) {
00231 motors_running_ = false;
00232 ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
00233 }
00234 }
00235
00236
00237 if (motors_running_) {
00238 Vector3 acceleration_command;
00239 acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
00240 acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
00241 acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
00242 Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
00243
00244 ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
00245 ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
00246 ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
00247 ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
00248 ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
00249 ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
00250 ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
00251
00252 wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
00253 wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
00254 wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period);
00255 wrench_.wrench.force.x = 0.0;
00256 wrench_.wrench.force.y = 0.0;
00257 wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
00258
00259 if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z;
00260 if (wrench_.wrench.force.z <= std::numeric_limits<double>::min()) wrench_.wrench.force.z = std::numeric_limits<double>::min();
00261 if (limits_.torque.x > 0.0) {
00262 if (wrench_.wrench.torque.x > limits_.torque.x) wrench_.wrench.torque.x = limits_.torque.x;
00263 if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x;
00264 }
00265 if (limits_.torque.y > 0.0) {
00266 if (wrench_.wrench.torque.y > limits_.torque.y) wrench_.wrench.torque.y = limits_.torque.y;
00267 if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y;
00268 }
00269 if (limits_.torque.z > 0.0) {
00270 if (wrench_.wrench.torque.z > limits_.torque.z) wrench_.wrench.torque.z = limits_.torque.z;
00271 if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z;
00272 }
00273
00274 ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
00275 ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
00276
00277 } else {
00278 reset();
00279 }
00280
00281
00282 wrench_.header.stamp = time;
00283 wrench_.header.frame_id = base_link_frame_;
00284 wrench_output_->setCommand(wrench_.wrench);
00285 }
00286
00287 private:
00288 PoseHandlePtr pose_;
00289 TwistHandlePtr twist_;
00290 AccelerationHandlePtr acceleration_;
00291 TwistCommandHandlePtr twist_input_;
00292 WrenchCommandHandlePtr wrench_output_;
00293
00294 ros::NodeHandle node_handle_;
00295 ros::Subscriber twist_subscriber_;
00296 ros::Subscriber cmd_vel_subscriber_;
00297 ros::ServiceServer engage_service_server_;
00298 ros::ServiceServer shutdown_service_server_;
00299
00300 geometry_msgs::TwistStamped command_;
00301 geometry_msgs::WrenchStamped wrench_;
00302 bool command_given_in_stabilized_frame_;
00303 std::string base_link_frame_;
00304
00305 struct {
00306 struct {
00307 PID x;
00308 PID y;
00309 PID z;
00310 } linear, angular;
00311 } pid_;
00312
00313 geometry_msgs::Wrench limits_;
00314 bool auto_engage_;
00315 double load_factor_limit;
00316 double mass_;
00317 double inertia_[3];
00318
00319 bool motors_running_;
00320 double linear_z_control_error_;
00321 boost::mutex command_mutex_;
00322
00323 };
00324
00325 }
00326
00327 #include <pluginlib/class_list_macros.h>
00328 PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)