Revision history [back]

Here my dummy s-function. Note, this was modified from the c-function template. I'm new to the whole s-functions and it would probably be nicer to switch to the cpp template. You will have to comment everything that has to do with telekyb::Timer or telekyb::Time. This is not necessary for the ros stuff to work.

Btw, ROS_INFO Macro does not output to MATLAB (should work in rxconsole), but cout does.

#define S_FUNCTION_NAME  test_s_fun
#define S_FUNCTION_LEVEL 2


#include "simstruc.h"


//#include <string>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>

// Telekyb Timer
#include <telekyb_base/Time.hpp>
using namespace telekyb;



/*====================*
 * S-function methods *
 *====================*/

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    /* See sfuntmpl_doc.c for more details on the macros below */

    ssSetNumSFcnParams(S, 0);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /* Return if number of expected != number of actual parameters */
        return;
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    /*
     * Set direct feedthrough flag (1=yes, 0=no).
     * A port has direct feedthrough if the input is used in either
     * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
     * See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
     */
    ssSetInputPortDirectFeedThrough(S, 0, 1);

    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, 1);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

}



#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  /* Function: mdlInitializeConditions ========================================
   * Abstract:
   *    In this function, you should initialize the continuous and discrete
   *    states for your S-function block.  The initial states are placed
   *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
   *    You can also perform any other initialization activities that your
   *    S-function may require. Note, this routine will be called at the
   *    start of simulation and if it is present in an enabled subsystem
   *    configured to reset states, it will be call when the enabled subsystem
   *    restarts execution to reset the states.
   */
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */


void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

double doubleData;

void doubleCallback(const std_msgs::Float64::ConstPtr& msg)
{
   std::cout << "Received Double value! " << msg->data << std::endl;
   doubleData = msg->data;
}

ros::Time start;

void timerCallback(const ros::TimerEvent& event)
{
    //ROS_INFO("Got ROS Timer CB!");
    std::cout << "Got ROS Timer CB! Time: " << (ros::Time::now() - start).toSec() << std::endl;
}


ros::Subscriber sub;
ros::AsyncSpinner* spinner;
ros::Timer timer;

ros::Subscriber doubleSub;
ros::Publisher doublePub;

Timer rtTimer;
Time sleepTime;


#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */
  static void mdlStart(SimStruct *S)
  {
    rtTimer.reset();
    sleepTime = Time(0.001);

    setenv("ROS_MASTER_URI", "http://localhost:11311", 1);
    doubleData = -1.0;


    int argc = 0;
    char* argv[0];
    ros::init(argc, argv, "MatLabTest", ros::init_options::NoSigintHandler);

    //std::cout << "Test" << std::endl;
    //std::cerr << "Testerr" << std::endl;

    ros::NodeHandle n;

    spinner = new ros::AsyncSpinner(2);
    spinner->start();


    start = ros::Time::now();

    doublePub = n.advertise<std_msgs::Float64>("testDoublePub", 10);

    sub = n.subscribe("chatter", 1000, chatterCallback);
    doubleSub = n.subscribe("testDoubleSub", 1000, doubleCallback);
    //timer = n.createTimer(ros::Duration(0.5), timerCallback);

  }
#endif /*  MDL_START */



/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{    
    const real_T *u = (const real_T*) ssGetInputPortSignal(S,0);

    std_msgs::Float64 msg;
    msg.data = u[0];
    doublePub.publish(msg);

    real_T       *y = (real_T*)ssGetOutputPortSignal(S,0);

   // y[0] = 2*u[0];
    y[0] = doubleData;


    Time passed = rtTimer.getElapsed();
    Time actualSleepTime = sleepTime - passed;
    actualSleepTime.sleep();
    rtTimer.reset();
    //std::cout << "Slept: " << actualSleepTime.dSecToString() << std::endl;
}



#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  /* Function: mdlUpdate ======================================================
   * Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */



#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */



/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    delete spinner;
}


/*======================================================*
 * See sfuntmpl_doc.c for the optional S-function methods *
 *======================================================*/

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif