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 rc_cmd;
int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
bool rcclient;
bool multi_msg;

41
src/roscontroller.cpp
View File

@ -68,36 +68,26 @@ namespace rosbzz_node{
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
//fprintf(stdout, "before update routine\n");
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
//fprintf(stdout, "before code step\n");
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();
/*Prepare messages and publish them in respective topic*/
//fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish();
/*Set multi message available after update*/
if(get_update_status()){
set_read_update_status();
multi_msg=true;
}
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
/*if( get_update_mode() == CODE_STANDBY){
ros::Rate loop_rate(10);
loop_rate.sleep();
}
else{*/
ros::Rate loop_rate(10);
loop_rate.sleep();
//}
/*sleep for the mentioned loop rate*/
timer_step+=1;
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");}
/*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*/
n_c.getParam("out_payload", out_payload);
/*Obtain in payload name*/
@ -318,6 +309,13 @@ namespace rosbzz_node{
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*/
payload_pub.publish(payload_out);
delete[] out;
@ -352,6 +350,11 @@ namespace rosbzz_node{
for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]);
}
/*Add Robot id and message number to the published message*/
if (message_number < 0) message_number=0;
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;