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