Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

addition of sysid and message id

This commit is contained in:
vivek-shankar 2017-02-19 19:22:47 -05:00
parent 25fc91e432
commit 73f7662fc4
2 changed files with 46 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/roscontroller.h
View File

@ -53,6 +53,7 @@ private:
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
int barrier; int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by; std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
bool rcclient; bool rcclient;
bool multi_msg; bool multi_msg;

87
src/roscontroller.cpp
View File

@ -68,36 +68,26 @@ namespace rosbzz_node{
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl; //std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
} }
neigh_pos_pub.publish(neigh_pos_array); neigh_pos_pub.publish(neigh_pos_array);
//fprintf(stdout, "before update routine\n");
/*Check updater state and step code*/ /*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
//fprintf(stdout, "before code step\n"); /*Step buzz script */
if(get_update_status()){
buzz_utility::buzz_update_set((get_updater())->bcode, dbgfname.c_str(), *(size_t*)((get_updater())->bcode_size));
set_read_update_status();
mavros_msgs::Mavlink stop_req_packet;
stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
payload_pub.publish(stop_req_packet);
std::cout << "request xbee to stop multi-packet transmission" << std::endl;
}
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
//fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish(); prepare_msg_and_publish();
/*Set multi message available after update*/
if(get_update_status()){
set_read_update_status();
multi_msg=true;
}
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*loop rate of ros*/ /*loop rate of ros*/
/*if( get_update_mode() == CODE_STANDBY){
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
loop_rate.sleep(); loop_rate.sleep();
}
else{*/
ros::Rate loop_rate(10);
loop_rate.sleep();
//}
/*sleep for the mentioned loop rate*/ /*sleep for the mentioned loop rate*/
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
@ -126,7 +116,8 @@ namespace rosbzz_node{
} }
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
/*Obtain robot_id from parameter server*/ /*Obtain robot_id from parameter server*/
n_c.getParam("robot_id", robot_id); //n_c.getParam("robot_id", robot_id);
robot_id=(int)buzz_utility::get_robotid();
/*Obtain out payload name*/ /*Obtain out payload name*/
n_c.getParam("out_payload", out_payload); n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/ /*Obtain in payload name*/
@ -318,6 +309,13 @@ namespace rosbzz_node{
cout<<" [Debug:] sent message "<<out[k]<<endl; cout<<" [Debug:] sent message "<<out[k]<<endl;
} }
}*/ }*/
/*Add Robot id and message number to the published message*/
if (message_number < 0) message_number=0;
else message_number++;
payload_out.sysid=(uint8_t)robot_id;
payload_out.msgid=(uint32_t)message_number;
/*publish prepared messages in respective topic*/ /*publish prepared messages in respective topic*/
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
delete[] out; delete[] out;
@ -334,27 +332,32 @@ namespace rosbzz_node{
/*allocate mem and clear it*/ /*allocate mem and clear it*/
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize); buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize); memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/ /*Append updater msg size*/
*(uint16_t*)(buff_send + tot)=updater_msgSize; *(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/*Append updater msgs*/ /*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize; tot += updater_msgSize;
/*Destroy the updater out msg queue*/ /*Destroy the updater out msg queue*/
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
delete[] buff_send; delete[] buff_send;
/*Send a constant number to differenciate updater msgs*/ /*Send a constant number to differenciate updater msgs*/
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){ for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]); update_packets.payload64.push_back(payload_64[i]);
} }
payload_pub.publish(update_packets); /*Add Robot id and message number to the published message*/
multi_msg=false; if (message_number < 0) message_number=0;
delete[] payload_64; else message_number++;
update_packets.sysid=(uint8_t)robot_id;
update_packets.msgid=(uint32_t)message_number;
payload_pub.publish(update_packets);
multi_msg=false;
delete[] payload_64;
} }
/*Request xbee to stop any multi transmission updated not needed any more*/ /*Request xbee to stop any multi transmission updated not needed any more*/
//if(get_update_status()){ //if(get_update_status()){
@ -727,7 +730,7 @@ namespace rosbzz_node{
} }
return true; return true;
} }
} }