From 5bbcc4c630e6e31abb7ff650ef7b07e67c14b3a1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jan 2017 03:05:38 -0500 Subject: [PATCH] lots of changes in communication for packets with reply --- include/CommunicationManager.h | 22 +- launch/xbeemav.launch | 1 + src/CommunicationManager.cpp | 368 ++++++++++++++++++++++----------- 3 files changed, 262 insertions(+), 129 deletions(-) diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 126b94a..65084cf 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -18,6 +18,10 @@ #include"SerialDevice.h" +#define MESSAGE_CONSTANT 238 +#define ACK_MESSAGE_CONSTANT 911 +#define XBEE_MESSAGE_CONSTANT 586782343 +#define XBEE_STOP_TRANSMISSION 4355356352 namespace Mist { @@ -88,11 +92,21 @@ private: ros::Publisher mavlink_publisher_; ros::ServiceClient mav_dji_client_; ros::ServiceServer mav_dji_server_; + /*No of robots*/ + int no_of_dev; + int device_id; /*Vector msgs*/ - std::map< std::size_t, std::shared_ptr > multi_msgs; - std::vector multi_msgs_available; - uint16_t cur_checksum; - uint16_t counter; + std::map< std::size_t, std::shared_ptr > multi_msgs_receive; + std::vector multi_msgs_send_dict; + /*Sending param*/ + uint16_t sending_chunk_no, Sender_cur_checksum; + std::map< uint16_t, uint16_t > ack_received_dict; + //std::vector multi_msgs_send_counter; + //std::vector multi_msgs_sender; + /*Receiving param*/ + uint16_t receiver_cur_checksum; + uint16_t counter; //After implementation change this to vector.size() + uint16_t receiveing_cur_totalsize; uint16_t steps; //uint16_t multi_msg_size; }; diff --git a/launch/xbeemav.launch b/launch/xbeemav.launch index 060dac3..8a448ab 100644 --- a/launch/xbeemav.launch +++ b/launch/xbeemav.launch @@ -7,6 +7,7 @@ + diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 19f4262..c0e3391 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -22,6 +22,17 @@ return out; } +uint16_t get_deviceid(){ +/* Get hostname */ + char hstnm[30]; + gethostname(hstnm, 30); + /* Make numeric id from hostname */ + /* NOTE: here we assume that the hostname is in the format M100X */ + int id = strtol(hstnm + 4, NULL, 10); + //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); +return (uint16_t)id; +} + namespace Mist { @@ -148,6 +159,12 @@ void CommunicationManager::Run_In_Swarm_Mode() else std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl; + /*Get no of devices*/ + node_handle_.getParam("No_of_dev", no_of_dev); + /*Get device Id feom host name*/ + device_id=get_deviceid(); + std::cout << "Device Id" < 0) { - if(!multi_msgs.empty()) steps++; - if(steps==10){ + if(!multi_msgs_receive.empty()) steps++; + if(steps>10){ steps=0; - multi_msgs.clear(); - cur_checksum=0; + multi_msgs_receive.clear(); + receiver_cur_checksum=0; } uint64_t current_int64 = 0; for (std::size_t j = 0; j < size_in_messages; j++) @@ -372,76 +389,117 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() sscanf(in_message->c_str(), "%" PRIu64 " ", ¤t_int64); header = u64_cvt_u16(current_int64); - //std::cout << "Received header" <1 && header[2]==1){ - for (std::size_t i = 1; i < in_message->size()-1; i++) - { - - if (' ' == in_message->at(i) || 0 == i) + /*Check header for msgs or ack msg */ + if(header[0]==(uint16_t)MESSAGE_CONSTANT){ + //std::cout << "Received header" <1 && header[2]==1){ + for (std::size_t i = 1; i < in_message->size()-1; i++) { - sscanf(in_message->c_str() + i, "%" PRIu64 " ", - ¤t_int64); - mavlink_msg.payload64.push_back(current_int64); - } - - } - //std::cout << "Single packet message received" << std::endl; - mavlink_publisher_.publish(mavlink_msg); - //delete[] header; - } - else if (header[3]>1 && header[0]==0 && header[1]>1){ - //std::cout << "Multi packet: check_cur:"< >::iterator it = multi_msgs.find(header[2]); - if(it!=multi_msgs.end()){ - multi_msgs.erase(it); - multi_msgs.insert(make_pair(header[2], in_message)); + if (' ' == in_message->at(i) || 0 == i) + { + sscanf(in_message->c_str() + i, "%" PRIu64 " ", + ¤t_int64); + mavlink_msg.payload64.push_back(current_int64); } - else{ - multi_msgs.insert(make_pair(header[2], in_message)); - counter++; + } - //std::cout << "multi msg counter" <1 && header[1]>1){ + + /*multimsg received send ack msg*/ + char temporary_buffer[20]; + std::string frame; + std::cout << "Multi msg Received header " < >::iterator it = multi_msgs_receive.find(header[2]); + if(it!=multi_msgs_receive.end()){ + multi_msgs_receive.erase(it); + multi_msgs_receive.insert(make_pair(header[2], in_message)); + } + else{ + multi_msgs_receive.insert(make_pair(header[2], in_message)); + counter++; + } + //std::cout << "multi msg counter" <first << "Size of current map" <second->size()<< std::endl; - //std::cout << "received Frame:"<<(void *) it->second->c_str() << std::endl; - //std::cout<<"Size of map : "<< multi_msgs.size()<< std::endl; - for (std::size_t j = 1; j < it->second->size()-1; j++) - { + for(uint16_t i =1; i<=header[3];i++){ + it = multi_msgs_receive.find(i); + //std::cout<<"Transfering to topic chunk no. :"<first << "Size of current map" <second->size()<< std::endl; + //std::cout << "received Frame:"<<(void *) it->second->c_str() << std::endl; + //std::cout<<"Size of map : "<< multi_msgs.size()<< std::endl; + for (std::size_t j = 1; j < it->second->size()-1; j++) + { - if (' ' == it->second->at(j) || 0 == j) - { - sscanf(it->second->c_str() + j, "%" PRIu64 " ", - ¤t_int64); - //std::cout << "received Frame:" << current_int64 << std::endl; - mavlink_msg.payload64.push_back(current_int64); - } + if (' ' == it->second->at(j) || 0 == j) + { + sscanf(it->second->c_str() + j, "%" PRIu64 " ", + ¤t_int64); + //std::cout << "received Frame:" << current_int64 << std::endl; + mavlink_msg.payload64.push_back(current_int64); + } - } - } + } + } - std::cout << "one multi message published in topic with size :" <::iterator it = ack_received_dict.find(header[3]); + if(it!=ack_received_dict.end()){ + ack_received_dict.erase(it); + ack_received_dict.insert(std::make_pair((uint16_t)header[3], (uint16_t)ACK_MESSAGE_CONSTANT)); + } + else{ + ack_received_dict.insert( std::make_pair( (uint16_t)header[3], (uint16_t)ACK_MESSAGE_CONSTANT ) ); + } + + } + std::cout << "ACK added and size of ack map " << ack_received_dict.size()<< std::endl; + + } + delete[] header; + } } @@ -494,97 +552,157 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback( const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) { const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */ - //const unsigned short MAX_NBR_OF_INT64 = 20; + const unsigned short MAX_NBR_OF_INT64 = 10; char temporary_buffer[MAX_BUFFER_SIZE]; std::string frame; int converted_bytes = 0; - - /* Check the payload is not too long. Otherwise ignore it. */ -/* if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64) - { - for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) - { - converted_bytes += sprintf( - temporary_buffer + converted_bytes, "%" PRIu64 " ", - (uint64_t)mavlink_msg->payload64.at(i)); - - } - - Generate_Transmit_Request_Frame(temporary_buffer, &frame); - serial_device_.Send_Frame(frame); + if((uint64_t)mavlink_msg->payload64.at(0)==(uint64_t)XBEE_STOP_TRANSMISSION && mavlink_msg->payload64.size() == 1){ + std::cout << "clearing multi msg queue after request from buzz"<< std::endl; + multi_msgs_send_dict.clear(); + ack_received_dict.clear(); + sending_chunk_no=0; + /*Multi message packets stoped tell rosbuzz this*/ + mavros_msgs::Mavlink mavlink_msg; + mavlink_msg.payload64.push_back(XBEE_MESSAGE_CONSTANT); + mavlink_publisher_.publish(mavlink_msg); } - else - {*/ + else if(mavlink_msg->payload64.size() > MAX_NBR_OF_INT64 && !( multi_msgs_send_dict.empty() ) ){ + std::cout << "Sending previous multi message not complete yet, so dropping message"<payload64.size(); i++) { converted_bytes += sprintf( temporary_buffer_check + converted_bytes, "%" PRIu64 " ", (uint64_t)mavlink_msg->payload64.at(i)); - + } + frame.append(temporary_buffer_check, std::strlen(temporary_buffer_check)); uint16_t check_sum = (uint16_t)Caculate_Checksum(&frame); uint16_t cnt=0; uint16_t number=1; - uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)10)); + uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)MAX_NBR_OF_INT64)); //std::cout <<"Payload size" <payload64.size() << std::endl; - uint64_t header = (uint64_t)0 | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; + uint64_t header = (uint64_t)MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; //std::cout << "Total chunks:" <payload64.size(); i++) - { - - if(cnt<10){ - cnt++; - converted_bytes += sprintf( - temporary_buffer+converted_bytes, "%" PRIu64 " ", - (uint64_t)mavlink_msg->payload64.at(i)); - //std::cout << "Sent Frame in (uint64):"<payload64.at(i) << std::endl; - //std::cout << "Sent Frame in string"<payload64.size() <= MAX_NBR_OF_INT64){ + for (std::size_t i =0; ipayload64.size(); i++) + { + cnt++; + converted_bytes += sprintf( + temporary_buffer+converted_bytes, "%" PRIu64 " ", + (uint64_t)mavlink_msg->payload64.at(i)); + //std::cout << "Sent Frame in (uint64):"<payload64.at(i) << std::endl; + //std::cout << "Sent Frame in string"<payload64.size()<< std::endl; + Generate_Transmit_Request_Frame(temporary_buffer, &frame); + serial_device_.Send_Frame(frame); + } - if(total==1){ - //std::cout << "Single frame" << std::endl; - Generate_Transmit_Request_Frame(temporary_buffer, &frame); - serial_device_.Send_Frame(frame); - } - if(number==total){ - Generate_Transmit_Request_Frame(temporary_buffer, &frame); - serial_device_.Send_Frame(frame); - } + else{ + /*clear all the related parameter and get ready to send multi msg*/ + sending_chunk_no=0; + Sender_cur_checksum = check_sum; + ack_received_dict.clear(); + multi_msgs_send_dict.clear(); + /*Multi message frame received, split them into chunks and store them in dict*/ + for (std::size_t i =0; ipayload64.size(); i++) + { + + cnt++; + converted_bytes += sprintf( + temporary_buffer+converted_bytes, "%" PRIu64 " ", + (uint64_t)mavlink_msg->payload64.at(i)); + //std::cout << "Sent Frame in (uint64):"<payload64.at(i) << std::endl; + //std::cout << "Sent Frame in string"<