offboard_control.h
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2015 Nuno Marques, Andre Nguyen.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #include <array>
00019 #include <random>
00020 #include <angles/angles.h>
00021 #include <eigen_conversions/eigen_msg.h>
00022 #include <test_mavros/sitl_test/sitl_test.h>
00023 
00024 #include <geometry_msgs/Point.h>
00025 #include <geometry_msgs/PoseStamped.h>
00026 #include <geometry_msgs/TwistStamped.h>
00027 
00028 namespace testsetup {
00036 typedef enum {
00037         POSITION,
00038         VELOCITY,
00039         ACCELERATION
00040 } control_mode;
00041 
00042 typedef enum {
00043         SQUARE,
00044         CIRCLE,
00045         EIGHT,
00046         ELLIPSE
00047 } path_shape;
00048 
00049 class OffboardControl {
00050 public:
00051         OffboardControl() :
00052                 nh_sp(test.nh),
00053                 local_pos_sp_pub(nh_sp.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10)),
00054                 vel_sp_pub(nh_sp.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10)),
00055                 local_pos_sub(nh_sp.subscribe("/mavros/local_position/local", 10, &OffboardControl::local_pos_cb, this)),
00056                 threshold(threshold_definition())
00057         { };
00058 
00059         void init() {
00063                 test.setup(nh_sp);
00064                 rate = test.rate;
00065                 use_pid = test.use_pid;
00066                 num_of_tests = test.num_of_tests;
00067 
00068                 if (use_pid) {
00074                         // Linear velocity PID gains and bound of integral windup
00075                         nh_sp.param("linvel_p_gain", linvel_p_gain, 0.4);
00076                         nh_sp.param("linvel_i_gain", linvel_i_gain, 0.05);
00077                         nh_sp.param("linvel_d_gain", linvel_d_gain, 0.12);
00078                         nh_sp.param("linvel_i_max", linvel_i_max, 0.1);
00079                         nh_sp.param("linvel_i_min", linvel_i_min, -0.1);
00080 
00081                         // Yaw rate PID gains and bound of integral windup
00082                         nh_sp.param("yawrate_p_gain", yawrate_p_gain, 0.011);
00083                         nh_sp.param("yawrate_i_gain", yawrate_i_gain, 0.00058);
00084                         nh_sp.param("yawrate_d_gain", yawrate_d_gain, 0.12);
00085                         nh_sp.param("yawrate_i_max", yawrate_i_max, 0.005);
00086                         nh_sp.param("yawrate_i_min", yawrate_i_min, -0.005);
00087 
00088                         // Setup of the PID controllers
00089                         pid.setup_linvel_pid(linvel_p_gain, linvel_i_gain, linvel_d_gain, linvel_i_max, linvel_i_min, nh_sp);
00090                         pid.setup_yawrate_pid(yawrate_p_gain, yawrate_i_gain, yawrate_d_gain, yawrate_i_max, yawrate_i_min, nh_sp);
00091                 }
00092 
00101                 std::string mode_;
00102                 nh_sp.param<std::string>("mode", mode_, "position");
00103 
00113                 std::string shape_;
00114                 nh_sp.param<std::string>("shape", shape_, "square");
00115 
00116                 if (mode_ == "position")
00117                         mode = POSITION;
00118                 else if (mode_ == "velocity")
00119                         mode = VELOCITY;
00120                 else if (mode_ == "acceleration")
00121                         mode = ACCELERATION;
00122                 else {
00123                         ROS_ERROR_NAMED("sitl_test", "Control mode: wrong/unexistant control mode name %s", mode_.c_str());
00124                         return;
00125                 }
00126 
00127                 if (shape_ == "square")
00128                         shape = SQUARE;
00129                 else if (shape_ == "circle")
00130                         shape = CIRCLE;
00131                 else if (shape_ == "eight")
00132                         shape = EIGHT;
00133                 else if (shape_ == "ellipse")
00134                         shape = ELLIPSE;
00135                 else {
00136                         ROS_ERROR_NAMED("sitl_test", "Path shape: wrong/unexistant path shape name %s", shape_.c_str());
00137                         return;
00138                 }
00139         }
00140 
00141         /* -*- main routine -*- */
00142 
00143         void spin(int argc, char *argv[]) {
00144                 init();
00145                 ros::Rate loop_rate(rate);
00146 
00147                 ROS_INFO("SITL Test: Offboard control test running!");
00148 
00149                 if (mode == POSITION) {
00150                         ROS_INFO("Position control mode selected.");
00151                 }
00152                 else if (mode == VELOCITY) {
00153                         ROS_INFO("Velocity control mode selected.");
00154                 }
00155                 else if (mode == ACCELERATION) {
00156                         ROS_INFO("Acceleration control mode selected.");
00157                         ROS_ERROR_NAMED("sitl_test", "Control mode: acceleration control mode not supported in PX4 current Firmware.");
00161                         return;
00162                 }
00163 
00164                 if (shape == SQUARE) {
00165                         ROS_INFO("Test option: square-shaped path...");
00166                         square_path_motion(loop_rate, mode);
00167                 }
00168                 else if (shape == CIRCLE) {
00169                         ROS_INFO("Test option: circle-shaped path...");
00170                         circle_path_motion(loop_rate, mode);
00171                 }
00172                 else if (shape == EIGHT) {
00173                         ROS_INFO("Test option: eight-shaped path...");
00174                         eight_path_motion(loop_rate, mode);
00175                 }
00176                 else if (shape == ELLIPSE) {
00177                         ROS_INFO("Test option: ellipse-shaped path...");
00178                         ellipse_path_motion(loop_rate, mode);
00179                 }
00180         }
00181 
00182 private:
00183         TestSetup test;
00184         pidcontroller::PIDController pid;
00185 
00186         double rate;
00187         bool use_pid;
00188         int num_of_tests; //TODO: find a way to use this...
00189 
00190         double linvel_p_gain;
00191         double linvel_i_gain;
00192         double linvel_d_gain;
00193         double linvel_i_max;
00194         double linvel_i_min;
00195 
00196         double yawrate_p_gain;
00197         double yawrate_i_gain;
00198         double yawrate_d_gain;
00199         double yawrate_i_max;
00200         double yawrate_i_min;
00201 
00202         control_mode mode;
00203         path_shape shape;
00204 
00205         ros::NodeHandle nh_sp;
00206         ros::Publisher local_pos_sp_pub;
00207         ros::Publisher vel_sp_pub;
00208         ros::Subscriber local_pos_sub;
00209 
00210         geometry_msgs::PoseStamped localpos, ps;
00211         geometry_msgs::TwistStamped vs;
00212 
00213         Eigen::Vector3d current;
00214 
00215         std::array<double, 100> threshold;
00216 
00217         /* -*- helper functions -*- */
00218 
00222         Eigen::Vector3d pos_setpoint(int tr_x, int tr_y, int tr_z){
00224                 return Eigen::Vector3d(tr_x * 2.0f, tr_y * 2.0f, tr_z * 1.0f);  // meters
00225         }
00226 
00230         Eigen::Vector3d circle_shape(int angle){
00232                 double r = 5.0f;        // 5 meters radius
00233 
00234                 return Eigen::Vector3d(r * cos(angles::from_degrees(angle)),
00235                                 r * sin(angles::from_degrees(angle)),
00236                                 1.0f);
00237         }
00238 
00242         Eigen::Vector3d eight_shape(int angle){
00244                 double a = 5.0f;        // vertical tangent with 5 meters size
00245 
00246                 return Eigen::Vector3d(a * cos(angles::from_degrees(angle)),
00247                                 a * sin(angles::from_degrees(angle)) * cos(angles::from_degrees(angle)),
00248                                 1.0f);
00249         }
00250 
00254         Eigen::Vector3d ellipse_shape(int angle){
00256                 double a = 5.0f;        // major axis
00257                 double b = 2.0f;        // minor axis
00258 
00259                 // rotation around y-axis
00260                 return Eigen::Vector3d(a * cos(angles::from_degrees(angle)),
00261                                 0.0f,
00262                                 2.5f + b * sin(angles::from_degrees(angle)));
00263         }
00264 
00268         void square_path_motion(ros::Rate loop_rate, control_mode mode){
00269                 uint8_t pos_target = 1;
00270 
00271                 ROS_INFO("Testing...");
00272 
00273                 while (ros::ok()) {
00274                         wait_and_move(ps);
00275 
00276                         // motion routine
00277                         switch (pos_target) {
00278                         case 1:
00279                                 tf::pointEigenToMsg(pos_setpoint(1, 1, 1), ps.pose.position);
00280                                 break;
00281                         case 2:
00282                                 tf::pointEigenToMsg(pos_setpoint(-1, 1, 1), ps.pose.position);
00283                                 break;
00284                         case 3:
00285                                 tf::pointEigenToMsg(pos_setpoint(-1, -1, 1), ps.pose.position);
00286                                 break;
00287                         case 4:
00288                                 tf::pointEigenToMsg(pos_setpoint(1, -1, 1), ps.pose.position);
00289                                 break;
00290                         case 5:
00291                                 tf::pointEigenToMsg(pos_setpoint(1, 1, 1), ps.pose.position);
00292                                 break;
00293                         default:
00294                                 break;
00295                         }
00296 
00297                         if (pos_target == 6) {
00298                                 ROS_INFO("Test complete!");
00299                                 ros::shutdown();
00300                         }
00301                         else
00302                                 ++pos_target;
00303 
00304                         loop_rate.sleep();
00305                         ros::spinOnce();
00306                 }
00307         }
00308 
00312         void circle_path_motion(ros::Rate loop_rate, control_mode mode){
00313                 ROS_INFO("Testing...");
00314                 ros::Time last_time = ros::Time::now();
00315 
00316                 while (ros::ok()) {
00317                         tf::pointMsgToEigen(localpos.pose.position, current);
00318 
00319                         // starting point
00320                         if (mode == POSITION) {
00321                                 tf::pointEigenToMsg(Eigen::Vector3d(5.0f, 0.0f, 1.0f), ps.pose.position);
00322                                 local_pos_sp_pub.publish(ps);
00323                         }
00324                         else if (mode == VELOCITY) {
00325                                 if (use_pid)
00326                                         tf::vectorEigenToMsg(pid.compute_linvel_effort(
00327                                                                 Eigen::Vector3d(5.0f, 0.0f, 1.0f), current, last_time), vs.twist.linear);
00328                                 else
00329                                         tf::vectorEigenToMsg(Eigen::Vector3d(5.0f - current.x(), -current.y(), 1.0f - current.z()), vs.twist.linear);
00330                                 vel_sp_pub.publish(vs);
00331                         }
00332                         else if (mode == ACCELERATION) {
00333                                 // TODO
00334                                 return;
00335                         }
00336 
00337                         wait_and_move(ps);
00338 
00339                         // motion routine
00340                         for (int theta = 0; theta <= 360; theta++) {
00341                                 tf::pointMsgToEigen(localpos.pose.position, current);
00342 
00343                                 if (mode == POSITION) {
00344                                         tf::pointEigenToMsg(circle_shape(theta), ps.pose.position);
00345                                         local_pos_sp_pub.publish(ps);
00346                                 }
00347                                 else if (mode == VELOCITY) {
00348                                         if (use_pid)
00349                                                 tf::vectorEigenToMsg(pid.compute_linvel_effort(circle_shape(theta), current, last_time), vs.twist.linear);
00350                                         else
00351                                                 tf::vectorEigenToMsg(circle_shape(theta) - current, vs.twist.linear);
00352                                         vel_sp_pub.publish(vs);
00353                                 }
00354                                 else if (mode == ACCELERATION) {
00355                                         // TODO
00356                                         return;
00357                                 }
00358                                 if (theta == 360) {
00359                                         ROS_INFO("Test complete!");
00360                                         ros::shutdown();
00361                                 }
00362                                 last_time = ros::Time::now();
00363                                 loop_rate.sleep();
00364                                 ros::spinOnce();
00365                         }
00366                 }
00367         }
00368 
00372         void eight_path_motion(ros::Rate loop_rate, control_mode mode){
00373                 ROS_INFO("Testing...");
00374                 ros::Time last_time = ros::Time::now();
00375 
00376                 while (ros::ok()) {
00377                         tf::pointMsgToEigen(localpos.pose.position, current);
00378 
00379                         // starting point
00380                         if (mode == POSITION) {
00381                                 tf::pointEigenToMsg(Eigen::Vector3d(0.0f, 0.0f, 1.0f), ps.pose.position);
00382                                 local_pos_sp_pub.publish(ps);
00383                         }
00384                         else if (mode == VELOCITY) {
00385                                 if (use_pid)
00386                                         tf::vectorEigenToMsg(pid.compute_linvel_effort(
00387                                                                 Eigen::Vector3d(0.0f, 0.0f, 1.0f), current, last_time), vs.twist.linear);
00388                                 else
00389                                         tf::vectorEigenToMsg(Eigen::Vector3d(-current.x(), -current.y(), 1.0f - current.z()), vs.twist.linear);
00390                                 vel_sp_pub.publish(vs);
00391                         }
00392                         else if (mode == ACCELERATION) {
00393                                 // TODO
00394                                 return;
00395                         }
00396 
00397                         wait_and_move(ps);
00398 
00399                         // motion routine
00400                         for (int theta = -180; theta <= 180; theta++) {
00401                                 tf::pointMsgToEigen(localpos.pose.position, current);
00402 
00403                                 if (mode == POSITION) {
00404                                         tf::pointEigenToMsg(eight_shape(theta), ps.pose.position);
00405                                         local_pos_sp_pub.publish(ps);
00406                                 }
00407                                 else if (mode == VELOCITY) {
00408                                         if (use_pid)
00409                                                 tf::vectorEigenToMsg(pid.compute_linvel_effort(eight_shape(theta), current, last_time), vs.twist.linear);
00410                                         else
00411                                                 tf::vectorEigenToMsg(eight_shape(theta) - current, vs.twist.linear);
00412                                         vel_sp_pub.publish(vs);
00413                                 }
00414                                 else if (mode == ACCELERATION) {
00415                                         // TODO
00416                                         return;
00417                                 }
00418                                 if (theta == 180) {
00419                                         ROS_INFO("Test complete!");
00420                                         ros::shutdown();
00421                                 }
00422                                 last_time = ros::Time::now();
00423                                 loop_rate.sleep();
00424                                 ros::spinOnce();
00425                         }
00426                 }
00427         }
00428 
00432         void ellipse_path_motion(ros::Rate loop_rate, control_mode mode){
00433                 ROS_INFO("Testing...");
00434                 ros::Time last_time = ros::Time::now();
00435 
00436                 while (ros::ok()) {
00437                         tf::pointMsgToEigen(localpos.pose.position, current);
00438 
00439                         // starting point
00440                         if (mode == POSITION) {
00441                                 tf::pointEigenToMsg(Eigen::Vector3d(0.0f, 0.0f, 2.5f), ps.pose.position);
00442                                 local_pos_sp_pub.publish(ps);
00443                         }
00444                         else if (mode == VELOCITY) {
00445                                 if (use_pid)
00446                                         tf::vectorEigenToMsg(pid.compute_linvel_effort(
00447                                                                 Eigen::Vector3d(0.0f, 0.0f, 2.5f), current, last_time), vs.twist.linear);
00448                                 else
00449                                         tf::vectorEigenToMsg(Eigen::Vector3d(-current.x(), -current.y(), 2.5f - current.z()), vs.twist.linear);
00450                                 vel_sp_pub.publish(vs);
00451                         }
00452                         else if (mode == ACCELERATION) {
00453                                 // TODO
00454                                 return;
00455                         }
00456 
00457                         wait_and_move(ps);
00458 
00459                         // motion routine
00460                         for (int theta = 0; theta <= 360; theta++) {
00461                                 tf::pointMsgToEigen(localpos.pose.position, current);
00462 
00463                                 if (mode == POSITION) {
00464                                         tf::pointEigenToMsg(ellipse_shape(theta), ps.pose.position);
00465                                         local_pos_sp_pub.publish(ps);
00466                                 }
00467                                 else if (mode == VELOCITY) {
00468                                         if (use_pid)
00469                                                 tf::vectorEigenToMsg(pid.compute_linvel_effort(ellipse_shape(theta), current, last_time), vs.twist.linear);
00470                                         else
00471                                                 tf::vectorEigenToMsg(ellipse_shape(theta) - current, vs.twist.linear);
00472                                         vel_sp_pub.publish(vs);
00473                                 }
00474                                 else if (mode == ACCELERATION) {
00475                                         // TODO
00476                                         return;
00477                                 }
00478                                 if (theta == 360) {
00479                                         ROS_INFO("Test complete!");
00480                                         ros::shutdown();
00481                                 }
00482                                 last_time = ros::Time::now();
00483                                 loop_rate.sleep();
00484                                 ros::spinOnce();
00485                         }
00486                 }
00487         }
00488 
00493         void wait_and_move(geometry_msgs::PoseStamped target){
00494                 ros::Rate loop_rate(rate);
00495                 ros::Time last_time = ros::Time::now();
00496                 bool stop = false;
00497 
00498                 Eigen::Vector3d dest;
00499 
00500                 double distance;
00501                 double err_th = threshold[rand() % threshold.size()];
00502 
00503                 ROS_DEBUG("Next setpoint: accepted error threshold: %1.3f", err_th);
00504 
00505                 while (ros::ok() && !stop) {
00506                         tf::pointMsgToEigen(target.pose.position, dest);
00507                         tf::pointMsgToEigen(localpos.pose.position, current);
00508 
00509                         distance = sqrt((dest - current).x() * (dest - current).x() +
00510                                         (dest - current).y() * (dest - current).y() +
00511                                         (dest - current).z() * (dest - current).z());
00512 
00513                         if (distance <= err_th)
00514                                 stop = true;
00515 
00516                         if (mode == POSITION) {
00517                                 local_pos_sp_pub.publish(target);
00518                         }
00519                         else if (mode == VELOCITY) {
00520                                 if (use_pid)
00521                                         tf::vectorEigenToMsg(pid.compute_linvel_effort(dest, current, last_time), vs.twist.linear);
00522                                 else
00523                                         tf::vectorEigenToMsg(dest - current, vs.twist.linear);
00524                                 vel_sp_pub.publish(vs);
00525                         }
00526                         else if (mode == ACCELERATION) {
00527                                 // TODO
00528                                 return;
00529                         }
00530                         last_time = ros::Time::now();
00531                         loop_rate.sleep();
00532                         ros::spinOnce();
00533                 }
00534         }
00535 
00539         std::array<double, 100> threshold_definition(){
00540                 std::random_device rd;
00541                 std::mt19937 gen(rd());
00542                 std::array<double, 100> th_values;
00543 
00544                 std::normal_distribution<double> th(0.1f,0.05f);
00545 
00546                 for (auto &value : th_values) {
00547                         value = th(gen);
00548                 }
00549                 return th_values;
00550         }
00551 
00552         /* -*- callbacks -*- */
00553 
00554         void local_pos_cb(const geometry_msgs::PoseStampedConstPtr& msg){
00555                 localpos = *msg;
00556         }
00557 };
00558 };      // namespace testsetup


test_mavros
Author(s): Nuno Marques , Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:26