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);
}
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

  }
Locator::Locator(ros::NodeHandle* n, Controller* c)
{
    controller_ = c;
    if (controller_ != NULL)
    {
        controller_->setLocator(this);
    }
    initROS(n);
}
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);
    int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2));
    *ssGetIWork(S) = nRobots;
    ssGetIWork(S)[1] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 4)));

    ros::NodeHandle nodeHandle(ros::this_node::getName());


    // get Topic Strings
    size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
    size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1;
    char* prefix_topic = (char*)mxMalloc(prefix_buflen);
    char* postfix_topic = (char*)mxMalloc(postfix_buflen);
    mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen);
    mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen);

    //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
    std::stringstream sstream;
    mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2));
    for (unsigned int i = 0; i < nRobots; ++i) {
        sstream.str(std::string());

        // build topicstring
        sstream << prefix_topic;
        sstream << (uint)robotIDs[i];
        sstream << postfix_topic;

        GenericSubscriber<sensor_msgs::JointState>* sub
                = new GenericSubscriber<sensor_msgs::JointState>(nodeHandle, sstream.str(), 10);
        vecPWork[i] = sub;
    }

    // free char array
    mxFree(prefix_topic);
    mxFree(postfix_topic);


    // Create nRobot Spinners
    ros::AsyncSpinner* spinner = new ros::AsyncSpinner(nRobots); // nRobots Spinner
    spinner->start();
    vecPWork[nRobots] = spinner;

  }
void
RosCommandAdapter::init(int argc, char** argv)
{
  std::cout << "initializing ROS command adapter" << std::endl;

  timestep = DEFAULT_TIMESTEP;
  command_rate = DEFAULT_COMMAND_RATE;
  msg_type = DEFAULT_MESSAGE_TYPE;
  rtf = DEFAULT_RTF;

  pthread_mutex_init(&data_mutex, NULL);

  // MUSIC before ROS to read the config first!
  initMUSIC(argc, argv);
  initROS(argc, argv);
}
void
RosSensorAdapter::init(int argc, char** argv)
{
  std::cout << "initializing ROS sensor adapter" << std::endl;

  timestep = DEFAULT_TIMESTEP;
  sensor_update_rate = DEFAULT_SENSOR_UPDATE_RATE;
  ros_node_name = DEFAULT_ROS_NODE_NAME;
  rtf = DEFAULT_RTF;

  pthread_mutex_init(&data_mutex, NULL);

  // MUSIC before ROS to read the config first!
  initMUSIC(argc, argv);
  initROS(argc, argv);
}
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);
    // save nRobots in IWorkVector
    int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2));
    *ssGetIWork(S) = nRobots;

    ros::NodeHandle nodeHandle(ros::this_node::getName());

    // get Topic Strings
    size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
    size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1;
    char* prefix_topic = (char*)mxMalloc(prefix_buflen);
    char* postfix_topic = (char*)mxMalloc(postfix_buflen);
    mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen);
    mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen);

    //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
    std::stringstream sstream;
    mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2));
    for (unsigned int i = 0; i < nRobots; ++i) {
        sstream.str(std::string());

        // build topicstring
        sstream << prefix_topic;
        sstream << (uint)robotIDs[i];
    	sstream << postfix_topic;

        GenericPublisher<geometry_msgs::PointStamped>* pub
			= new GenericPublisher<geometry_msgs::PointStamped>(nodeHandle, sstream.str(), 10);
        vecPWork[i] = pub;
	}

    // free char array
    mxFree(prefix_topic);
    mxFree(postfix_topic);
  }
static void mdlStart(SimStruct *S) {
	SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
	// init ROS if not yet done.
	initROS(S);

	void** vecPWork = ssGetPWork(S);

	ros::NodeHandle nodeHandle(ros::this_node::getName());

	// get Topic Strings
	size_t buflen = mxGetN((ssGetSFcnParam(S, 1))) * sizeof(mxChar) + 1;
	char* topic = (char*) mxMalloc(buflen);
	mxGetString((ssGetSFcnParam(S, 1)), topic, buflen);

	//SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
	GenericPublisher<shape_msgs::Mesh>* pub = new GenericPublisher<
			shape_msgs::Mesh>(nodeHandle, topic, 10);
	vecPWork[0] = pub;

	mxFree(topic);

}
TeleopPanel::TeleopPanel( QWidget* parent )
: rviz::Panel( parent )
{

	ROS_INFO("CPR RViz Panel V01.5 Nov. 11th, 2016");

    flagMover4 = true;      // default choice
    flagMover6 = false;
    override_value = 40;    // Value in percent

    // whicht robot to operate?
    // should be defined in the launch file / parameter server
    //<param name="robot_type" value="mover4"/>
    std::string global_name, relative_name, default_param;
    if (nh_.getParam("/robot_type", global_name)){
        ROS_INFO(global_name.c_str());

        if(global_name == "mover4"){
            flagMover4 = true;
            flagMover6 = false;
        }else if(global_name == "mover6"){
            flagMover4 = false;
            flagMover6 = true;
        }else{
            flagMover4 = true;
            flagMover6 = false;
        }

    }else{
        ROS_INFO("no robot name found");
    }

    initGUI();
    initROS();

    output_timer->start( 100 );     // Start the timer.
}
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::NodeHandle nodeHandle(ros::this_node::getName());

	// get base service name strings
	size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
	char* base_name = (char*)mxMalloc(buflen);
	mxGetString((ssGetSFcnParam(S, 1)), base_name, buflen);

	const std::string start_name = std::string(base_name) + "/simRosStartSimulation";
	ros::ServiceClient* start_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosStartSimulation>(start_name));
	vecPWork[0] = start_client;
	if (!start_client->exists()){
		ssSetErrorStatus(S,"Start service does not exist!");
	}

	const std::string stop_name = std::string(base_name) + "/simRosStopSimulation";
	ros::ServiceClient* stop_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosStopSimulation>(stop_name));
	vecPWork[1] = stop_client;
	if (!stop_client->exists()){
		ssSetErrorStatus(S,"Stop service does not exist!");
	}

	const std::string synchronous_name = std::string(base_name) + "/simRosSynchronous";
	ros::ServiceClient* synchronous_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSynchronous>(synchronous_name));
	vecPWork[2] = synchronous_client;
	if (!synchronous_client->exists()){
		ssSetErrorStatus(S,"Synchronous service does not exist!");
	}

	const std::string trigger_name = std::string(base_name) + "/simRosSynchronousTrigger";
	ros::ServiceClient* trigger_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSynchronousTrigger>(trigger_name));
	vecPWork[3] = trigger_client;
	if (!trigger_client->exists()){
		ssSetErrorStatus(S,"Trigger service does not exist!");
	}

	const std::string info_name = std::string(base_name) + "/simRosGetInfo";
	ros::ServiceClient* info_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosGetInfo>(info_name));
	if (!info_client->exists()){
		ssSetErrorStatus(S,"Info service does not exist!");
	}

	const std::string parameter_name = std::string(base_name) + "/simRosSetFloatingParameter";
	ros::ServiceClient* parameter_client =
			new ros::ServiceClient(nodeHandle.serviceClient<vrep_common::simRosSetFloatingParameter>(parameter_name));
	if (!parameter_client->exists()){
		ssSetErrorStatus(S,"Set parameter service does not exist!");
	}
	mxFree(base_name);

	// Set V-Rep time step to this block time step
	const real_T tSim = mxGetScalar(ssGetSFcnParam(S, 0));
	vrep_common::simRosSetFloatingParameter paramSrv;
	paramSrv.request.parameter = sim_floatparam_simulation_time_step;
	paramSrv.request.parameterValue = tSim;
	parameter_client->call(paramSrv);

	if (paramSrv.response.result == -1){
		ssSetErrorStatus(S,"Error setting V-REP simulation time step.");
	}

	vrep_common::simRosGetInfo infoSrv;
	info_client->call(infoSrv);
	const real_T vrepTimeStep = ROUND(infoSrv.response.timeStep*1e6)*1e-6;

	if (fabs(vrepTimeStep-tSim) > 1e-5){
		char str[100];
		sprintf(str, "V-REP time step (%e) is different from block time step (%e).", vrepTimeStep, tSim);
		ssSetErrorStatus(S,str);
	}

	// Set synchronous mode
	vrep_common::simRosSynchronous syncSrv;
	syncSrv.request.enable = true;
	synchronous_client->call(syncSrv);
	if (syncSrv.response.result == -1){
		ssSetErrorStatus(S,"Error setting V-REP synchronous simulation mode.");
	}

	// Start V-Rep simulation
	vrep_common::simRosStartSimulation startSrv;
	start_client->call(startSrv);
	if (startSrv.response.result == -1){
		ssSetErrorStatus(S,"Error starting V-REP simulation.");
	}

	// Subscribe to all the topics to wait
	size_t wait_buflen = mxGetN((ssGetSFcnParam(S, 2)))*sizeof(mxChar)+1;
	char* wait_name = (char*)mxMalloc(wait_buflen);
	mxGetString((ssGetSFcnParam(S, 2)), wait_name, wait_buflen);

	int_T nWaitTopic = 0;
	char* topic = strtok(wait_name, TOPIC_SEPARATORS);
	while(topic != NULL){
		GenericSubscriber<topic_tools::ShapeShifter>* sub
			= new GenericSubscriber<topic_tools::ShapeShifter>(nodeHandle, topic, 1);
		vecPWork[4+nWaitTopic] = sub;
		nWaitTopic++;
		topic = strtok (NULL, TOPIC_SEPARATORS);
	}

	if (nWaitTopic>0){
		ros::AsyncSpinner* spinner = new ros::AsyncSpinner(nWaitTopic);
		spinner->start();
		vecPWork[4+nWaitTopic] = spinner;
		ssGetIWork(S)[0] = nWaitTopic;
	}

	mxFree(wait_name);

}