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

more topics fix and merge log

Browse Source
This commit is contained in:
dave 2017-08-30 22:05:57 -04:00
commit a745de8f31
5 changed files with 65 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -5,3 +5,4 @@ src/test*
*.bo
*.bdb
*.bdbg
buzz_scripts/log*

2
include/buzz_utility.h
View File

@ -77,4 +77,6 @@ int get_robotid();
buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
int get_inmsg_size();
}

10
misc/cmdlinectr.sh
View File

@ -31,13 +31,5 @@ function stoprobot {
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch
}

4
src/buzz_utility.cpp
View File

@ -786,4 +786,8 @@ int create_stig_tables() {
buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM);
}
int get_inmsg_size(){
return IN_MSG.size();
}
}

60
src/roscontroller.cpp
View File

@ -46,7 +46,12 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
}
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path += "Update.log";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_old.log").c_str());
path += "logger_"+std::to_string(robot_id)+".log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
}
@ -195,11 +200,42 @@ void roscontroller::RosControllerRun()
while (ros::ok() && !buzz_utility::buzz_script_done()) {
/*Update neighbors position inside Buzz*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
neigh pos, RSSI val, Packet loss, filtered packet loss*/
log<<ros::Time::now()<<",";
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << neighbours_pos_map.size()<< ",";
for (; it != neighbours_pos_map.end(); ++it)
{
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z << ",";
}
const uint8_t shrt_id= 0xFF;
float result;
if ( GetAPIRssi(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetRawPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<",";
else
log<<"0,";
/*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher();
send_MPpayload();
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
@ -209,7 +245,7 @@ void roscontroller::RosControllerRun()
/*Set multi message available after update*/
if (get_update_status())
{
set_read_update_status();
/* set_read_update_status();
multi_msg = true;
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
@ -222,12 +258,27 @@ void roscontroller::RosControllerRun()
log << "," << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
}
log << std::endl;
log << std::endl;*/
}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots();
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
SCRIPT IS NOT graphform.bzz*/
static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
buzzvm_gload(VM);
buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
std::stringstream state_buff;
state_buff<< graph_state->s.value.str<<",";
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
state_buff<< uav_state->s.value.str;
log<<state_buff.str()<<std::endl;
// if(neighbours_pos_map.size() >0) no_of_robots
// =neighbours_pos_map.size()+1;
// buzz_utility::set_robot_var(no_of_robots);
@ -565,6 +616,9 @@ void roscontroller::prepare_msg_and_publish()
payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number;
/*Log out message id and message size*/
log<<(int)message_number<<",";
log<<(int)out[0]<<",";
/*publish prepared messages in respective topic*/
payload_pub.publish(payload_out);
delete[] out;