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

logger addition

This commit is contained in:
vivek-shankar 2017-08-30 21:02:37 -04:00
parent 32c10dbc42
commit 397a6ccda9
5 changed files with 90 additions and 45 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*

41
buzz_scripts/graphform.bzz
View File

@ -4,9 +4,10 @@
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
include "barrier.bzz" # reserve stigmergy id=11 for this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
include "graphs/shapes_Y.bzz"
@ -88,7 +89,7 @@ m_fTargetDistanceTolerance=0
#step cunt
step_cunt=0
#virtual stigmergy
# virtual stigmergy for the LOCK barrier.
m_lockstig = 1
# Lennard-Jones parameters, may need change
@ -101,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){
return mag
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
return math.atan(v.y, v.x)
}
#
#return the number of value in table
#
@ -205,20 +188,6 @@ function find(table,value){
return index
}
function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
}
#
#pack message into 1 number
#
@ -654,7 +623,7 @@ function DoJoining(){
#the vector from self to target in global coordinate
var S2Target=math.vec2.add(S2Pred,P2Target)
#change the vector to local coordinate of self
var S2Target_bearing=Angle(S2Target)
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias

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();
}

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();
}
}

87
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);
}
@ -176,6 +181,7 @@ void roscontroller::send_MPpayload(){
MPpayload_pub.publish(buzzuav_closures::get_status());
}
/*-------------------------------------------------
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
@ -195,11 +201,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 +246,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 +259,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);
@ -337,7 +389,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
std::string battery_topic;
node_handle.getParam("topics/battery", battery_topic);
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryState"));
std::string status_topic;
node_handle.getParam("topics/status", status_topic);
@ -439,9 +491,9 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
m_smTopic_infos.begin();
it != m_smTopic_infos.end(); ++it) {
if (it->second == "mavros_msgs/ExtendedState") {
flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this);
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
} else if (it->second == "mavros_msgs/State") {
flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
} else if (it->second == "mavros_msgs/BatteryStatus") {
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
} else if (it->second == "sensor_msgs/NavSatFix") {
@ -459,17 +511,31 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
/-------------------------------------------------------*/
std::string roscontroller::Compile_bzz(std::string bzzfile_name)
{
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
// bzzfile_in_compile << path << "/";
// path = bzzfile_in_compile.str();
// bzzfile_in_compile.str("");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path
<< "include/";
<< "include/"; //<<" "<<path<< name<<".basm";
// bzzfile_in_compile.str("");
// bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo
// "<<path<<name<<".bdbg";
// system(bzzfile_in_compile.str().c_str());
// bzzfile_in_compile.str("");
bzzfile_in_compile << " -b " << path << name << ".bo";
// bcfname = bzzfile_in_compile.str();
// std::string tmp_bcfname = path + name + ".bo";
// bzzfile_in_compile.str("");
bzzfile_in_compile << " -d " << path << name << ".bdb ";
// bzzfile_in_compile << " -a " << path << name << ".asm ";
bzzfile_in_compile << bzzfile_name;
// std::string tmp_dbgfname = path + name + ".bdb";
ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
@ -565,6 +631,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;
@ -953,7 +1022,7 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) {
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if (mode_client.call(set_mode_message)) {
;//ROS_INFO("Set Mode Service call successful!");
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
@ -969,7 +1038,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
//ROS_INFO("Set stream rate call successful");
ROS_INFO("Set stream rate call successful");
}
/*-------------------------------------------------------------