Ejemplo n.º 1
0
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");
	} 
}
Ejemplo n.º 2
0
static void
publishErrorMessageAndStop( tr_tracker * t,
                            const char * msg )
{
    t->isRunning = 0;
    publishMessage( t, msg, TR_TRACKER_ERROR );
}
Ejemplo n.º 3
0
// 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);
}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
0
void Listener::run(){
	ros::Rate loopRate(pubRate);
	
	while (ros::ok()){    
		ros::spinOnce();
		
		publishMessage();

		loopRate.sleep();
	}
}
Ejemplo n.º 7
0
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;
}
Ejemplo n.º 8
0
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);
}
}
Ejemplo n.º 9
0
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());
	}
}
Ejemplo n.º 10
0
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);
  }
  
}
Ejemplo n.º 12
0
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;
	}
}
Ejemplo n.º 13
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); 
  }
  
}
Ejemplo n.º 14
0
static void
publishErrorClear( tr_tracker * t )
{
    publishMessage( t, NULL, TR_TRACKER_ERROR_CLEAR );
}
Ejemplo n.º 15
0
static void
publishWarning( tr_tracker * t,
                const char * msg )
{
    publishMessage( t, msg, TR_TRACKER_WARNING );
}
Ejemplo n.º 16
0
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();
  } 
 
}
Ejemplo n.º 17
0
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");
  }
}