void EventDispatcher::listen(string event,EventCallback callback,IEventPublisher* publisher){
//this->listeners[event].insert(pair<IEventPublisher*,vector<EventCallback>(publisher,callback));
if(isListening(event)){
if(hasPublisher(event,publisher)){
this->listeners[event].at(publisher).push_back(callback);
} else {
this->listeners[event].insert(pair<IEventPublisher*,vector<EventCallback>>(publisher,{callback}));
}
} else {
this->listeners[event].insert(pair<IEventPublisher*,vector<EventCallback>>(publisher,{callback}));
}
g_message("EventDispatcher: Adding event >> %s",event.c_str());
}
//Methods
int RosAriaVRep::runNode() {
ROS_INFO("Running node");
if(VRepUtils::getObjectHandle(motorEsquerdoObjectHandleName,nodeHandler,signalObjectMap) &&
VRepUtils::getObjectHandle(motorDireitoObjectHandleName,nodeHandler,signalObjectMap) &&
VRepUtils::getObjectHandle(pionnerLxObjectHandleName,nodeHandler,signalObjectMap) &&
VRepUtils::getObjectHandle(laserObjectHandleName,nodeHandler,signalObjectMap) &&
VRepUtils::getObjectHandle(laserBodyObjectHandleName,nodeHandler,signalObjectMap)) {
rosaria_v_rep::simRosEnableSubscriber simRosEnableSubscriber =
createEnableSubscriber(cmdVelTopic,simros_strmcmd_set_twist_command, -1, -1);
serviceClientsMap[enableSubscriberService].call(simRosEnableSubscriber);
if(simRosEnableSubscriber.response.subscriberID == -1) {
infoFailAndExit(cmdVelTopic);
}
rosaria_v_rep::simRosEnablePublisher simRosEnablePublisher =
createEnablePublisher(poseTopic, simros_strmcmd_get_object_pose,
signalObjectMap[pionnerLxObjectHandleName], poseRelativeToWorld, "");
serviceClientsMap[enablePublisherService].call(simRosEnablePublisher);
if(simRosEnablePublisher.response.effectiveTopicName.length() == 0) {
infoFailAndExit(poseTopic);
}
rosaria_v_rep::simRosEnablePublisher simRosEnablePublisher2 =
createEnablePublisher(laserTopic, simros_strmcmd_get_laser_scanner_data,
signalObjectMap[laserBodyObjectHandleName], -1, laserVRepSignal);
serviceClientsMap[enablePublisherService].call(simRosEnablePublisher2);
if(simRosEnablePublisher2.response.effectiveTopicName.length() == 0) {
infoFailAndExit(laserTopic);
}
std_msgs::Bool boolean;
boolean.data = true;
if(hasPublisher(motorStateTopic)) {
publisherMap[motorStateTopic].publish(boolean);
} else {
infoFailAndExit(motorStateTopic);
}
} else {
ROS_ERROR("Failed to get objects from V-Rep simulation. Be sure"
" that the simulation is running.");
return shutdownAndExit();
}
ros::spin();
return shutdownAndExit();
}
