ctrl_test.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include <string.h>
00033 #include <stdio.h>
00034 #include <math.h>
00035 
00036 #include <ros/ros.h>
00037 #include <asctec_hl_comm/mav_ctrl.h>
00038 #include <asctec_hl_comm/mav_ctrl_motors.h>
00039 
00040 void usage()
00041 {
00042   std::string text("usage: \n\n");
00043   text = "ctrl_test motors [0 | 1]\n";
00044   text += "ctrl_test ctrl [acc | vel | pos | vel_b | pos_b] x y z yaw\n";
00045   text += "position / velocity in [m] / [m/s] and yaw in [deg] / [deg/s] (-180 ... 180)\n";
00046   std::cout << text << std::endl;
00047 }
00048 
00049 int main(int argc, char ** argv)
00050 {
00051 
00052   ros::init(argc, argv, "interfacetest");
00053   ros::NodeHandle nh;
00054 
00055   ros::Publisher pub;
00056 
00057   if (argc == 1)
00058   {
00059     ROS_ERROR("Wrong number of arguments!!!");
00060     usage();
00061     return -1;
00062   }
00063 
00064   std::string command = std::string(argv[1]);
00065 
00066   if (command == "motors")
00067   {
00068     if (argc != 3)
00069     {
00070       ROS_ERROR("Wrong number of arguments!!!");
00071       usage();
00072       return -1;
00073     }
00074 
00075     asctec_hl_comm::mav_ctrl_motors::Request req;
00076     asctec_hl_comm::mav_ctrl_motors::Response res;
00077     req.startMotors = atoi(argv[2]);
00078     ros::service::call("fcu/motor_control", req, res);
00079     std::cout << "motors running: " << (int)res.motorsRunning << std::endl;
00080   }
00081   else if (command == "ctrl")
00082   {
00083     if (argc != 7)
00084     {
00085       ROS_ERROR("Wrong number of arguments!");
00086       usage();
00087       return -1;
00088     }
00089     asctec_hl_comm::mav_ctrl msg;
00090     msg.x = atof(argv[3]);
00091     msg.y = atof(argv[4]);
00092     msg.z = atof(argv[5]);
00093     msg.yaw = atof(argv[6]) * M_PI / 180.0;
00094     msg.v_max_xy = -1; // use max velocity from config
00095     msg.v_max_z = -1;
00096 
00097     std::string type(argv[2]);
00098     if (type == "acc")
00099       msg.type = asctec_hl_comm::mav_ctrl::acceleration;
00100     else if (type == "vel")
00101       msg.type = asctec_hl_comm::mav_ctrl::velocity;
00102     else if (type == "pos")
00103       msg.type = asctec_hl_comm::mav_ctrl::position;
00104     else if (type == "vel_b")
00105       msg.type = asctec_hl_comm::mav_ctrl::velocity_body;
00106     else if (type == "pos_b")
00107       msg.type = asctec_hl_comm::mav_ctrl::position_body;
00108     else
00109     {
00110       ROS_ERROR("Command type not recognized");
00111       usage();
00112       return -1;
00113     }
00114 
00115     pub = nh.advertise<asctec_hl_comm::mav_ctrl> ("fcu/control", 1);
00116     ros::Rate r(15); // ~15 Hz
00117 
00118     for (int i = 0; i < 15; i++)
00119     {
00120       pub.publish(msg);
00121       if (!ros::ok())
00122         return 0;
00123       r.sleep();
00124     }
00125 
00126     // reset
00127     if (type != "pos" && type != "pos_b")
00128     {
00129       msg.x = 0;
00130       msg.y = 0;
00131       msg.z = 0;
00132       msg.yaw = 0;
00133     }
00134 
00135     for(int i=0; i<5; i++){
00136       pub.publish(msg);
00137       r.sleep();
00138     }
00139 
00140     ros::spinOnce();
00141   }
00142 
00143   return 0;
00144 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52