00001 /****************************************************************************** 00002 Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 00003 00004 Redistribution and use in source and binary forms, with or without 00005 modification, are permitted provided that the following conditions are met: 00006 00007 * Redistributions of source code must retain the above copyright notice, this 00008 list of conditions and the following disclaimer. 00009 00010 * Redistributions in binary form must reproduce the above copyright notice, 00011 this list of conditions and the following disclaimer in the documentation 00012 and/or other materials provided with the distribution. 00013 00014 * Neither the name of the copyright holder nor the names of its 00015 contributors may be used to endorse or promote products derived from 00016 this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00028 ******************************************************************************/ 00029 00030 #include <ros/ros.h> 00031 #include <rosbag/bag.h> 00032 00033 #include <xpp_msgs/RobotStateJoint.h> 00034 #include <xpp_states/convert.h> 00035 #include <xpp_states/state.h> 00036 00037 00038 using namespace xpp; 00039 00040 int main(int argc, char *argv[]) 00041 { 00042 // creates a bag file wherever executable is run (usually ~/.ros/) 00043 rosbag::Bag bag; 00044 bag.open("quadrotor_traj.bag", rosbag::bagmode::Write); 00045 00046 // visualize the state of a drone (only has a base) 00047 State3d base; 00048 00049 // create a sequence of states for a total duration of T spaced 0.01s apart. 00050 double T = 2*M_PI; 00051 double dt = 0.01; 00052 double t = 1e-6; 00053 while (t < T) 00054 { 00055 // base and foot follow half a sine motion up and down 00056 base.lin.p_.z() = 0.7 - 0.5*sin(t); //[m] 00057 base.lin.p_.x() = 1.0/T*t; //[m] 00058 00059 double roll = 30./180*M_PI*sin(t); 00060 base.ang.q = GetQuaternionFromEulerZYX(0.0, 0.0, roll); 00061 00062 // save the state message with current time in the bag 00063 auto timestamp = ::ros::Time(t); 00064 00065 xpp_msgs::RobotStateJoint msg; 00066 msg.base = Convert::ToRos(base); 00067 bag.write("xpp/joint_quadrotor_des", timestamp, msg); 00068 00069 t += dt; 00070 } 00071 00072 std::string bag_file = bag.getFileName(); // save location before closing 00073 bag.close(); 00074 00075 // plays back the states at desired speeds. 00076 // can now also use all the tools from rosbag to pause, speed-up, inspect, 00077 // debug the trajectory. 00078 // e.g. rosbag -play -r 0.2 /path/to/bag.bag 00079 int success = system(("rosbag play " + bag_file).c_str()); 00080 00081 return 0; 00082 } 00083