diff --git a/CMakeLists.txt b/CMakeLists.txt index abe0535..4d868f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,16 +5,8 @@ if(UNIX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") endif() -if(SIM) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1") -else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0") -endif() -if(KIN) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAVROSKINETIC=1") -else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAVROSKINETIC=0") -endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}") + ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 2edf7d3..d657797 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -97,7 +97,7 @@ function take_picture() { takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" - pic_time=0 + pic_time=0 } pic_time=pic_time+1 } @@ -142,7 +142,7 @@ function aggregate() { centroid = math.vec2.scale(centroid, 1.0 / neighbors.count()) cmdbin = math.vec2.sub(centroid, math.vec2.newp(3.0, id * 2 * math.pi / ROBOTS)) cmdbin = LimitSpeed(cmdbin, 1.0/2.0) - goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # circle all together around centroid @@ -164,7 +164,7 @@ function pursuit() { dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) vfg = LimitSpeed(vfg, 2.0) - goto_abs(vfg.x, vfg.y, 0.0, 0.0) + goto_abs(vfg.x, vfg.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude @@ -174,22 +174,18 @@ function lj_magnitude(dist, target, epsilon) { lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return lj } - + # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) } - + # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) { return math.vec2.add(data, accum) } - + # Calculates and actuates the flocking interaction -<<<<<<< HEAD -old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes -======= ->>>>>>> 064760108611591426d86c679c7789b1a95cebee function formation() { BVMSTATE="FORMATION" # Calculate accumulator @@ -205,7 +201,7 @@ function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 - BVMSTATE = "LAUNCH" + BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { flight.rc_cmd=0 @@ -308,7 +304,7 @@ function nei_cmd_listen() { # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto - # }) + # }) } } }) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 044228b..c2de02c 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -190,7 +190,7 @@ function pack_guide_msg(send_table){ else{ pon=1 } - var b=math.abs(send_table.Bearing) + var b=math.abs(send_table.Bearing) send_value=r_id*1000+pon*100+b return send_value } @@ -257,10 +257,10 @@ function LJ_vec(i){ var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) + cDir=math.vec2.newp(lj_magnitude(dis,target,EPSILON_GRAPH), ljbearing) } #log(id,"dis=",dis,"target=",target,"label=",nei_id) - #log("x=",cDir.x,"y=",cDir.y) + #log("x=",cDir.x,"y=",cDir.y) return cDir } # @@ -272,11 +272,11 @@ function motion_vector(){ while(i get_inmsg_vector(); -std::string getuavstate(); +std::string get_bvmstate(); int get_timesync_state(); diff --git a/include/rosbuzz/mavrosCC.h b/include/rosbuzz/mavrosCC.h index 1f84c29..7917bac 100644 --- a/include/rosbuzz/mavrosCC.h +++ b/include/rosbuzz/mavrosCC.h @@ -1,6 +1,6 @@ #pragma once -#ifdef MAVROSKINETIC +#if MAVROSKINETIC const short MISSION_START = mavros_msgs::CommandCode::MISSION_START; const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; diff --git a/include/roscontroller.h b/include/roscontroller.h index c157c38..d7bbbc5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,7 +35,6 @@ #include #include #include -#include #include "buzzuav_closures.h" #include "rosbuzz/mavrosCC.h" @@ -45,13 +44,13 @@ typedef enum { ROS_BUZZ_MSG_NIL = 0, // dummy msg UPDATER_MESSAGE, // Update msg - BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info + BUZZ_MESSAGE_NO_TIME, // Broadcast message wihout time info BUZZ_MESSAGE_TIME, // Broadcast message with time info } rosbuzz_msgtype; // Time sync algo. constants #define COM_DELAY 100000000 // in nano seconds i.e 100 ms -#define TIME_SYNC_JUMP_THR 500000000 +#define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 #define MAX_NUMBER_OF_ROBOTS 10 @@ -60,7 +59,7 @@ typedef enum { using namespace std; -namespace rosbzz_node +namespace rosbuzz_node { class roscontroller { @@ -98,6 +97,7 @@ private: }; typedef struct POSE ros_pose; + ros_pose target, home, cur_pos; struct MsgData { @@ -106,14 +106,12 @@ private: uint16_t size; double sent_time; uint64_t received_time; - MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): + MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){}; MsgData(){}; }; typedef struct MsgData msg_data; - ros_pose target, home, cur_pos; - uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; @@ -122,8 +120,8 @@ private: int robot_id = 0; ros::Time logical_clock; ros::Time previous_step_time; - std::vector inmsgdata; - uint64_t out_msg_time; + std::vector inmsgdata; + uint64_t out_msg_time; double logical_time_rate; bool time_sync_jumped; std::string robot_name = ""; @@ -133,7 +131,6 @@ private: int armstate; int barrier; int update; - int statepub_active; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; @@ -141,6 +138,7 @@ private: bool multi_msg; uint8_t no_cnt = 0; uint8_t old_val = 0; + bool debug = false; std::string bzzfile_name; std::string fcclient_name; std::string armclient; @@ -164,7 +162,7 @@ private: ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; - ros::Publisher uavstate_pub; + ros::Publisher bvmstate_pub; ros::Publisher grid_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; @@ -212,7 +210,7 @@ private: void neighbours_pos_publisher(); /*UAVState publisher*/ - void uavstate_publisher(); + void state_publisher(); /*Grid publisher*/ void grid_publisher(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index ebedb52..42b5f57 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -204,25 +204,25 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); @@ -258,25 +258,25 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); @@ -366,7 +366,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_ } // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); @@ -426,7 +426,7 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t } // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); buzzvm_gstore(VM); @@ -539,14 +539,6 @@ int update_step_test() return a == BUZZVM_STATE_READY; } -buzzvm_t get_vm() -/* -/ return the BVM -----------------*/ -{ - return VM; -} - void set_robot_var(int ROBOTS) /* / set swarm size in the BVM @@ -569,7 +561,15 @@ std::vector get_inmsg_vector(){ return IN_MSG; } -string getuavstate() +buzzvm_t get_vm() +/* +/ return the BVM +----------------*/ +{ + return VM; +} + +string get_bvmstate() /* / return current BVM state ---------------------------------------*/ @@ -579,12 +579,19 @@ string getuavstate() buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_gload(VM); buzzobj_t obj = buzzvm_stack_at(VM, 1); - uav_state = string(obj->s.value.str); + if(obj->o.type == BUZZTYPE_STRING) + uav_state = string(obj->s.value.str); + else + uav_state = "Not Available"; buzzvm_pop(VM); } return uav_state; } +int get_swarmsize() { + return (int)buzzdict_size(VM->swarmmembers) + 1; +} + int get_timesync_state() /* / return time sync state from bzz script diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 36d47a7..4c8b2a5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -105,10 +105,10 @@ float constrainAngle(float x) / Wrap the angle between -pi, pi ----------------------------------------------------------- */ { - x = fmod(x, 2 * M_PI); + x = fmod(x + M_PI, 2 * M_PI); if (x < 0.0) x += 2 * M_PI; - return x; + return x - M_PI; } void rb_from_gps(double nei[], double out[], double cur[]) @@ -229,8 +229,8 @@ int buzzuav_moveto(buzzvm_t vm) goto_pos[2] = height + dh; goto_pos[3] = dyaw; // DEBUG - ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], - goto_pos[1], goto_pos[2]); + // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], + // goto_pos[1], goto_pos[2]); buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? return buzzvm_ret0(vm); } @@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm) double rb[3]; rb_from_gps(goal, rb, cur_pos); - if (fabs(rb[0]) < 150.0) + if (fabs(rb[0]) < 150.0) { rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - - ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + } return buzzvm_ret0(vm); } @@ -675,7 +675,7 @@ void update_neighbors(buzzvm_t vm) } } -// Clear neighbours pos +// Clear neighbours pos void clear_neighbours_pos(){ neighbors_map.clear(); } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index b849cbd..3816f55 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -17,7 +17,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "rosBuzz"); ros::NodeHandle nh_priv("~"); ros::NodeHandle nh; - rosbzz_node::roscontroller RosController(nh, nh_priv); + rosbuzz_node::roscontroller RosController(nh, nh_priv); RosController.RosControllerRun(); return 0; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3b3fe58..4e1f2b8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -8,12 +8,11 @@ #include "roscontroller.h" #include -namespace rosbzz_node +namespace rosbuzz_node { const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; -const bool debug = true; -roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): logical_clock(ros::Time()), previous_step_time(ros::Time()) /* / roscontroller class Constructor @@ -132,23 +131,15 @@ void roscontroller::RosControllerRun() // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // check for BVMSTATE variable - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_gload(VM); - buzzobj_t obj = buzzvm_stack_at(VM, 1); - if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1; - else - { - statepub_active = 0; - ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled."); - } + if(buzz_utility::get_bvmstate()=="Not Available") + ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE."); // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { // Publish topics neighbours_pos_publisher(); - if(statepub_active) uavstate_publisher(); + state_publisher(); grid_publisher(); send_MPpayload(); // Check updater state and step code @@ -184,10 +175,6 @@ void roscontroller::RosControllerRun() << cur_pos.altitude * 100000 << ","; log << (int)no_of_robots<<","; log << neighbours_pos_map.size()<< ","; - log<<(int)inmsgdata.size()<<","; - log<< message_number<<","; - log<< out_msg_time<<","; - log < 0)log<<","; map::iterator it = neighbours_pos_map.begin(); @@ -202,7 +189,9 @@ void roscontroller::RosControllerRun() <<","<received_time; } inmsgdata.clear(); - log<(topic, "sensor_msgs/LaserScan")); node_handle.getParam("topics/battery", topic); - m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/BatteryState")); node_handle.getParam("topics/status", topic); m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); @@ -398,7 +395,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", MAX_NUMBER_OF_ROBOTS); - uavstate_pub = n_c.advertise("uavstate", 5); + bvmstate_pub = n_c.advertise("bvmstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); @@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c) { flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); } - else if (it->second == "mavros_msgs/BatteryStatus") + else if (it->second == "sensor_msgs/BatteryState") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } @@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher() neigh_pos_pub.publish(neigh_pos_array); } -void roscontroller::uavstate_publisher() +void roscontroller::state_publisher() /* / Publish current UAVState from Buzz script /----------------------------------------------------*/ { - std_msgs::String uavstate_msg; - uavstate_msg.data = buzz_utility::getuavstate(); - uavstate_pub.publish(uavstate_msg); + std_msgs::String state_msg; + state_msg.data = buzz_utility::get_bvmstate(); + bvmstate_pub.publish(state_msg); } void roscontroller::grid_publisher() @@ -619,7 +616,7 @@ with size......... | / rheader[0]=0; payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)message_number; - + if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; @@ -630,7 +627,7 @@ with size......... | / payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[2]); //payload_out.payload64.push_back((uint64_t)message_number); - // add time sync algo data + // add time sync algo data payload_out.payload64.push_back(ros::Time::now().toNSec()); payload_out.payload64.push_back(logical_clock.toNSec()); //uint64_t ltrate64 = 0; @@ -639,7 +636,7 @@ with size......... | / } else{ // prepare rosbuzz msg header - tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; + tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME; memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -873,10 +870,10 @@ float roscontroller::constrainAngle(float x) / Wrap the angle between -pi, pi ----------------------------------------------------------- */ { - x = fmod(x, 2 * M_PI); + x = fmod(x + M_PI, 2 * M_PI); if (x < 0.0) x += 2 * M_PI; - return x; + return x - M_PI; } void roscontroller::gps_rb(POSE nei_pos, double out[]) @@ -888,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[]) float ned_x = 0.0, ned_y = 0.0; gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); - // out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y, ned_x); - out[1] = constrainAngle(atan2(ned_y, ned_x)); - // out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } @@ -967,7 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt { cur_pos.x = msg->pose.position.x; cur_pos.y = msg->pose.position.y; - + buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead tf::Quaternion q( @@ -1099,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint16_t mtype = r16head[0]; uint16_t mid = r16head[1]; uint32_t temptime=0; - memcpy(&temptime, r16head+2, sizeof(uint32_t)); + memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; // if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid); // Check for Updater message, if updater message push it into updater FIFO @@ -1128,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) } // BVM FIFO message else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || - (int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) + (int)mtype == (int)BUZZ_MESSAGE_NO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1262,7 +1256,7 @@ void roscontroller::get_number_of_robots() / Garbage collector for the number of robots in the swarm --------------------------------------------------------------------------*/ { - int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; + int cur_robots = buzz_utility::get_swarmsize(); if (no_of_robots == 0) { no_of_robots = cur_robots; @@ -1442,7 +1436,7 @@ void roscontroller::get_xbee_status() void roscontroller::time_sync_step() /* - * Steps the time syncronization algorithm + * Steps the time syncronization algorithm ------------------------------------------------------------------ */ { //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); @@ -1454,13 +1448,13 @@ void roscontroller::time_sync_step() { avgRate += (it->second).relative_rate; // estimate current offset - int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; + int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; avgOffset = avgOffset + offset; offsetCount++; if((it->second).age > BUZZRATE){ neighbours_time_map.erase(it++); } - else{ + else{ (it->second).age++; ++it; } @@ -1478,10 +1472,10 @@ void roscontroller::time_sync_step() //neighbours_time_map.clear(); logical_time_rate = avgRate; -} +} void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) /* - * pushes a time syncronization msg into its data slot + * pushes a time syncronization msg into its data slot ------------------------------------------------------------------ */ { map::iterator it = neighbours_time_map.find(nid); @@ -1491,10 +1485,10 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; - relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - - ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", + + ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate); neighbours_time_map.erase(it); }