netft_utils_sim.cpp
Go to the documentation of this file.
00001 
00002 /*
00003 Copyright (c) 2016, Los Alamos National Security, LLC
00004 All rights reserved.
00005 Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software.  NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE.  If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL.
00006 
00007 Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00008 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
00009 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 
00010 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 
00011 
00012 THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00013 
00014 Author: Alex von Sternberg
00015 */
00016 
00017 #include "ros/ros.h"
00018 #include "geometry_msgs/WrenchStamped.h"
00019 #include "netft_utils/StartSim.h"
00020 #include "netft_utils/StopSim.h"
00021 #include <math.h>
00022 
00027 // SIM vars
00028 enum SimType
00029 {
00030   TYPE_ZEROS    = 0,
00031   TYPE_LINEAR   = 1,
00032   TYPE_EXP      = 2,
00033   TYPE_TRIANGLE = 3
00034 };
00035 enum Dim   {DIM_X      = 0,
00036             DIM_Y      = 1,
00037             DIM_Z      = 2,
00038             DIM_RX     = 3,
00039             DIM_RY     = 4,
00040             DIM_RZ     = 5};
00041 
00042 geometry_msgs::WrenchStamped simWrench;        // Wrench containing the simulated data
00043 bool toSim = false;                            // True if we should output simulated data
00044 SimType simType;                               // Defines the simulation profile type
00045 Dim simDim;                                    // Dimension to simulate FT in
00046 double simStart;                               // Time that the simulation started
00047 double simSlope;                               // Parameter to tweak how fast force increases
00048 double maxForce = 0.0;                         // Max force to be simulated
00049 double upSlope = true;                         // True if the triangle wave is on up slope
00050 
00051 void setWrench(double x, double y, double z, double rx, double ry, double rz)
00052 {
00053   simWrench.wrench.force.x = x;
00054   simWrench.wrench.force.y = y;
00055   simWrench.wrench.force.z = z;
00056   simWrench.wrench.torque.x = rx;
00057   simWrench.wrench.torque.y = ry;
00058   simWrench.wrench.torque.z = rz;
00059 }
00060 
00061 void simData()
00062 {
00063   if(toSim)
00064   {
00065     double ft = 0.0;
00066     double deltaT = ros::Time::now().toSec() - simStart;
00067     switch (simType)
00068     {
00069       case TYPE_ZEROS:
00070         break;      
00071       case TYPE_LINEAR:
00072       {
00073         if((deltaT * simSlope) < maxForce)
00074           ft = deltaT * simSlope;
00075         else
00076           ft = maxForce;
00077       }
00078         break;
00079       case TYPE_EXP:
00080       {
00081         if((pow(deltaT,2) * simSlope) < maxForce)
00082           ft = pow(deltaT,2) * simSlope;
00083         else
00084           ft = maxForce;
00085       }
00086         break;
00087       case TYPE_TRIANGLE:
00088       {
00089         if((deltaT * simSlope) < maxForce && upSlope)
00090         {
00091           ft = deltaT * simSlope;
00092         }
00093         else if((deltaT * simSlope) >= maxForce && upSlope)
00094         {
00095           ft = maxForce;
00096           upSlope = false;
00097         }
00098         else if(2*maxForce - deltaT*simSlope > 0.0)
00099         {
00100           ft = 2*maxForce - deltaT*simSlope;
00101         }
00102         else
00103         {
00104           ft = 0.0;
00105         }
00106       }
00107         break;
00108       default:
00109         ROS_ERROR("Sim type invalid. Stopping sim.");
00110         toSim = false;
00111         break;
00112     }
00113     switch (simDim)
00114     {
00115       case DIM_X:
00116         setWrench(ft, 0.0, 0.0, 0.0, 0.0, 0.0);
00117         break;
00118       case DIM_Y:
00119         setWrench(0.0, ft, 0.0, 0.0, 0.0, 0.0);
00120         break;
00121       case DIM_Z:
00122         setWrench(0.0, 0.0, ft, 0.0, 0.0, 0.0);
00123         break;
00124       case DIM_RX:
00125         setWrench(0.0, 0.0, 0.0, ft, 0.0, 0.0);
00126         break;
00127       case DIM_RY:
00128         setWrench(0.0, 0.0, 0.0, 0.0, ft, 0.0);
00129         break;
00130       case DIM_RZ:
00131         setWrench(0.0, 0.0, 0.0, 0.0, 0.0, ft);
00132         break;
00133       default:
00134         ROS_ERROR("Dimension invalid. Stopping sim.");
00135         toSim = false;
00136         break;
00137     }
00138   }
00139   else
00140   {
00141     upSlope = true;
00142     setWrench(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
00143   }
00144 }                 
00145                   
00146 bool startSim(netft_utils::StartSim::Request &req, netft_utils::StartSim::Response &res)
00147 {  
00148   simStart = ros::Time::now().toSec();
00149   simDim = (Dim)req.simDim;
00150   simType = (SimType)req.simType;
00151   simSlope = req.simSlope;              
00152   maxForce = req.maxForce;
00153   toSim = true;
00154   return true;
00155 }  
00156 
00157 bool stopSim(netft_utils::StopSim::Request &req, netft_utils::StopSim::Response &res)
00158 { 
00159   toSim = false;                
00160   return true;    
00161 }  
00162     
00163 int main(int argc, char **argv)
00164 {                 
00165   //Node name     
00166   ros::init(argc, argv, "netft_utils_sim");
00167   
00168   //Access main ROS system
00169   ros::NodeHandle n;
00170                   
00171   //Publish on the /netft_transformed_data topic. Queue up to 100000 data points
00172   ros::Publisher netft_data_pub = n.advertise<geometry_msgs::WrenchStamped>("netft_data", 100000);
00173   
00174   //Advertise bias and threshold services
00175   ros::ServiceServer start_service = n.advertiseService("start_sim", startSim);
00176   ros::ServiceServer stop_service = n.advertiseService("stop_sim", stopSim);
00177   
00178   ros::Rate loop_rate(400);
00179 
00180   //Initialize variables
00181   upSlope = true;
00182   
00183   while ( ros::ok() )
00184   {
00185     simData();
00186     
00187     // Publish transformed dat
00188     netft_data_pub.publish( simWrench );
00189   
00190     loop_rate.sleep();          
00191     ros::spinOnce();
00192   }
00193   
00194   return 0;
00195 }


netft_utils
Author(s): Alex von Sternberg , Derek King, Andy Zelenak
autogenerated on Sat Jul 15 2017 02:45:58