Spiri
/
services-ros1-xbee_mav
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

modifications in step clearing of multi msg map

This commit is contained in:
vivek-shankar 2017-01-29 18:38:09 -05:00
parent 88a6b12b14
commit 1e6afbf004
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/CommunicationManager.cpp
View File

@ -170,11 +170,13 @@ void CommunicationManager::Run_In_Swarm_Mode()
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
counter=0; counter=0;
test_1=1;
while (ros::ok()) while (ros::ok())
{ {
Check_In_Messages_and_Transfer_To_Topics(); Check_In_Messages_and_Transfer_To_Topics();
Send_multi_msg(); Send_multi_msg();
//if(counter==10) {test_1=5;}
//else counter++;
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
@ -373,7 +375,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{ {
std::size_t size_in_messages = in_messages_->Get_Size(); std::size_t size_in_messages = in_messages_->Get_Size();
uint16_t* header; uint16_t* header;
if(!multi_msgs_receive.empty()) steps++; //if(!multi_msgs_receive.empty()) steps++;
if(steps>200){ if(steps>200){
steps=0; steps=0;
multi_msgs_receive.clear(); multi_msgs_receive.clear();
@ -407,8 +409,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
char temporary_buffer[20]; char temporary_buffer[20];
std::string frame; std::string frame;
if(current_int64 <= 3){ /*if(current_int64 <= 3){
Generate_Transmit_Request_Frame(in_message->c_str(), &frame,sizeof(uint64_t) * 4); Generate_Transmit_Request_Frame(in_message->c_str(), &frame,sizeof(uint64_t) * );
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
} }
if(current_int64 == (uint64_t)device_id ){ if(current_int64 == (uint64_t)device_id ){
@ -419,7 +421,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
std::cout<<"Time taken for send and receive : "<<time_spent<<std::endl; std::cout<<"Time taken for send and receive : "<<time_spent<<std::endl;
test_1=1; test_1=1;
} }*/
if(header[0]==(uint16_t)MESSAGE_CONSTANT){ if(header[0]==(uint16_t)MESSAGE_CONSTANT){
if(header[3]==1 && header[1]>1 && header[2]==1){ if(header[3]==1 && header[1]>1 && header[2]==1){
/*copy msg size*/ /*copy msg size*/
@ -454,8 +456,9 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
//char temporary_buffer[20]; //char temporary_buffer[20];
//std::string frame; //std::string frame;
std::cout << "Multi msg Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl; std::cout << "Multi msg Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
if (multi_msgs_receive.empty()){ if (header[2]==1){
//std::cout << "first message" << std::endl; //std::cout << "first message" << std::endl;
multi_msgs_receive.clear();
multi_msgs_receive.insert(make_pair(header[2], in_message)); multi_msgs_receive.insert(make_pair(header[2], in_message));
receiver_cur_checksum=header[1]; receiver_cur_checksum=header[1];
//counter=1; //counter=1;
@ -801,21 +804,22 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
void CommunicationManager::Send_multi_msg(){ void CommunicationManager::Send_multi_msg(){
if(test_1){ /*if(test_1>0){
test_1=0; test_1--;
char temporary_buffer[sizeof(uint64_t) * 4]; char temporary_buffer[sizeof(uint64_t) * 5];
std::string frame; std::string frame;
uint64_t constant_msg[3]; uint64_t constant_msg[3];
constant_msg[1]=245253253253532; constant_msg[1]=245253253253532;
constant_msg[2]=245253253253532; constant_msg[2]=245253253253532;
constant_msg[3]=245253253253532; constant_msg[3]=245253253253532;
constant_msg[0]=(uint64_t) device_id; constant_msg[0]=(uint64_t) device_id;
constant_msg[4]=(uint64_t) test_t;
gettimeofday(&t1, NULL); gettimeofday(&t1, NULL);
/*Copy the data to char buff*/ /*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)constant_msg,sizeof(uint64_t) * 4); /*memcpy((void*)temporary_buffer,(void*)constant_msg,sizeof(uint64_t) * 5);
Generate_Transmit_Request_Frame(temporary_buffer, &frame,sizeof(uint64_t) * 4); Generate_Transmit_Request_Frame(temporary_buffer, &frame,sizeof(uint64_t) * 5);
multi_msgs_send_dict.push_back(frame); multi_msgs_send_dict.push_back(frame);
} }*/
if( !( multi_msgs_send_dict.empty() ) ){ if( !( multi_msgs_send_dict.empty() ) ){
/*If the sent message chunk not the last message then send else clear the dict*/ /*If the sent message chunk not the last message then send else clear the dict*/
if( (uint16_t)(multi_msgs_send_dict.size() ) - 1 == sending_chunk_no && (uint16_t)ack_received_dict.size() == (uint16_t)(no_of_dev)-1){ if( (uint16_t)(multi_msgs_send_dict.size() ) - 1 == sending_chunk_no && (uint16_t)ack_received_dict.size() == (uint16_t)(no_of_dev)-1){