void turn(enum protocol where) {
if (where == COMMAND_LEFT) {
currentDirection = (currentDirection - 1) % DIRECTION_NUM;
publishMessage("Turned left");
} else if (where == COMMAND_RIGHT) {
currentDirection = (currentDirection + 1) % DIRECTION_NUM;
publishMessage("Turned right");
}
}
static void
publishErrorMessageAndStop( tr_tracker * t,
const char * msg )
{
t->isRunning = 0;
publishMessage( t, msg, TR_TRACKER_ERROR );
}
// Will be called when WiFi station was connected to AP
void connectOk()
{
if (USE_SERIAL) Serial.println("I'm CONNECTED");
// Run MQTT client
mqtt.connect("esp8266");
publishMessage();
}
void PhasespaceCore::readAndPublish() {
// queries the server for new markers data and returns the number of current active markers (0 means old data)
int num_markers = owlGetMarkers(markers_, init_marker_count_);
if (owlGetError() != OWL_NO_ERROR) {
ROS_FATAL_STREAM("[PhaseSpace] Error while reading markers' positions: " << owlGetError());
throw excp_;
}
// waits for available data
if (num_markers == 0) {
return;
}
ros::Time current_time = ros::Time::now();
// please, make sure that a node does not use both this function and 'gloveMessageCallback(&msg)' one
num_data_retrieved_++;
partial_num_data_retrieved_++;
// counts how many markers are visible at the moment
int* markers_count = markers_count_->data();
int num_visible_markers = 0;
for (int i=0; i<num_markers; i++) {
if (markers_[i].cond > 0) {
markers_count[i]++;
num_visible_markers++;
}
}
publishMessage(current_time, num_visible_markers, num_markers);
}
bool checkNextStep() {
printField();
struct position nextPosition;
nextPosition.i = currentPosition.i + shiftList[currentDirection][0];
nextPosition.j = currentPosition.j + shiftList[currentDirection][1];
enum state_type next;
if (nextPosition.i < 0 || nextPosition.j < 0 || nextPosition.i >= field.cols ||
nextPosition.j >= field.rows) {
next = WALL;
} else {
next = field.m[currentPosition.i + shiftList[currentDirection][0]]
[currentPosition.j + shiftList[currentDirection][1]];
}
if (next != NONE) {
char res[25];
currentSensor = next;
if (currentState == GO) {
currentState = STOP;
sprintf(res, "Stopped: %s", sensor_type_str[currentSensor]);
} else if (currentState == STOP) {
sprintf(res, "The %s was detected", sensor_type_str[currentSensor]);
}
publishRobotState();
publishMessage(res);
return false;
}
currentSensor = next;
return true;
}
void Listener::run(){
ros::Rate loopRate(pubRate);
while (ros::ok()){
ros::spinOnce();
publishMessage();
loopRate.sleep();
}
}
int mainSubASync(int argc, char* argv[])
{
initializeObjects();
readFile();
printf("\n Broker URL is := %s \n",brokerUrl);
initiliazeMQTT(argc,argv);
MQTTClient client;
MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
int rc;
int ch;
MQTTClient_create(&client, ADDRESS, CLIENTID,
MQTTCLIENT_PERSISTENCE_NONE, NULL);
conn_opts.keepAliveInterval = 20;
conn_opts.cleansession = 1;
MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered);
if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS)
{
printf("Failed to connect, return code %d\n", rc);
exit(-1);
}
printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n"
"Press Q<Enter> to quit\n\n", TOPIC, CLIENTID, QOS);
MQTTClient_subscribe(client, TOPIC, QOS);
// do
// {
// ch = getchar();
// } while(ch!='Q' && ch != 'q');
while(1){
ch = getchar();
if(ch=='Q'){
printf("\n Quit! \n");
} else {
if(ch != '\n'){
ch=='0';
printf("\n Sending Message to publish \n");
publishMessage(userid);
}
}
}
MQTTClient_disconnect(client, 10000);
MQTTClient_destroy(&client);
return rc;
}
void makeStep() {
if (currentState != STOP) {
currentPosition.i += shiftList[currentDirection][0];
currentPosition.j += shiftList[currentDirection][1];
publishRobotState();
char* fieldStr = getFieldAsString();
char* res = (char*) malloc(MAX_LENGTH_OF_UPDATE_JSON_BUFFER*sizeof(char));
strcpy(res, "");
strcat(res, "I am going\n");
strcat(res, fieldStr);
publishMessage(res);
free(res);
}
}
void handleCommand() {
IOT_INFO("current state %d %s", currentState, state_type_str[currentState]);
if (currentState == GO) { // GO
if (currentCommand != COMMAND_STOP) {
publishMessage("Prohibited command. Only STOP available");
} else {
currentState = STOP;
IOT_INFO("stopped");
currentSensor = NONE;
publishRobotState();
publishMessage("Stopped");
}
}
else if (currentState == STOP) { // STOP
IOT_INFO("current command %d %s", currentCommand, protocol_str[currentCommand]);
if (currentCommand == COMMAND_LEFT || currentCommand == COMMAND_RIGHT) {
turn(currentCommand);
publishRobotState();
checkNextStep();
} else if (currentCommand == COMMAND_GO) {
if (checkNextStep()) {
// makeStep();
currentState = GO;
// currentSensor = NONE;
// publishRobotState();
}
} else if (currentCommand == COMMAND_STOP) {
publishMessage("I am not moving");
}
}
if (currentState == STOP) {
publishMessage(getFieldAsString());
}
}
void iot_subscribe_callback_handler(AWS_IoT_Client *pClient, char *topicName, uint16_t topicNameLen,
IoT_Publish_Message_Params *params, void *pData) {
IOT_UNUSED(pData);
IOT_UNUSED(pClient);
IOT_INFO("Message: %.*s\t%.*s", topicNameLen, topicName, (int) params->payloadLen, (char*)params->payload);
char* msg = (char*)(malloc((int) params->payloadLen) + 1);
strncpy(msg, (char*) params->payload, (int) params->payloadLen);
msg[(int) params->payloadLen] = '\0';
if (findCommand(msg)) {
handleCommand();
} else {
publishRobotState();
publishMessage("Undefined command");
}
}
void DiscoveryHelperServer::forward(const std::string& strMessage, const std::string& fromIP)
{
char serialization_letter = strMessage[0];
std::string str = "";
switch (serialization_letter) {
case 'H': // message_code == HELLO_CODE
{
HelloMessage message = deserializeMessage<HelloMessage>(strMessage);
coros_messages::HelloMsg m = message.toMsg();
publishMessage("hello_pub", m);
ROS_INFO("[Simulation Server][Event]: received/forwarded HELLO message:\n%s.", mtos(m).c_str());
break;
}
default:
ROS_ERROR("[Simulation Server][Error]: unable to identify message serialization letter: %c.", serialization_letter);
}
}
int main(int argc, char **argv) {
IoT_Error_t rc = FAILURE;
parseInputArgsForConnectParams(argc, argv);
if (!readDataFromFile()) {
printf("Config file '%s' not found. Robot cannot work without configuration.\n", filename);
return -1;
}
else {
if (connectToThingAndSubscribeToTopic(argc, argv)) {
publishMessage(getFieldAsString());
publishRobotState();
while (true) {
//Max time the yield function will wait for read messages
rc = aws_iot_mqtt_yield(&mqttClient, 1000);
if (NETWORK_ATTEMPTING_RECONNECT == rc) { // || NETWORK_ATTEMPTING_RECONNECT == rc1) {
// If the client is attempting to reconnect we will skip the rest of the loop.
continue;
}
sleep(3);
if (currentState == GO) {
if (checkNextStep()) {
currentState = GO;
currentSensor = NONE;
makeStep();
}
}
}
}
return 0;
}
}
void DiscoveryOperator::broadcast_hello_message(double freqInSeconds){
//impose frequency to be larger than 5 seconds
//freqInSeconds = ((freqInSeconds>5.0)?freqInSeconds:5.0);
if( myLocalConfig->getAgentRunningMode() == AgentConfiguration::SIMULATION_RUN_MODE )
{
ROS_INFO("[Discovery Operator]: broadcast the first HELLO message to initialize simulation...");
// HelloMessage hello_message(HELLO_FROM_ROBOT, myLocalConfig->getAgentId(), myLocalConfig->getAgentAddress(), myLocalConfig->getAgentPort(), ros::Time::now().toSec(), myLocalConfig->getAgentRole(), myLocalConfig->getAgentStatus());
coros_messages::HelloMsg hello_message;
hello_message.message_code = HELLO_FROM_ROBOT;
hello_message.id = myLocalConfig->getAgentId();
hello_message.ip = myLocalConfig->getAgentAddress();
hello_message.name = myLocalConfig->getAgentName();
hello_message.port = myLocalConfig->getAgentPort();
hello_message.timestamp = ros::Time::now().toSec();
hello_message.role = myLocalConfig->getAgentRole();
hello_message.status = myLocalConfig->getAgentStatus();
//old boost serialization
//std::string msg_str = serializeMessage(hello_message);
//send(msg_str, myLocalConfig->getAgentAddress(), myLocalConfig->getInitSimulationPort());
//new json serialization
//publishMessage("to_json", hello_message);
}
// TODO: Check in the loop that if a robot did not sent any hello message from a long time (30 seconds for example), its entry will be dropped from the neighbors table
while (ros::ok()){
ROS_INFO("[Discovery Operator]: broadcast a new HELLO message ...");
//HelloMessage hello_message(HELLO_FROM_ROBOT, myLocalConfig->getAgentId(), myLocalConfig->getAgentAddress(), myLocalConfig->getAgentPort(), ros::Time::now().toSec(), myLocalConfig->getAgentRole(), myLocalConfig->getAgentStatus());
//std::string msg_str = serializeMessage(hello_message);
coros_messages::HelloMsg hello_message;
hello_message.message_code = HELLO_FROM_ROBOT;
hello_message.id = myLocalConfig->getAgentId();
hello_message.name = myLocalConfig->getAgentName();
hello_message.ip = myLocalConfig->getAgentAddress();
hello_message.port = myLocalConfig->getAgentPort();
hello_message.timestamp = ros::Time::now().toSec();
hello_message.role = myLocalConfig->getAgentRole();
hello_message.status = myLocalConfig->getAgentStatus();
if (myLocalConfig->getAgentRunningMode() == AgentConfiguration::SIMULATION_RUN_MODE)
{
vector<std::string> destinations;
vector<int> ports;
int size = 0;
std::vector<AgentState*> neighborAgents = myNeighborsConfig->getNeighbors();
size = neighborAgents.size();
for (unsigned i=0; i<size; i++){
destinations.push_back(neighborAgents.at(i)->getIpAddress());
ports.push_back(neighborAgents.at(i)->getPort());
}
hello_message.header.distribution_mode="multicast";
hello_message.header.ip_addresses=destinations;
hello_message.header.ports=ports;
//multicast(msg_str, &destinations[0], &ports[0], size);
}
else { //if(myLocalConfig->getAgentRunningMode() == AgentConfiguration::EXPERIMENTAL_RUN_MODE)
hello_message.header.distribution_mode="broadcast";
hello_message.header.ports.push_back(myLocalConfig->getAgentPort());
//broadcast(msg_str, myLocalConfig->getAgentPort());
}
publishMessage("to_json", hello_message);
usleep(freqInSeconds*1000*1000);
}
}
static void
publishErrorClear( tr_tracker * t )
{
publishMessage( t, NULL, TR_TRACKER_ERROR_CLEAR );
}
static void
publishWarning( tr_tracker * t,
const char * msg )
{
publishMessage( t, msg, TR_TRACKER_WARNING );
}
void DeliveryOperator::requestCallback(const delivery_app::DeliveryRequestMsg::ConstPtr& msg)
{
ROS_INFO("Courier Robot Operator received Request message from the Courier Robot server.");
// save the request source/target coordinates
myLocalConfig->setSourcePositionX( msg->source_x );
myLocalConfig->setSourcePositionY( msg->source_y );
myLocalConfig->setTargetPositionX( msg->target_x );
myLocalConfig->setTargetPositionY( msg->target_y );
// prepare the header of the status message
delivery_app::DeliveryStatusMsg status_message;
status_message.header.distribution_mode = "unicast";
status_message.header.ip_addresses.push_back( msg->agent_address );
status_message.header.ports.push_back( msg->agent_port );
status_message.agent_id = myLocalConfig->getAgentId();
if (myLocalConfig->isUnavailableAgent())
{
// send busy.
status_message.delivery_status = "Busy";
publishMessage("sta_to_json", status_message);
}
else
{
myLocalConfig->setUnavailableAgentStatus();
// send ready
status_message.delivery_status = "Ready";
publishMessage("sta_to_json", status_message);
// moving to the source
SimpleMoveToTask task(0,
myLocalConfig->getAgentRunningMode(),
myLocalConfig->getSourcePositionX(),
myLocalConfig->getSourcePositionY());
perform(&task);
// send source // arrived to source
status_message.delivery_status = "Source";
publishMessage("sta_to_json", status_message);
// wait until putting on the delivery object
ROS_INFO("Please put the object to be delivered in the specific plane, then press 'Enter'...");
getchar();
// send mission
status_message.delivery_status = "Mission";
publishMessage("sta_to_json", status_message);
// moving to the target
task.setTargetX( myLocalConfig->getTargetPositionX() );
task.setTargetY( myLocalConfig->getTargetPositionY() );
perform(&task);
// send target // arrived to the target
status_message.delivery_status = "Target";
publishMessage("sta_to_json", status_message);
// wait until getting off the delivery object
ROS_INFO("Please get off the delivered object, then press 'Enter'...");
getchar();
// send home // in the way to its initial localtion.
status_message.delivery_status = "Home";
publishMessage("sta_to_json", status_message);
// moving to the initial position
task.setTargetX( task.getInitialPositionX() );
task.setTargetY( task.getInitialPositionY() );
perform(&task);
// send Result message
delivery_app::DeliveryResultMsg result_message;
result_message.header.distribution_mode = "unicast";
result_message.header.ip_addresses.push_back( msg->agent_address );
result_message.header.ports.push_back( msg->agent_port );
result_message.agent_id = myLocalConfig->getAgentId();
result_message.agent_cost = task.getCost();
result_message.source_x = myLocalConfig->getSourcePositionX();
result_message.source_y = myLocalConfig->getSourcePositionY();
result_message.target_x = myLocalConfig->getTargetPositionX();
result_message.target_y = myLocalConfig->getTargetPositionY();
publishMessage("res_to_json", result_message);
myLocalConfig->setAvailableAgentStatus();
}
}
void DiscoveryOperator::helloCallback(const coros_messages::HelloMsg::ConstPtr& msg)
{
ROS_INFO("\n[Discovery Operator]\n\t\t[Event]: received HELLO message: \n%s.", mtos(*msg).c_str());
int message_code = msg->message_code;
if (message_code == HELLO_FROM_CONTROL_STATION) {// HELLO MESSAGE COMING FROM CONTROL STATION
ROS_INFO("\n[Discovery Operator]\n\t\t[Event]: received HELLO message from a Control Station: \n%s. \n\t\t[Action]: Update Control Station information", mtos(*msg).c_str());
//myControlStationIP = msg->ip;
//myControlStationPort = msg->port;
}
else if (msg->id == myLocalConfig->getAgentId())
{
//update the internal agent state
myLocalConfig->setAgentTimestamp(ros::Time::now().toSec());
myLocalConfig->setAgentRole(msg->role);
myLocalConfig->setAgentStatus(msg->status);
}
else if (message_code == HELLO_FROM_ROBOT)
{
ROS_INFO("[Discovery Operator]: [Event]: Received HELLO message from an agent: \n%s" , mtos(*msg).c_str());
ROS_INFO("[Discovery Operator]: [Action]: Add robot to robot neighbor list.");
AgentState* newNeighborAgent = new AgentState(msg->id, msg->ip, msg->timestamp, msg->port, msg->role, msg->status);
myNeighborsConfig->addNeighbor(newNeighborAgent);
myNeighborsConfig->printNeighbors();
/*
hello_robot_info_type newNeighborRobot;
newNeighborRobot.id = msg->id;
newNeighborRobot.ip = msg->ip;
newNeighborRobot.port = msg->port;
newNeighborRobot.timestamp = msg->timestamp;
*/
// check if the robots is already in the neighbor list
/*bool robotExists = false;
unsigned int robotIndex=-1;
for (unsigned int i=0; i<neighborAgents.size(); i++){
if (neighborAgents.at(i).id == newNeighborRobot.id){
robotExists = true;
robotIndex=i;
break;
}
}
//if the robot exist, delete it to add a new entry with new timestamp
if (robotExists)
neighborAgents.erase(neighborAgents.begin() + robotIndex);
//add the robot to the neighbor list
neighborAgents.push_back(newNeighborRobot); */
/** publish the new neighborhood_list */
std::vector<AgentState*> neighborAgents = myNeighborsConfig->getNeighbors();
coros_messages::NeighborhoodMsg m;
for (unsigned i=0; i<neighborAgents.size(); i++){
m.ids.push_back(neighborAgents.at(i)->getId());
m.ip_addresses.push_back(neighborAgents.at(i)->getIpAddress());
m.ports.push_back(neighborAgents.at(i)->getPort());
m.timestamps.push_back(ros::Time::now().toSec());
m.roles.push_back(neighborAgents.at(i)->getRole());
m.statuses.push_back(neighborAgents.at(i)->getStatus());
}
publishMessage("neighborhood_pub", m);
}
else{
ROS_WARN("\n[Discovery Operator]\n\t\t[WARNING]: received an unknown HELLO message type.\n\t\t[Action]: Check issue in Discovery Robot Operator\n");
}
}
