diff --git a/.gitignore b/.gitignore index 64f22eb..f426dc9 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ src/test* *.bo *.bdb *.bdbg +buzz_scripts/log* diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 4909ed8..f3f9550 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -77,4 +77,6 @@ int get_robotid(); buzzvm_t get_vm(); void set_robot_var(int ROBOTS); + +int get_inmsg_size(); } diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index a41c787..b519148 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -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 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 3d5531d..01d960a 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -786,4 +786,8 @@ int create_stig_tables() { buzzvm_pushi(VM, ROBOTS); buzzvm_gstore(VM); } + + int get_inmsg_size(){ + return IN_MSG.size(); + } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 639c9ea..0ddfd74 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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<::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<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<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;