自学教程：C++ ssGetPWork函数代码示例

static void mdlStart(SimStruct *S) {  //      ssGetPWork(S)[0] = (void *) new counter; // store new C++ object in the pointers vector  ssGetPWork(S)[0] = (void *) new yarp::os::Network();  ssGetPWork(S)[1] = (void *) new yarp::os::BufferedPort<yarp::os::Bottle>();  yarp::os::BufferedPort<yarp::os::Bottle> *yPortIn = (yarp::os::BufferedPort<yarp::os::Bottle> *) ssGetPWork(S)[1]; #define LENGTH 100    char_T buf01[LENGTH];  mxGetString(ssGetSFcnParam(S, 0), buf01, LENGTH);  char_T buf02[LENGTH];  mxGetString(ssGetSFcnParam(S, 1), buf02, LENGTH);  std::string strPortNameSender(buf01);  std::string strPortNameReceiver(buf02);  mexPrintf("opening port: %s/n", strPortNameReceiver.c_str());  yPortIn->open(strPortNameReceiver.c_str());   Sleep(1000);  yarp::os::Network *yNetwork = (yarp::os::Network *) ssGetPWork(S)[0];  if(!yNetwork->connect(strPortNameSender.c_str(), strPortNameReceiver.c_str())){    std::string strMessage = "error connecting ports /"" + strPortNameSender + "/" to /"" + strPortNameReceiver + "/"";    mexWarnMsgTxt(strMessage.c_str());  }       }                                            

static void mdlTerminate(SimStruct *S){    // IF YOU FORGET TO DESTROY OBJECTS OR DEALLOCATE MEMORY, MATLAB WILL CRASH.    // Retrieve and destroy C++ object     BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);     toPort->close();     if(ssGetPWork(S) != NULL){         ssSetPWorkValue(S,0,NULL);     }    Network::fini();    fprintf(stderr,"Everything was closed correctly/n");}

static void mdlTerminate(SimStruct *S){	USING_NAMESPACE_QPOASES	int i;	/* reset global message handler */	MessageHandling_reset( qpOASES_getGlobalMessageHandler() );	for ( i=1; i<5; ++i )	{		if ( ssGetPWork(S)[i] != 0 )			free( ssGetPWork(S)[i] );	}}

static void mdlTerminate(SimStruct *S){    PCONFIG_DATA config;    /* Retrieve and destroy C object */    if( ssGetNumPWork(S) > 0 )    {        config = ssGetPWork(S)[0];        if( config )        {            free(config);            ssGetPWork(S)[0] = 0;        }    }}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       // get Objects    void** vecPWork = ssGetPWork(S);    int_T nRobots = *ssGetIWork(S);    // get Pointers    // accessing inputs    const real_T *position = (const real_T*) ssGetInputPortSignal(S,0);   // const real_T *orientation = (const real_T*) ssGetInputPortSignal(S,1);        geometry_msgs::PointStamped msg;    for (unsigned int i = 0; i < nRobots; ++i) {        GenericPublisher<geometry_msgs::PointStamped>* pub                 = (GenericPublisher<geometry_msgs::PointStamped>*)vecPWork[i];        // define send Time.        msg.header.stamp = ros::Time::now();        msg.point.x = position[i*3 + 0];        msg.point.y = position[i*3 + 1];        msg.point.z = position[i*3 + 2];        pub->publish(msg);	}}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       // get Objects    void** vecPWork = ssGetPWork(S);    int_T nRobots = *ssGetIWork(S);    // Preparing Outputs    real_T *position = (real_T*)ssGetOutputPortSignal(S,0);    real_T *quaternion = (real_T*)ssGetOutputPortSignal(S,1);    real_T *time = (real_T*)ssGetOutputPortSignal(S,2);        for (unsigned int i = 0; i < nRobots; ++i) {    	GenericSubscriber<geometry_msgs::PoseStamped>* sub =    	        (GenericSubscriber<geometry_msgs::PoseStamped>*)vecPWork[i];    	geometry_msgs::PoseStampedConstPtr lastMsg = sub->getLastMsg();        if (lastMsg){ // copy only if a message was actually received            position[i*3 + 0] = lastMsg->pose.position.x;            position[i*3 + 1] = lastMsg->pose.position.y;            position[i*3 + 2] = lastMsg->pose.position.z;            // w, x, y, z            quaternion[i*4 + 0] = lastMsg->pose.orientation.w;            quaternion[i*4 + 1] = lastMsg->pose.orientation.x;            quaternion[i*4 + 2] = lastMsg->pose.orientation.y;            quaternion[i*4 + 3] = lastMsg->pose.orientation.z;            time[i + 0] = lastMsg->header.stamp.toSec();        }    }}

/* 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){	O22SnapIoMemMap *Brain;	Brain = (O22SnapIoMemMap *) ssGetPWork(S)[0];	// Variador de Frecuencia	Brain->SetAnaPtValue(16,4.0);	// Valvula Solenoide	Brain->SetAnaPtValue(13,4.0);	// Valvula Motorizada	Brain->SetAnaPtValue(12,4.0);	// Calefactor 1	Brain->SetDigPtState(20,0);	// Calefactor 2	Brain->SetDigPtState(22,0);	// Calefactor 3	Brain->SetDigPtState(21,0);	// Agitador	Brain->SetDigPtState(23,0);	// Valv. Solenoide 1	Brain->SetDigPtState(24,0);	// Valv. Solenoide 2	Brain->SetDigPtState(25,0);	// Luces del Laboratorio	Brain->SetDigPtState(26,0);	Brain->Close();	delete Brain;}

static void mdlStart(SimStruct *S){    SFUNPRINTF("Starting Instance of %s./n", TOSTRING(S_FUNCTION_NAME));    // init ROS if not yet done.    initROS(S);    void** vecPWork = ssGetPWork(S);    ros::AsyncSpinner* spinner = new ros::AsyncSpinner(1); // Spinner    spinner->start();    vecPWork[0] = spinner;    ros::NodeHandle nodeHandle(ros::this_node::getName());    // get String    size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;    char* topic = (char*)mxMalloc(buflen);    size_t status = mxGetString((ssGetSFcnParam(S, 1)), topic, buflen);    //SFUNPRINTF("The string being passed as a Paramater is - %s/n ", topic);    GenericSubscriber<sensor_msgs::Joy>* sub = new GenericSubscriber<sensor_msgs::Joy>(nodeHandle, std::string(topic), 100);    vecPWork[1] = sub;    //*sub = n.subscribe(std::string(topic), 1, msgCallback);    // free char array    mxFree(topic);}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){    // get Objects    void** vecPWork = ssGetPWork(S);    GenericSubscriber<sensor_msgs::Joy>* sub = (GenericSubscriber<sensor_msgs::Joy>*)vecPWork[1];    // Preparing Outputs    real_T* axes = (real_T*)ssGetOutputPortSignal(S,0);    int32_T* buttons = (int32_T*)ssGetOutputPortSignal(S,1);    real_T *time = (real_T*)ssGetOutputPortSignal(S,2);    sensor_msgs::JoyConstPtr lastMsg = sub->getLastMsg();    if (lastMsg) { // copy only if a message was actually received        // axes        for (unsigned int i=0; i < lastMsg->axes.size() && i < CONSTANT_AXES_SIZE; i++) {            axes[i] = lastMsg->axes[i];        }        // do we have to fill in the rest?        // buttons        for (unsigned int i=0; i < lastMsg->buttons.size() && i < CONSTANT_BUTTONS_SIZE; i++) {            buttons[i] = lastMsg->buttons[i];        }        time[0] = lastMsg->header.stamp.toSec();    }}

// Function: mdlOutputs =======================================================// Abstract://   In this function, you compute the outputs of your S-function//   block.static void mdlOutputs(SimStruct *S, int_T tid){    BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);        Vector *v = toPort->read(false); // Read from the port.  Waits until data arrives.    if (v!=NULL)    {        for (int i = 0; i < SIZE_READING_PORT; i++)        {            real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);            int_T widthPort = ssGetOutputPortWidth(S,i);            if (widthPort == 1)            {                for(int_T j=0; j<widthPort; j++)                    if (i < (v->length())){                        pY[j] = v->data()[i];			//cout<<v->data()[i]<<" ";		    }                    else                        pY[j] = 0;            }            else                cout << "ERROR: something wrong with port dimensions /n";        }    }}

  /* 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){    Context* context;        context = (Context*)malloc(sizeof(Context));    if (context == NULL)        setErrorStatus(S, "mdlStart: memeory allocation error");    memset(context, 0, sizeof(context));        //save context in SS    ssGetPWork(S)[0] = context;            context->enableSpacemouse = (int)(*((double*)mxGetData(ssGetSFcnParam(S, 0))));    #  if defined(RT)        if (context->enableSpacemouse) {        context->tmcd2005.deviceBall.handle= tmcdDeviceBallOpen("/dev/tmcd2005/ball/ser1");        if ( !context->tmcd2005.deviceBall.handle )            setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");    }    else {        context->tmcd2005.device.handle= tmcdDeviceOpen("/dev/tmcd2005/noball/ser1");        if ( !context->tmcd2005.device.handle )            setErrorStatus(S, "mdlStart: Could not open TMCD2005 @ /dev/tmcd2005/ser1");    }#  endif            }

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       // get Objects    void** vecPWork = ssGetPWork(S);    int_T nRobots = *ssGetIWork(S);    // Preparing Outputs    real_T *force = (real_T*)ssGetOutputPortSignal(S,0);    real_T *time = (real_T*)ssGetOutputPortSignal(S,1);        telekyb_msgs::TKMotorCommandsConstPtr lastMsg;    for (unsigned int i = 0; i < nRobots; ++i) {        GenericSubscriber<telekyb_msgs::TKMotorCommands>* sub                 = (GenericSubscriber<telekyb_msgs::TKMotorCommands>*)vecPWork[i];                 lastMsg = sub->getLastMsg();        if (lastMsg){ // copy only if a message was actually received                if (lastMsg->force.size() == 4){                for (unsigned int j=0; j<4; j++){                    force[i*4 + j] = lastMsg->force[j];                }            } else {                SFUNPRINTF("Wrong message size (%d)./n", (int)lastMsg->force.size());            }                time[i + 0] = lastMsg->header.stamp.toSec();        }    }}

static void mdlStart(SimStruct *S){#ifndef MATLAB_MEX_FILE	char name[7];	MBX *mbx;	int i;#ifdef KRTAI	i = mbx_rt_get_adr(TargetLedMbxID, MAX_RTAI_LEDS);	sprintf(name, "%s%d", TargetLedMbxID, i);	rtaiLed[i] = S;	mbx = mbx_rt_mbx_init(name, (MBX_RTAI_LED_SIZE/(sizeof(unsigned int))*(sizeof(unsigned int))));#else	for (i = 0; i < MAX_RTAI_LEDS; i++) {		sprintf(name, "%s%d", TargetLedMbxID, i);		if (!rt_get_adr(nam2num(name))) break;	}	rtaiLed[i] = S;	if (!(mbx = (MBX *)rt_mbx_init(nam2num(name), (MBX_RTAI_LED_SIZE/(sizeof(unsigned int))*(sizeof(unsigned int)))))) {		printf("Cannot init mailbox/n");		exit(1);	}#endif	ssGetPWork(S)[0]= (void *)mbx;#endif}

static void mdlStart(SimStruct *S){#ifndef MATLAB_MEX_FILE	MBX *mbx;	char name[7];	int i;	int_T *dim = ssGetInputPortDimensions(S,0);#ifdef KRTAI	i = mbx_rt_get_adr(TargetLogMbxID,MAX_RTAI_LOGS);	sprintf(name, "%s%d", TargetLogMbxID, i);	rtaiLog[i] = S;	mbx = mbx_rt_mbx_init(name, (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float))));#else	for (i = 0; i < MAX_RTAI_LOGS; i++) {		sprintf(name, "%s%d", TargetLogMbxID, i);		if (!rt_get_adr(nam2num(name))) break;	}	rtaiLog[i] = S;	if (!(mbx = (MBX *)rt_mbx_init(nam2num(name), (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float)))))) {		printf("Cannot init mailbox/n");		exit(1);	}#endif	ssGetPWork(S)[0] = (void *)mbx;#endif}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       // get Objects    void** vecPWork = ssGetPWork(S);    int_T nRobots = *ssGetIWork(S);    // get Pointers    // accessing inputs    const real_T *linVel = (const real_T*) ssGetInputPortSignal(S,0);    const real_T *angVel = (const real_T*) ssGetInputPortSignal(S,1);        geometry_msgs::WrenchStamped msg;    for (unsigned int i = 0; i < nRobots; ++i) {        GenericPublisher<geometry_msgs::WrenchStamped>* pub                = (GenericPublisher<geometry_msgs::WrenchStamped>*)vecPWork[i];        // define send Time.        msg.header.stamp = ros::Time::now();        msg.wrench.force.x = linVel[i*3 + 0];        msg.wrench.force.y = linVel[i*3 + 1];        msg.wrench.force.z = linVel[i*3 + 2];        // w, x, y, z        msg.wrench.torque.x = angVel[i*3 + 0];        msg.wrench.torque.y = angVel[i*3 + 1];        msg.wrench.torque.z = angVel[i*3 + 2];        pub->publish(msg);	}}

static void mdlStart(SimStruct *S){    char *bufSimEng;  // SIMULATION_ENGINE    int bufSimEngLen;    // Build the list of parameters that configure the tres::Network    std::vector<std::string> ns_params = readMaskAndBuildConfVector(S);    // Get the type of the adapter, i.e., the concrete implementation of tres::Network    bufSimEngLen = mxGetN( ssGetSFcnParam(S,SIMULATION_ENGINE) )+1;    bufSimEng = new char[bufSimEngLen];    mxGetString(ssGetSFcnParam(S,SIMULATION_ENGINE), bufSimEng, bufSimEngLen);    std::string engine(bufSimEng);    delete bufSimEng;    // Instantiate the concrete representation of tres::Network    std::unique_ptr<tres::Network> ns = Factory<tres::Network>::instance()                                                    .create(engine, ns_params);    // Save the C++ object to the pointers vector    tres::Network *_ns = ns.release();    ssGetPWork(S)[0] = _ns;    // Set the `New pending activations available' flag to false    boolean_T *pendingActivsAvail = (boolean_T*) ssGetDWork(S,0);    pendingActivsAvail[0] = false;    // Get the time resolution (actually, its floating point representation)    std::vector<std::string>::iterator it = ns_params.end()-1;    double time_resolution = atof((*it).c_str());    // Save the time resolution to the real vector workspace    ssGetRWork(S)[0] = time_resolution;}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. */static void mdlOutputs(SimStruct *S, int_T tid){       // get Objects    void** vecPWork = ssGetPWork(S);    int_T nRobots = *ssGetIWork(S);    // Preparing Outputs    real_T *rpy = (real_T*)ssGetOutputPortSignal(S,0);    real_T *thrust = (real_T*)ssGetOutputPortSignal(S,1);    real_T *estim_mass = (real_T*)ssGetOutputPortSignal(S,2);    real_T *time = (real_T*)ssGetOutputPortSignal(S,3);        telekyb_msgs::TKCommandsConstPtr lastMsg;    for (unsigned int i = 0; i < nRobots; ++i) {        GenericSubscriber<telekyb_msgs::TKCommands>* sub                 = (GenericSubscriber<telekyb_msgs::TKCommands>*)vecPWork[i];                 lastMsg = sub->getLastMsg();        if (lastMsg){ // copy only if a message was actually received                rpy[i*3 + 0] = lastMsg->roll;            rpy[i*3 + 1] = lastMsg->pitch;            rpy[i*3 + 2] = lastMsg->yaw;            thrust[i + 0] = lastMsg->thrust;            estim_mass[i + 0] = lastMsg->mass;            time[i + 0] = lastMsg->header.stamp.toSec();        }        }}

static void mdlStart(SimStruct *S){      // Notify    SFUNPRINTF("Starting Instance of %s./n", TOSTRING(S_FUNCTION_NAME));    // init ROS if not yet done.    initROS(S);    ros::NodeHandle nodeHandle(ros::this_node::getName());    void** vecPWork = ssGetPWork(S);    ros::Time::init();    ros::Time* firstExecTime = new ros::Time(ros::Time::now());    vecPWork[0] = firstExecTime;    ros::Time* lastExecTime = new ros::Time(*firstExecTime);    vecPWork[1] = lastExecTime;    real_T* vecRWork = ssGetRWork(S);    vecRWork[0] = mxGetScalar(ssGetSFcnParam(S, 0)); // Tsim    vecRWork[1] = 0.0; // simulationTime     int_T* vecIWork = ssGetIWork(S);    vecIWork[0] = 1; // initialize step counter  }

// 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){    // Get the C++ object back from the pointers vector    tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]);    // Call its destructor    delete ns;}

static void mdlTerminate(SimStruct *S){    USING_NAMESPACE_QPOASES    int i;    /* reset global message handler */    getGlobalMessageHandler( )->reset( );    if ( ssGetPWork(S)[0] != 0 )        delete ssGetPWork(S)[0];    for ( i=1; i<5; ++i )    {        if ( ssGetPWork(S)[i] != 0 )            free( ssGetPWork(S)[i] );    }}

static void mdlZeroCrossings(SimStruct *S){    // Get the C++ object back from the pointers vector    tres::Network *ns = static_cast<tres::Network *>(ssGetPWork(S)[0]);    // Get the time resolution back from the real vector workspace    double time_resolution = ssGetRWork(S)[0];    ssGetNonsampledZCs(S)[0] = ns->getTimeOfNextEvent()/(time_resolution) - ssGetT(S);}

static void mdlOutputs(SimStruct *S, int_T tid){    int j;    ocp_nlp_dims *nlp_dims = (ocp_nlp_dims *) ssGetPWork(S)[0];    ocp_nlp_in *nlp_in = (ocp_nlp_in *) ssGetPWork(S)[1];    ocp_nlp_out *nlp_out = (ocp_nlp_out *) ssGetPWork(S)[2];    ocp_nlp_solver *nlp_solver = (ocp_nlp_solver *) ssGetPWork(S)[4];    ocp_nlp_constraints_bgh_model **constraints = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints;	ocp_nlp_constraints_bgh_dims **constraints_dims = (ocp_nlp_constraints_bgh_dims **) nlp_dims->constraints;    ocp_nlp_cost_nls_model **cost = (ocp_nlp_cost_nls_model **) nlp_in->cost;    const double *x0 = ssGetInputPortRealSignal(S, 0);    const double *reference = ssGetInputPortRealSignal(S, 1);    // bounds    double lb_0[] = {-10000, -10000, 50, 50, 1.14275, 1.53787};    double ub_0[] = {+10000, +10000, 50, 50, 1.14275, 1.53787};    for (j = 0; j < NUM_STATES; ++j) {        lb_0[NUM_CONTROLS+j] = x0[j];        ub_0[NUM_CONTROLS+j] = x0[j];    }     ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "lb", lb_0);    ocp_nlp_constraints_bounds_set(config, dims, nlp_in, 0, "ub", ub_0);        for (j = 0; j <= NUM_STAGES; ++j)        BLASFEO_DVECEL(&cost[j]->y_ref, 0) = *reference;    int status = ocp_nlp_solve(nlp_solver, nlp_in, nlp_out);        double *u0_opt = ssGetOutputPortRealSignal(S, 0);    double *x1 = ssGetOutputPortRealSignal(S, 1);    double *status_out = ssGetOutputPortRealSignal(S, 2);    double *comp_time = ssGetOutputPortRealSignal(S, 3);    blasfeo_unpack_dvec(NUM_CONTROLS, &nlp_out->ux[0], 0, u0_opt);    blasfeo_unpack_dvec(NUM_STATES, &nlp_out->ux[1], NUM_CONTROLS, x1);    *status_out = (double) status;    *comp_time = nlp_out->total_time;}

static void mdlTerminate(SimStruct *S){#ifndef MATLAB_MEX_FILE	MBX *mbx = (MBX *)ssGetPWork(S)[0];#ifdef KRTAI	mbx_rt_mbx_delete(mbx);#else	rt_mbx_delete(mbx);#endif#endif}

  /* Function: mdlStart =======================================================   * Abstract:   *  allocate memory for work vectors */static void mdlStart(SimStruct *S){	ssGetPWork(S)[0] = calloc(NSTATES(S)*NSTATES(S), sizeof(double)); /* Mn */	ssGetPWork(S)[1] = calloc(NSTATES(S), sizeof(double)); /* qn */	ssGetPWork(S)[2] = calloc(NSTATES(S), sizeof(double)); /* r */	ssGetPWork(S)[3] = calloc(NSTATES(S), sizeof(double)); /* c */	ssGetPWork(S)[4] = calloc(NSTATES(S), sizeof(double)); /* x */	ssGetPWork(S)[5] = Matrix_Init(NSTATES(S),NSTATES(S)+2,"A"); /* pA */	ssGetPWork(S)[6] = Basis_Init(NSTATES(S));  /* pB */}

static void mdlStart(SimStruct *S){    // ######### YARP INITIALIZATION STUFF ##################    Network::init();    fprintf(stderr,"YARP NETWORK INITIALIZED/n");    if (!Network::checkNetwork() || !Network::initialized()){        ssSetErrorStatus(S,"YARP server wasn't found active!! /n");        return;    }    else        cout<<"YARP is running!!/n"<<endl;    int_T buflen, status;    char *String;    buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1;    String = static_cast<char*>(mxMalloc(buflen));    status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen);    if (status) {        ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! /n");        return;    }    //string port_name = String;        char *port_name = String;				//FROM port name    buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1;    String = static_cast<char*>(mxMalloc(buflen));    status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen);    if (status) {        ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! /n");        return;    }    char *toPort_name = String;        // ######## CHECKING INPUT PARAMETERS ############    BufferedPort<Vector> *toPort;    toPort = new BufferedPort<Vector>;    toPort->open(toPort_name);    ConstString toPortName = toPort->getName();        cout<<"[From] Port name will be: "<<port_name<<endl;    cout<<"[To] Port name will be:   "<<toPort->getName()<<endl;    ssGetPWork(S)[0] = toPort;        Network::connect(port_name,toPortName);}

/* 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){    // get Objects    void** vecPWork = ssGetPWork(S);    ros::AsyncSpinner* spinner = (ros::AsyncSpinner*)vecPWork[0];    GenericSubscriber<sensor_msgs::Joy>* sub = (GenericSubscriber<sensor_msgs::Joy>*)vecPWork[1];    // cleanup    delete spinner;    delete sub;    SFUNPRINTF("Terminating Instance of %s./n", TOSTRING(S_FUNCTION_NAME));}

/* Function: mdlOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. Generally outputs are placed in the output vector, ssGetY(S). */static void mdlOutputs(SimStruct *S, int_T tid){    #if defined(RT)    TmcdDeviceBallActual* actualBall = 0;    TmcdDeviceActual* actual = 0;# endif    Context* context = (Context*)(ssGetPWork(S)[0]);    #if defined(RT)    if (context->enableSpacemouse)     {                   actualBall = tmcdDeviceBallGetActual(context->tmcd2005.deviceBall.handle);                if ( !actualBall )            ssSetErrorStatus(S,"could not get state from TMCD2005");        // copy events to simulink outputs       *((boolean_T*)ssGetOutputPortSignal(S, 0)) = actualBall->on;       ((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actualBall->keys[0];       ((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actualBall->keys[1];       ((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actualBall->keys[2];          ((real_T*)ssGetOutputPortSignal(S, 2))[0] = actualBall->ballData[0];       ((real_T*)ssGetOutputPortSignal(S, 2))[1] = actualBall->ballData[1];       ((real_T*)ssGetOutputPortSignal(S, 2))[2] = actualBall->ballData[2];       ((real_T*)ssGetOutputPortSignal(S, 2))[3] = actualBall->ballData[3];       ((real_T*)ssGetOutputPortSignal(S, 2))[4] = actualBall->ballData[4];       ((real_T*)ssGetOutputPortSignal(S, 2))[5] = actualBall->ballData[5];       *((boolean_T*)ssGetOutputPortSignal(S, 3)) = actualBall->isBallActive;       }    else         {               actual = tmcdDeviceGetActual(context->tmcd2005.device.handle);       if ( !actual )            ssSetErrorStatus(S,"could not get state from TMCD2005");        // copy events to simulink outputs       *((boolean_T*)ssGetOutputPortSignal(S, 0)) = actual->on;       ((boolean_T*)ssGetOutputPortSignal(S, 1))[0] = actual->keys[0];       ((boolean_T*)ssGetOutputPortSignal(S, 1))[1] = actual->keys[1];       ((boolean_T*)ssGetOutputPortSignal(S, 1))[2] = actual->keys[2];       ((boolean_T*)ssGetOutputPortSignal(S, 1))[3] = actual->keys[3];    }   #endif}

/* 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) {    // counter *c = (counter *) ssGetPWork(S)[0];   // delete c;  yarp::os::BufferedPort<yarp::os::Bottle> *yPortIn = (yarp::os::BufferedPort<yarp::os::Bottle> *) ssGetPWork(S)[1];   yPortIn->close();      yarp::os::Network *yNetwork = (yarp::os::Network *) ssGetPWork(S)[0];   yNetwork->fini();  delete yNetwork;  delete yPortIn;    }                                              

/* Function: SimInitializeConditions ======================================== * 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. */void SimInitializeConditions(SimStruct *S){    Sim *sim = (Sim *) ssGetPWork(S)[0];   		// retrieve C++ object from the pointers vector and use member functions of the object    try    {        sim->init_cond();    }    catch(Exception e)    {        e.kill(S);        return;    }}

/* Function: SimOutputs ======================================================= * Abstract: *    In this function, you compute the outputs of your S-function *    block. Generally outputs are placed in the output vector, ssGetY(S). */void SimOutputs(SimStruct *S, int_T tid){    Sim *sim = (Sim *) ssGetPWork(S)[0];   		// retrieve C++ object from the pointers vector and use member functions of the object    try    {        sim->output();    }    catch(Exception e)    {        e.kill(S);        return;    }}