From ed8a0a1ceea70699e9dbc8e0be6c1bb07e64c938 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 7 May 2018 19:01:41 -0400 Subject: [PATCH 01/98] files for LJ optim test and first working WebGUI for ROSBuzz --- buzz_scripts/testLJ.bzz | 61 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 buzz_scripts/testLJ.bzz diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100644 index 0000000..f9cfee5 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -0,0 +1,61 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +V_TYPE = 0 + +#State launched after takeoff +AUTO_LAUNCH_STATE = "FORMATION" + + +TARGET = 8.0 +EPSILON = 3.0 +GOTO_MAXVEL = 2.5 # m/steps + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="FORMATION") + statef=formation + + statef() + + log("Current state: ", BVMSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From c12b29538a2347679bf380a86dee2f5b3f76f21a Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 8 May 2018 18:53:10 -0400 Subject: [PATCH 02/98] fleet and stakeholder status on ROSBuzz web control --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cf54154..787d85c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -334,7 +334,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", 5); - uavstate_pub = n_c.advertise("uavstate", 5); + uavstate_pub = n_c.advertise("bvmstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); From 309f33fff1e4a67f462d9baeeb7738c27e02e5d8 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 16 May 2018 15:32:48 -0400 Subject: [PATCH 03/98] fixed rosbag name and auto-takeoff for optimization --- buzz_scripts/testLJ.bzz | 11 +++++++++++ 1 file changed, 11 insertions(+) mode change 100644 => 100755 buzz_scripts/testLJ.bzz diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz old mode 100644 new mode 100755 index f9cfee5..5240618 --- a/buzz_scripts/testLJ.bzz +++ b/buzz_scripts/testLJ.bzz @@ -24,6 +24,7 @@ function init() { # Starting state: TURNEDOFF to wait for user input. BVMSTATE = "TURNEDOFF" + TAKEOFF_COUNTER = 20 } # Executed at each time step. @@ -50,6 +51,16 @@ function step() { statef() log("Current state: ", BVMSTATE) + + # Auto-takeoff (delayed for simulator boot) + if(id == 0) { + if(TAKEOFF_COUNTER>0) + TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1 + else if(TAKEOFF_COUNTER == 0) { + BVMSTATE="LAUNCH" + TAKEOFF_COUNTER = -1 + } + } } # Executed once when the robot (or the simulator) is reset. From 44280c3d91dcf11ecb3607ac33bb774e647e0198 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 29 May 2018 19:04:39 +0000 Subject: [PATCH 04/98] initial commit, fix compile issues in kinetic, compatible with new mavros headers --- src/buzzuav_closures.cpp | 4 ++-- src/roscontroller.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 364f2f8..a9a06ff 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -400,7 +400,7 @@ int buzzuav_arm(buzzvm_t vm) / Buzz closure to arm /---------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); buzz_cmd = COMMAND_ARM; return buzzvm_ret0(vm); @@ -411,7 +411,7 @@ int buzzuav_disarm(buzzvm_t vm) / Buzz closure to disarm /---------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); buzz_cmd = COMMAND_DISARM; return buzzvm_ret0(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cf54154..aae209f 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -682,7 +682,7 @@ script } else ROS_ERROR("Failed to call service from flight controller"); - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -720,7 +720,7 @@ script cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -1094,8 +1094,8 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; armstate = req.param1; if (armstate) { @@ -1123,10 +1123,10 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: + case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; From f5a091f659cd579a142d49bfaa3595536a47b28a Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Tue, 29 May 2018 19:57:28 +0000 Subject: [PATCH 05/98] fix batterystate msgs type and launch file for TX1 --- include/roscontroller.h | 4 ++-- src/roscontroller.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 7599c58..2972cc1 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -11,10 +11,10 @@ #include "mavros_msgs/ExtendedState.h" #include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" -#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" +#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/WaypointPush.h" #include "mavros_msgs/Waypoint.h" #include "mavros_msgs/PositionTarget.h" @@ -205,7 +205,7 @@ private: double gps_r_lat); /*battery status callback */ - void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); + void battery(const sensor_msgs::BatteryState::ConstPtr& msg); /*flight extended status callback*/ void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index aae209f..e46b189 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -271,7 +271,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) m_smTopic_infos.insert(pair(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")); @@ -367,7 +367,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); } @@ -836,12 +836,12 @@ void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); }; -void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) +void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg) /* / Update battery status into BVM from subscriber /------------------------------------------------------*/ { - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining); + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage*100.0); // DEBUG // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // msg->current, msg ->remaining); From db452bd063f255db761d1cb086353dc4326fd7b5 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 1 Jun 2018 13:10:59 -0400 Subject: [PATCH 06/98] added states to webcontrol and enhance barrier for state machine --- buzz_scripts/include/act/barrier.bzz | 37 +++++++---- buzz_scripts/include/act/states.bzz | 98 ++++++++++++++++------------ buzz_scripts/main.bzz | 2 +- 3 files changed, 82 insertions(+), 55 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..23e87d7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -7,10 +7,11 @@ # # Constants # -BARRIER_TIMEOUT = 1200 # in steps +BARRIER_TIMEOUT = 200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil +bc = 0; # # Sets a barrier @@ -22,15 +23,15 @@ function barrier_create() { #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) if(barrier!=nil) { barrier=nil - BARRIER_VSTIG = BARRIER_VSTIG +1 + # BARRIER_VSTIG = BARRIER_VSTIG +1 } #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef) { +function barrier_set(threshold, transf, resumef, bc) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bc); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -39,30 +40,42 @@ function barrier_set(threshold, transf, resumef) { # # Make yourself ready # -function barrier_ready() { +function barrier_ready(bc) { #log("BARRIER READY -------") - barrier.put(id, 1) + barrier.put(id, bc) barrier.put("d", 0) } # # Executes the barrier # -function barrier_wait(threshold, transf, resumef) { - barrier.put(id, 1) - +function barrier_wait(threshold, transf, resumef, bc) { + barrier.put(id, bc) barrier.get(id) - #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + var allgood = 0 + + log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + var bi = 0 + allgood = 1 + while (bi= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") - barrier=nil + barrier = nil timeW = 0 BVMSTATE = resumef - } + } else if(timeW % 10 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) timeW = timeW+1 } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9668295..cc57742 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -32,31 +32,31 @@ function launch() { if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) + barrier_ready(22) } else { log("Altitude: ", pose.position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) + barrier_ready(22) } } function stop() { - BVMSTATE = "STOP" + BVMSTATE = "STOP" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) } } else { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) } } @@ -117,7 +117,7 @@ function aggregate() { # circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" - rd = 15.0 + rd = 20.0 pc = 3.2 vd = 15.0 r_vec = neighbors.reduce(function(rid, data, r_vec) { @@ -137,9 +137,9 @@ function pursuit() { # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 3.0 +EPSILON = 0.1 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2) return lj } @@ -161,12 +161,12 @@ function formation() { accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) - old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) + old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0) goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } function rc_cmd_listen() { - if(flight.rc_cmd==22) { + if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 BVMSTATE = "LAUNCH" @@ -192,22 +192,26 @@ function rc_cmd_listen() { stattab_send() } else if (flight.rc_cmd==900){ flight.rc_cmd=0 - BVMSTATE = "TASK_ALLOCATE" + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "PURSUIT" + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "AGGREGATE" + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "FORMATION" + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + barrier_ready(903) neighbors.broadcast("cmd", 903) } } @@ -216,30 +220,40 @@ function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "LAUNCH" - } else if(value==21 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "STOP" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ # Shapes - BVMSTATE = "TASK_ALLOCATE" - } else if(value==901){ # Pursuit - destroyGraph() - BVMSTATE = "PURSUIT" - } else if(value==902){ # Agreggate - destroyGraph() - BVMSTATE = "AGGREGATE" - } else if(value==903){ # Formation - destroyGraph() - BVMSTATE = "FORMATION" - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) + if(BVMSTATE!="BARRIERWAIT") { + if(value==22) { + BVMSTATE = "LAUNCH" + } else if(value==21) { + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + uav_arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + uav_disarm() + } else if(value==900){ # Shapes + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + #barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if(value==901){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + #barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if(value==902){ # Agreggate + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + #barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if(value==903){ # Formation + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + #barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if(value==16 and BVMSTATE=="IDLE"){ + # 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/main.bzz b/buzz_scripts/main.bzz index e87f015..f560857 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "IDLE" ##### # Vehicule type: From 42d990676c9c7687d0f2e4bab304481b8983fa37 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 1 Jun 2018 20:12:06 -0400 Subject: [PATCH 07/98] bringup debug mode to launch file, fine tuned LJ params and rename hooks (generic hardware) --- buzz_scripts/include/act/states.bzz | 19 +++++++++++++------ buzz_scripts/main.bzz | 2 +- include/roscontroller.h | 1 + launch/rosbuzz.launch | 1 + launch/rosbuzzd.launch | 1 + src/buzz_utility.cpp | 12 ++++++------ src/buzzuav_closures.cpp | 4 ++-- src/roscontroller.cpp | 19 ++++++++++++------- 8 files changed, 37 insertions(+), 22 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index cc57742..55a10c8 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -45,8 +45,13 @@ function launch() { } } +gohomeT=100 function stop() { BVMSTATE = "STOP" + if(gohomeT > 0) { # TODO: Make a real check if home is reached + gohome() + gohomeT = gohomeT - 1 + } if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() @@ -64,7 +69,7 @@ function take_picture() { BVMSTATE="PICTURE" uav_setgimbal(0.0, 0.0, -90.0, 20.0) if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() + takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" pic_time=0 @@ -137,9 +142,10 @@ function pursuit() { # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 0.1 +EPSILON = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + log(target, dist, epsilon, lj) return lj } @@ -154,17 +160,17 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction -old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) - old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0) - goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) + accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# listens to commands from the remote control (web, commandline, etc) function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") @@ -216,6 +222,7 @@ function rc_cmd_listen() { } } +# listens to other fleet members broadcasting commands function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index f560857..7565281 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "IDLE" +AUTO_LAUNCH_STATE = "FORMATION" ##### # Vehicule type: diff --git a/include/roscontroller.h b/include/roscontroller.h index 2972cc1..dedc737 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -103,6 +103,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; diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index c2531f4..9762a3f 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -10,6 +10,7 @@ + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index de7857d..9dc5b90 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -10,6 +10,7 @@ + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 507e70c..5a5948f 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)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a9a06ff..ae0e2f9 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -104,10 +104,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[]) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 15efa1f..460e7cf 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -11,7 +11,6 @@ namespace rosbzz_node { const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; -const bool debug = false; roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) /* @@ -188,6 +187,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } + // Obtain debug mode from launch file parameter + if (n_c.getParam("debug", debug)) + ; + else + { + ROS_ERROR("Provide a debug mode in Launch file"); + system("rosnode kill rosbuzz_node"); + } // Obtain rc service option from parameter server if (n_c.getParam("rcclient", rcclient)) { @@ -670,7 +677,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! + case buzzuav_closures::COMMAND_GOTO: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param6 = goto_pos[1]; @@ -794,10 +801,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[]) @@ -809,10 +816,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; } @@ -1055,6 +1059,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; + // Compute RB in robot body ref. frame gps_rb(nei_pos, cvt_neighbours_pos_payload); // Extract robot id of the neighbour uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); From 845742767b7bfb9fa738692aad905cd3cf174663 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 1 Jun 2018 21:20:57 -0400 Subject: [PATCH 08/98] reinstate gohome in sim and webcontrol --- buzz_scripts/include/act/states.bzz | 39 ++++++++++++++++++-------- buzz_scripts/include/plan/rrtstar.bzz | 2 +- buzz_scripts/main.bzz | 2 ++ include/buzzuav_closures.h | 13 --------- launch/rosbuzz.launch | 2 +- src/buzzuav_closures.cpp | 16 +++++------ src/roscontroller.cpp | 40 ++++++--------------------- 7 files changed, 48 insertions(+), 66 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 55a10c8..d048f41 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -46,12 +46,18 @@ function launch() { } gohomeT=100 -function stop() { - BVMSTATE = "STOP" +function goinghome() { + BVMSTATE = "GOHOME" if(gohomeT > 0) { # TODO: Make a real check if home is reached gohome() gohomeT = gohomeT - 1 - } + neighbors.broadcast("cmd", 20) + } else + BVMSTATE = AUTO_LAUNCH_STATE +} + +function stop() { + BVMSTATE = "STOP" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() @@ -62,12 +68,12 @@ function stop() { } else { barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) barrier_ready(21) - } + } } function take_picture() { BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) + setgimbal(0.0, 0.0, -90.0, 20.0) if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture @@ -180,18 +186,25 @@ function rc_cmd_listen() { } else if(flight.rc_cmd==21) { log("cmd 21") flight.rc_cmd=0 - BVMSTATE = "STOP" + AUTO_LAUNCH_STATE = "STOP" + BVMSTATE = "GOHOME" neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + log("cmd 20") + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + neighbors.broadcast("cmd", 20) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - uav_arm() + arm() neighbors.broadcast("cmd", 400) } else if (flight.rc_cmd==401){ flight.rc_cmd=0 - uav_disarm() + disarm() neighbors.broadcast("cmd", 401) } else if (flight.rc_cmd==666){ flight.rc_cmd=0 @@ -230,12 +243,16 @@ function nei_cmd_listen() { if(BVMSTATE!="BARRIERWAIT") { if(value==22) { BVMSTATE = "LAUNCH" + }else if(value==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" } else if(value==21) { - BVMSTATE = "STOP" + AUTO_LAUNCH_STATE = "STOP" + BVMSTATE = "GOHOME" } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() + arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() + disarm() } else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 54c69ab..a55aff1 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -20,7 +20,7 @@ mapgoal = {} function navigate() { BVMSTATE="NAVIGATE" if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7565281..d5fea90 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -53,6 +53,8 @@ function step() { statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE statef=launch + else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE + statef=goinghome else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="AGGREGATE") diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e6c3f48..f1eda2f 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -13,19 +13,6 @@ namespace buzzuav_closures { -typedef enum { - COMMAND_NIL = 0, // Dummy command - COMMAND_TAKEOFF, // Take off - COMMAND_LAND, - COMMAND_GOHOME, - COMMAND_ARM, - COMMAND_DISARM, - COMMAND_GOTO, - COMMAND_MOVETO, - COMMAND_PICTURE, - COMMAND_GIMBAL, -} Custom_CommandCode; - /* * prextern int() function in Buzz * This function is used to print data from buzz diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index 9762a3f..cd0de8d 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -10,7 +10,7 @@ - + diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ae0e2f9..f80bebb 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -230,7 +230,7 @@ int buzzuav_moveto(buzzvm_t vm) // 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]); - buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? + buzz_cmd = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT; return buzzvm_ret0(vm); } @@ -331,7 +331,7 @@ int buzzuav_takepicture(buzzvm_t vm) / Buzz closure to take a picture here. /----------------------------------------*/ { - buzz_cmd = COMMAND_PICTURE; + buzz_cmd = mavros_msgs::CommandCode::IMAGE_START_CAPTURE; return buzzvm_ret0(vm); } @@ -355,7 +355,7 @@ int buzzuav_setgimbal(buzzvm_t vm) rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); - buzz_cmd = COMMAND_GIMBAL; + buzz_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; return buzzvm_ret0(vm); } @@ -402,7 +402,7 @@ int buzzuav_arm(buzzvm_t vm) { cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); - buzz_cmd = COMMAND_ARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -413,7 +413,7 @@ int buzzuav_disarm(buzzvm_t vm) { cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); - buzz_cmd = COMMAND_DISARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -429,7 +429,7 @@ int buzzuav_takeoff(buzzvm_t vm) height = goto_pos[2]; cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); - buzz_cmd = COMMAND_TAKEOFF; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -440,7 +440,7 @@ int buzzuav_land(buzzvm_t vm) { cur_cmd = mavros_msgs::CommandCode::NAV_LAND; printf(" Buzz requested Land !!! \n"); - buzz_cmd = COMMAND_LAND; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -451,7 +451,7 @@ int buzzuav_gohome(buzzvm_t vm) { cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); - buzz_cmd = COMMAND_GOHOME; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 460e7cf..8f25de5 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -622,7 +622,7 @@ script float* gimbal; switch (buzzuav_closures::bzz_cmd()) { - case buzzuav_closures::COMMAND_TAKEOFF: + case mavros_msgs::CommandCode::NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); @@ -645,7 +645,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_LAND: + case mavros_msgs::CommandCode::NAV_LAND: cmd_srv.request.command = buzzuav_closures::getcmd(); if (current_mode != "LAND") { @@ -664,10 +664,7 @@ script armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - cmd_srv.request.param5 = home.latitude; - cmd_srv.request.param6 = home.longitude; - cmd_srv.request.param7 = home.altitude; + case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY TESTED HITL/FIELD !!! cmd_srv.request.command = buzzuav_closures::getcmd(); if (mav_client.call(cmd_srv)) { @@ -677,28 +674,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_GOTO: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; - - case buzzuav_closures::COMMAND_ARM: + case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: if (!armstate) { SetMode("LOITER", 0); @@ -707,7 +683,7 @@ script } break; - case buzzuav_closures::COMMAND_DISARM: + case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM+1: if (armstate) { armstate = 0; @@ -716,12 +692,12 @@ script } break; - case buzzuav_closures::COMMAND_MOVETO: + case mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT: goto_pos = buzzuav_closures::getgoto(); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; - case buzzuav_closures::COMMAND_GIMBAL: + case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: gimbal = buzzuav_closures::getgimbal(); cmd_srv.request.param1 = gimbal[0]; cmd_srv.request.param2 = gimbal[1]; @@ -736,7 +712,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_PICTURE: + case mavros_msgs::CommandCode::IMAGE_START_CAPTURE: ROS_INFO("TAKING A PICTURE HERE!! --------------"); mavros_msgs::CommandBool capture_command; if (capture_srv.call(capture_command)) From 6aa43f20bc751ea2dc2c957f517ead32a9d7bbe1 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 5 Jun 2018 17:22:25 -0400 Subject: [PATCH 09/98] added buzz script editor to webcontrol --- buzz_scripts/include/act/states.bzz | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d048f41..a082cb7 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -51,7 +51,6 @@ function goinghome() { if(gohomeT > 0) { # TODO: Make a real check if home is reached gohome() gohomeT = gohomeT - 1 - neighbors.broadcast("cmd", 20) } else BVMSTATE = AUTO_LAUNCH_STATE } @@ -151,7 +150,6 @@ TARGET = 8.0 EPSILON = 30.0 function lj_magnitude(dist, target, epsilon) { lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) - log(target, dist, epsilon, lj) return lj } @@ -184,16 +182,17 @@ function rc_cmd_listen() { BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { - log("cmd 21") flight.rc_cmd=0 AUTO_LAUNCH_STATE = "STOP" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + barrier_ready(21) BVMSTATE = "GOHOME" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==20) { - log("cmd 20") flight.rc_cmd=0 AUTO_LAUNCH_STATE = "IDLE" - BVMSTATE = "GOHOME" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) neighbors.broadcast("cmd", 20) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 From 3ad277bd5ec98c928ff573d41f482c5021e8e3d9 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 6 Jun 2018 12:40:24 -0400 Subject: [PATCH 10/98] added script list menu and fix rostopic list to webcontrol --- buzz_scripts/main.bzz | 2 ++ 1 file changed, 2 insertions(+) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d5fea90..e4a33b3 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -8,6 +8,8 @@ include "vstigenv.bzz" #State launched after takeoff AUTO_LAUNCH_STATE = "FORMATION" +TARGET = 9.0 +EPSILON = 30.0 ##### # Vehicule type: From 58c09093a77202ad289f82fbabbf240688da05c6 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 6 Jun 2018 22:40:26 -0400 Subject: [PATCH 11/98] Fixed the update crash caused by BVM state publisher. --- buzz_scripts/include/update.bzz | 2 +- buzz_scripts/minimal.bzz | 1 - buzz_scripts/stand_by.bzz | 48 ++++++++++++++++++--------------- include/buzz_update.h | 36 +++++++++++++------------ include/buzz_utility.h | 2 ++ include/buzzuav_closures.h | 4 --- src/buzz_update.cpp | 27 +++++-------------- src/buzz_utility.cpp | 23 +++++++++++++--- src/buzzuav_closures.cpp | 13 --------- src/roscontroller.cpp | 11 ++++---- 10 files changed, 82 insertions(+), 85 deletions(-) diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index f49fd7c..8d64de2 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,6 @@ #################################################################################################### updated="update_ack" update_no=0 -function updated_neigh(){ +function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz index 8f0494b..a49329b 100644 --- a/buzz_scripts/minimal.bzz +++ b/buzz_scripts/minimal.bzz @@ -51,7 +51,6 @@ function step() { statef=action statef() - log("Current state: ", BVMSTATE) } diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1349089..1b30b06 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -1,35 +1,41 @@ +include "act/states.bzz" +include "vstigenv.bzz" + updated="update_ack" update_no=0 -function init(){ -barrier = stigmergy.create(101) -barrier.put(id,1) -barrier_val=0 -barrier.onconflict(function (k,l,r) { -if(r. data < l. data or (r. data == l. data )) return l -else return r -}) +BVMSTATE = "UPDATESTANDBY" -s = swarm.create(1) -s.join() +# Executed once at init time. +function init(){ + barrier = stigmergy.create(101) + barrier.put(id,1) + barrier_val=0 + barrier.onconflict(function (k,l,r) { + if(r. data < l. data or (r. data == l. data )) return l + else return r + }) + + BVMSTATE = "UPDATESTANDBY" + init_swarm() + # start the swarm command listener + nei_cmd_listen() } function stand_by(){ + barrier.get(id) + barrier_val = barrier.size() -barrier.get(id) -barrier_val = barrier.size() - -neighbors.listen(updated, - function(vid, value, rid) { - print(" got from",vid," ", " value = ",value," ","rid"," " ) - if(value==update_no) barrier.put(rid,1) - } + neighbors.listen(updated, + function(vid, value, rid) { + print(" got from",vid," ", " value = ",value," ","rid"," " ) + if(value==update_no) barrier.put(rid,1) + } ) } - +# Executed at each time step. function step() { - -stand_by() + stand_by() } diff --git a/include/buzz_update.h b/include/buzz_update.h index 5680f85..b34f671 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,7 +1,5 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H -/*Simulation or robot check*/ -//#define SIMULATION 1 // set in CMAKELIST #include #include @@ -112,31 +110,30 @@ void code_message_inqueue_append(uint8_t* msg, uint16_t size); void code_message_inqueue_process(); /*****************************************************/ -/* obtains messages from out msgs queue of the updater*/ +/*Obtains messages from out msgs queue of the updater*/ /*******************************************************/ uint8_t* getupdater_out_msg(); /******************************************************/ -/*obtains out msg queue size*/ +/*Obtains out msg queue size*/ /*****************************************************/ uint8_t* getupdate_out_msg_size(); /**************************************************/ -/*destroys the out msg queue*/ +/*Destroys the out msg queue*/ /*************************************************/ void destroy_out_msg_queue(); -/***************************************************/ -/*obatins updater state*/ -/***************************************************/ -// int get_update_mode(); - // buzz_updater_elem_t get_updater(); /***************************************************/ -/*sets bzz file name*/ +/*Sets bzz file name*/ /***************************************************/ void set_bzz_file(const char* in_bzz_file); +/****************************************************/ +/*Tests the code from a buffer*/ +/***************************************************/ + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); /****************************************************/ @@ -145,17 +142,22 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); void destroy_updater(); +/****************************************************/ +/*Checks for updater message*/ +/***************************************************/ + int is_msg_present(); -int get_update_status(); - -void set_read_update_status(); +/****************************************************/ +/*Compiles a bzz script to bo and bdbg*/ +/***************************************************/ int compile_bzz(std::string bzz_file); +/****************************************************/ +/*Set number of robots in the updater*/ +/***************************************************/ + void updates_set_robots(int robots); -// void set_packet_id(int packet_id); - -// void collect_data(std::ofstream& logger); #endif diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 12cf011..600b9e2 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -77,4 +77,6 @@ buzzvm_t get_vm(); void set_robot_var(int ROBOTS); int get_inmsg_size(); + +std::string getuavstate(); } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f1eda2f..6e31ec6 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -85,10 +85,6 @@ double* getgoto(); * returns the current grid */ std::map> getgrid(); -/* - * returns the current Buzz state - */ -std::string getuavstate(); /* * returns the gimbal commands */ diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 1d169f9..996412a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -45,7 +45,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script, c { perror("inotify_init error"); } - /*If simulation set the file monitor only for robot 1*/ + /*If simulation set the file monitor only for robot 0*/ if (SIMULATION == 1 && robot_id == 0) { /* watch /.bzz file for any activity and report it back to update */ @@ -321,6 +321,7 @@ void code_message_inqueue_process() // fprintf(stdout,"[debug]Inside inmsg code running"); if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) { + updated = 1; size += sizeof(uint8_t); if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) { @@ -413,9 +414,6 @@ void create_update_patch() fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); system(genpatch.str().c_str()); - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - /* delete old files & rename new files */ remove((path + name1 + ".bo").c_str()); @@ -460,7 +458,7 @@ void update_routine() // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); if (*(int*)updater->mode == CODE_RUNNING) { - buzzvm_function_call(VM, "updated_neigh", 0); + buzzvm_function_call(VM, "updated_no_bct", 0); // Check for update if (check_update()) { @@ -479,7 +477,7 @@ void update_routine() name = name.substr(0, name.find_last_of(".")); bzzfile_in_compile << path << name << "-update.bo"; uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); if (!fp) { perror(bzzfile_in_compile.str().c_str()); @@ -528,7 +526,6 @@ void update_routine() // collect_data(); buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); // buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated = 1; update_try_counter = 0; timer_steps = 0; } @@ -538,7 +535,6 @@ void update_routine() /*Time out hit deceided to roll back*/ *(int*)(updater->mode) = CODE_RUNNING; buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - updated = 1; update_try_counter = 0; timer_steps = 0; } @@ -580,6 +576,9 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1)); + buzzvm_gstore(VM); return 1; } /*Unable to step something wrong*/ @@ -634,18 +633,6 @@ void destroy_out_msg_queue() delete_p(updater->outmsg_queue); updater_msg_ready = 0; } -int get_update_status() -{ - return updated; -} -void set_read_update_status() -{ - updated = 0; -} -/*int get_update_mode() -{ - return (int)*(int*)(updater->mode); -}*/ int is_msg_present() { diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 5a5948f..8a03316 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -351,17 +351,17 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_ // Set byte code if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { + ROS_ERROR("[%i] %s: Error loading Buzz bytecode, VM state : %i", Robot_id, VM->state); buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); return 0; } // Register hook functions if (buzz_register_hooks() != BUZZVM_STATE_READY) { + ROS_ERROR("[%i] Error registering hooks, VM state : %i", Robot_id, VM->state); buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); return 0; } @@ -379,7 +379,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_ // Call the Init() function if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) { - ROS_ERROR("Error in calling init, VM state : %i", VM->state); + ROS_ERROR("Error in calling init, VM state : %i", VM->state); return 0; } // All OK @@ -564,4 +564,21 @@ int get_inmsg_size() { return IN_MSG.size(); } + +string getuavstate() +/* +/ return current BVM state +---------------------------------------*/ +{ + std::string uav_state = "Unknown"; + if(VM && VM->strings){ + 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); + buzzvm_pop(VM); + } + return uav_state; +} + } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index f80bebb..abff74f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -479,19 +479,6 @@ float* getgimbal() return rc_gimbal; } -string getuavstate() -/* -/ return current BVM state ----------------------------------------*/ -{ - static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_gload(VM); - buzzobj_t uav_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - return uav_state->s.value.str; -} - int getcmd() /* / return current mavros command to the FC diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 8f25de5..bd8fbde 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -120,6 +120,7 @@ void roscontroller::RosControllerRun() send_MPpayload(); // Check updater state and step code update_routine(); + ROS_WARN("OUT OF UPDATE ROUTINE"); if (time_step == BUZZRATE) { time_step = 0; @@ -144,10 +145,10 @@ void roscontroller::RosControllerRun() // Call the flight controler service flight_controller_service_call(); // Set multi message available after update - if (get_update_status()) - { - set_read_update_status(); - } + //if (get_update_status()) + //{ + // set_read_update_status(); + //} // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); @@ -455,7 +456,7 @@ void roscontroller::uavstate_publisher() /----------------------------------------------------*/ { std_msgs::String uavstate_msg; - uavstate_msg.data = buzzuav_closures::getuavstate(); + uavstate_msg.data = buzz_utility::getuavstate(); uavstate_pub.publish(uavstate_msg); } From e953de61916dd95fb5937193646a7fff34fd2c94 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 6 Jun 2018 22:55:49 -0400 Subject: [PATCH 12/98] added a new namespace for update functions --- include/buzz_update.h | 250 ++++---- src/buzz_update.cpp | 1264 +++++++++++++++++++++-------------------- src/roscontroller.cpp | 20 +- 3 files changed, 772 insertions(+), 762 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index b34f671..1849b56 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -3,161 +3,169 @@ #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include + #define delete_p(p) \ do \ { \ free(p); \ p = NULL; \ } while (0) - -static const uint16_t CODE_REQUEST_PADDING = 250; -static const uint16_t MIN_UPDATE_PACKET = 251; -static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; -static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; -/*********************/ -/* Updater states */ -/********************/ - -typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update -} code_states_e; - -/*********************/ -/*Message types */ -/********************/ - -typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request -} code_message_e; - -/*************************/ -/*Updater message queue */ -/*************************/ - -struct updater_msgqueue_s +namespace buzz_update { - uint8_t* queue; - uint8_t* size; -}; -typedef struct updater_msgqueue_s* updater_msgqueue_t; + static const uint16_t CODE_REQUEST_PADDING = 250; + static const uint16_t MIN_UPDATE_PACKET = 251; + static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; + static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; + /*********************/ + /* Updater states */ + /********************/ -struct updater_code_s -{ - uint8_t* bcode; - uint8_t* bcode_size; -}; -typedef struct updater_code_s* updater_code_t; + typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update + } code_states_e; -/**************************/ -/*Updater data*/ -/**************************/ + /*********************/ + /*Message types */ + /********************/ -struct buzz_updater_elem_s -{ - /* robot id */ - // uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; -}; -typedef struct buzz_updater_elem_s* buzz_updater_elem_t; + typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request + } code_message_e; -/**************************************************************************/ -/*Updater routine from msg processing to file checks to be called from main*/ -/**************************************************************************/ -void update_routine(); + /*************************/ + /*Updater message queue */ + /*************************/ -/************************************************/ -/*Initalizes the updater */ -/************************************************/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + struct updater_msgqueue_s + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; -/*********************************************************/ -/*Appends buffer of given size to in msg queue of updater*/ -/*********************************************************/ + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; -void code_message_inqueue_append(uint8_t* msg, uint16_t size); + /**************************/ + /*Updater data*/ + /**************************/ -/*********************************************************/ -/*Processes messages inside the queue of the updater*/ -/*********************************************************/ + struct buzz_updater_elem_s + { + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; + }; + typedef struct buzz_updater_elem_s* buzz_updater_elem_t; -void code_message_inqueue_process(); + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); -/*****************************************************/ -/*Obtains messages from out msgs queue of the updater*/ -/*******************************************************/ -uint8_t* getupdater_out_msg(); + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); -/******************************************************/ -/*Obtains out msg queue size*/ -/*****************************************************/ -uint8_t* getupdate_out_msg_size(); + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ -/**************************************************/ -/*Destroys the out msg queue*/ -/*************************************************/ -void destroy_out_msg_queue(); + void code_message_inqueue_append(uint8_t* msg, uint16_t size); -// buzz_updater_elem_t get_updater(); -/***************************************************/ -/*Sets bzz file name*/ -/***************************************************/ -void set_bzz_file(const char* in_bzz_file); + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ -/****************************************************/ -/*Tests the code from a buffer*/ -/***************************************************/ + void code_message_inqueue_process(); -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); -/****************************************************/ -/*Destroys the updater*/ -/***************************************************/ + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); -void destroy_updater(); + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); -/****************************************************/ -/*Checks for updater message*/ -/***************************************************/ + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file); -int is_msg_present(); + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ -/****************************************************/ -/*Compiles a bzz script to bo and bdbg*/ -/***************************************************/ + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); -int compile_bzz(std::string bzz_file); + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ -/****************************************************/ -/*Set number of robots in the updater*/ -/***************************************************/ + void destroy_updater(); -void updates_set_robots(int robots); + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ + int is_msg_present(); + + /****************************************************/ + /*Compiles a bzz script to bo and bdbg*/ + /***************************************************/ + + int compile_bzz(std::string bzz_file); + + /****************************************************/ + /*Set number of robots in the updater*/ + /***************************************************/ + + void updates_set_robots(int robots); +} #endif diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 996412a..210e98f 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,598 +1,620 @@ -#include -#include -#include -#include -#include +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "buzz_update.h" -#include -#include -#include -#include -/*Temp for data collection*/ -// static int neigh=-1; -static struct timeval t1, t2; -static int timer_steps = 0; -// static clock_t end; +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; -/*Temp end */ -static int fd, wd = 0; -static int old_update = 0; -static buzz_updater_elem_t updater; -static int no_of_robot; -static const char* dbgf_name; -static const char* bcfname; -static const char* old_bcfname = "old_bcode.bo"; -static const char* bzz_file; -static int Robot_id = 0; -static int neigh = -1; -static int updater_msg_ready; -static uint16_t update_try_counter = 0; -static int updated = 0; -static const uint16_t MAX_UPDATE_TRY = 5; -static int packet_id_ = 0; -static size_t old_byte_code_size = 0; + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static int updated = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static int packet_id_ = 0; + static size_t old_byte_code_size = 0; -/*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) -{ - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) + /*Initialize updater*/ + void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 0*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; - - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); -} -/*Check for .bzz file chages*/ -int check_update() -{ - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - while (i < len) - { - struct inotify_event* event = (struct inotify_event*)&buf[i]; - /* file was modified this flag is true in nano and self delet in gedit and other editors */ - // fprintf(stdout,"inside file monitor.\n"); - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + buzzvm_t VM = buzz_utility::get_vm(); + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_INFO("Initializing file monitor..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) { - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); - fd = inotify_init1(IN_NONBLOCK); + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ - if (!old_update) + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; + + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + } + /*Check for .bzz file chages*/ + int check_update() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /* file was modified this flag is true in nano and self delet in gedit and other editors */ + // fprintf(stdout,"inside file monitor.\n"); + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) { - check = 1; - old_update = 1; + /*respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /* To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /* update index to start of next event */ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + std::stringstream patch_exsisting; + patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 + << "tmp_patch.bo"; + fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + uint8_t* patched_BO_Buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_BO_Buf = (uint8_t*)malloc(patched_size); + if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_BO_Buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + uint8_t* patched_BO_Buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_BO_Buf = (uint8_t*)malloc(patched_size); + if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_BO_Buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); + } + + void code_message_inqueue_append(uint8_t* msg, uint16_t size) + { + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_INFO("[Debug] Updater received patch of size %u \n", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + updated = 1; + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + printf("TEST PASSED!\n"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } } - /* update index to start of next event */ - i += sizeof(struct inotify_event) + event->len; - } - if (!check) - old_update = 0; - return check; -} - -int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) -{ - if (SIMULATION == 1) - { - return 1; - } - else - { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ - FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); - fwrite(patch, update_patch_size, 1, fp); - fclose(fp); - std::stringstream patch_exsisting; - patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 - << "tmp_patch.bo"; - fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); - if (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; - } -} - -updater_code_t obtain_patched_bo(std::string& path, std::string& name1) -{ - if (SIMULATION == 1) - { - /*Read the exsisting file to simulate the patching*/ - std::stringstream read_patched; - read_patched << path << name1 << ".bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched << path << name1 << "-patched.bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } -} - -void code_message_outqueue_append() -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; -} - -void outqueue_append_code_request(uint16_t update_no) -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; - updater_msg_ready = 1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); -} - -void code_message_inqueue_append(uint8_t* msg, uint16_t size) -{ - updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; -} -/*Used for data logging*/ -/*void set_packet_id(int packet_id) -{ - packet_id_ = packet_id; -}*/ -void code_message_inqueue_process() -{ - int size = 0; - updater_code_t out_code = NULL; - - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - - if (*(int*)(updater->mode) == CODE_RUNNING) - { - // fprintf(stdout,"[debug]Inside inmsg code running"); - if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) - { - updated = 1; size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - out_code = obtain_patched_bo(path, name1); + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + ROS_ERROR("Code appended! update try : %u \n", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } - // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - // FILE *fp; - // fp=fopen("update.bo", "wb"); - // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - // fclose(fp); + void create_update_patch() + { + std::stringstream genpatch; - if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) - { - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + // CALL BSDIFF CMD + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /* delete old files & rename new files */ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \nUpdating script ...\n"); + + if (compile_bzz(bzz_file)) + { + ROS_WARN("Errors in comipilg script so staying on old script\n"); } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + ROS_INFO("Sending code... \n"); + code_message_outqueue_append(); + } + delete_p(BO_BUF); } } } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + + else { - size += sizeof(uint16_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ..................... %i \n", tObj->i.value); + if (tObj->i.value == no_of_robot) { - update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); - code_message_outqueue_append(); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, decided to roll back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); } } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); -} -void create_update_patch() -{ - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-update"; - - // CALL BSDIFF CMD - genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); - - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); - if (!fp) + uint8_t* getupdater_out_msg() { - perror(patchfileName.str().c_str()); + return (uint8_t*)updater->outmsg_queue->queue; } - fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); - rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + + uint8_t* getupdate_out_msg_size() { - perror(patchfileName.str().c_str()); + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; } - fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); -} - -void update_routine() -{ - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if (*(int*)updater->mode == CODE_RUNNING) + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) { - buzzvm_function_call(VM, "updated_no_bct", 0); - // Check for update - if (check_update()) + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_INFO("Update found \nUpdating script ...\n"); - - if (compile_bzz(bzz_file)) + ROS_WARN("Initializtion of script test passed\n"); + if (buzz_utility::update_step_test()) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + ROS_WARN("Step test passed\n"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1)); + buzzvm_gstore(VM); + return 1; } + /*Unable to step something wrong*/ else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); - if (!fp) + if (*(int*)(updater->mode) == CODE_RUNNING) { - perror(bzzfile_in_compile.str().c_str()); + ROS_ERROR("step test failed, stick to old script\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + else { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + /*You will never reach here*/ + ROS_ERROR("step test failed, Return to stand by\n"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - neigh = -1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); } - delete_p(BO_BUF); + return 0; } } - } - - else - { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); - if (tObj->i.value == no_of_robot) - { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, decided to roll back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - update_try_counter = 0; - timer_steps = 0; - } - } -} - -uint8_t* getupdater_out_msg() -{ - return (uint8_t*)updater->outmsg_queue->queue; -} - -uint8_t* getupdate_out_msg_size() -{ - // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; -} - -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) -{ - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) - { - ROS_WARN("Initializtion of script test passed\n"); - if (buzz_utility::update_step_test()) - { - /*data logging*/ - old_byte_code_size = *(size_t*)updater->bcode_size; - /*data logging*/ - ROS_WARN("Step test passed\n"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1)); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + ROS_ERROR("Initialization test failed, stick to old script\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -603,106 +625,86 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) return 0; } } - else - { - if (*(int*)(updater->mode) == CODE_RUNNING) - { - ROS_ERROR("Initialization test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); - } - else - { - /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } -} -void destroy_out_msg_queue() -{ - // fprintf(stdout,"out queue freed\n"); - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - updater_msg_ready = 0; -} - -int is_msg_present() -{ - return updater_msg_ready; -} -/*buzz_updater_elem_t get_updater() -{ - return updater; -}*/ -void destroy_updater() -{ - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if (updater->outmsg_queue) + void destroy_out_msg_queue() { + // fprintf(stdout,"out queue freed\n"); delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue); + updater_msg_ready = 0; } - if (updater->inmsg_queue) + + int is_msg_present() { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + return updater_msg_ready; + } + /*buzz_updater_elem_t get_updater() + { + return updater; + }*/ + void destroy_updater() + { + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); } - inotify_rm_watch(fd, wd); - close(fd); -} -void set_bzz_file(const char* in_bzz_file) -{ - bzz_file = in_bzz_file; -} + void set_bzz_file(const char* in_bzz_file) + { + bzz_file = in_bzz_file; + } -void updates_set_robots(int robots) -{ - no_of_robot = robots; -} + void updates_set_robots(int robots) + { + no_of_robot = robots; + } -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file) -{ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - 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/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; -}*/ + /*void collect_data(std::ofstream& logger) + { + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // + Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bd8fbde..82c665a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -26,7 +26,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) std::string fname = Compile_bzz(bzzfile_name); bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; - set_bzz_file(bzzfile_name.c_str()); + buzz_update::set_bzz_file(bzzfile_name.c_str()); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); // Initialize variables SetMode("LOITER", 0); @@ -106,7 +106,7 @@ void roscontroller::RosControllerRun() ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; // Intialize the update monitor - init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // DEBUG @@ -119,7 +119,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - update_routine(); + buzz_update::update_routine(); ROS_WARN("OUT OF UPDATE ROUTINE"); if (time_step == BUZZRATE) { @@ -152,7 +152,7 @@ void roscontroller::RosControllerRun() // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - updates_set_robots(no_of_robots); + buzz_update::updates_set_robots(no_of_robots); // get_xbee_status(); // commented out because it may slow down the node too much, to be tested ros::spinOnce(); @@ -570,10 +570,10 @@ with size......... | / delete[] out; delete[] payload_out_ptr; // Check for updater message if present send - if (is_msg_present()) + if (buzz_update::is_msg_present()) { uint8_t* buff_send = 0; - uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size()); + uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; @@ -586,10 +586,10 @@ with size......... | / *(uint16_t*)(buff_send + tot) = updater_msgSize; tot += sizeof(uint16_t); // Append updater msgs - memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); tot += updater_msgSize; // Destroy the updater out msg queue - destroy_out_msg_queue(); + buzz_update::destroy_out_msg_queue(); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint64_t* payload_64 = new uint64_t[total_size]; memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); @@ -1012,8 +1012,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); - code_message_inqueue_process(); + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); } free(pl); } From 8fa58e5e5bb78eb8776fff68cd5e0b19373b2d59 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 6 Jun 2018 23:58:38 -0400 Subject: [PATCH 13/98] Made updates optional: including update.bzz will turnon update and not including it will disable updates. --- buzz_scripts/include/update.bzz | 1 + include/buzz_update.h | 2 +- include/roscontroller.h | 1 + src/buzz_update.cpp | 192 +++++++++++++++++--------------- src/roscontroller.cpp | 11 +- 5 files changed, 108 insertions(+), 99 deletions(-) diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index 8d64de2..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,7 @@ #################################################################################################### updated="update_ack" update_no=0 +updates_active = 1 function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/include/buzz_update.h b/include/buzz_update.h index 1849b56..13bd665 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -103,7 +103,7 @@ namespace buzz_update /************************************************/ /*Initalizes the updater */ /************************************************/ - void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); /*********************************************************/ /*Appends buffer of given size to in msg queue of updater*/ diff --git a/include/roscontroller.h b/include/roscontroller.h index dedc737..29c97e6 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -96,6 +96,7 @@ private: float fcu_timeout; int armstate; int barrier; + int update; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 210e98f..43ceb7f 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -34,90 +34,101 @@ namespace buzz_update{ static size_t old_byte_code_size = 0; /*Initialize updater*/ - void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { buzzvm_t VM = buzz_utility::get_vm(); - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) - { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 0*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + ROS_INFO("Initializing file monitor..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) + { + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } } /*Check for .bzz file chages*/ int check_update() @@ -187,7 +198,7 @@ namespace buzz_update{ std::stringstream read_patched; read_patched << path << name1 << ".bo"; fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; + uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) { @@ -196,15 +207,15 @@ namespace buzz_update{ fseek(fp, 0, SEEK_END); size_t patched_size = ftell(fp); rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) { perror(read_patched.str().c_str()); } fclose(fp); /*Write the patched to a code struct and return*/ updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; + update_code->bcode = patched_bo_buf; update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); *(uint16_t*)(update_code->bcode_size) = patched_size; @@ -217,7 +228,7 @@ namespace buzz_update{ std::stringstream read_patched; read_patched << path << name1 << "-patched.bo"; fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; + uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) { @@ -226,8 +237,8 @@ namespace buzz_update{ fseek(fp, 0, SEEK_END); size_t patched_size = ftell(fp); rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) { perror(read_patched.str().c_str()); } @@ -239,7 +250,7 @@ namespace buzz_update{ /*Write the patched to a code struct and return*/ updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; + update_code->bcode = patched_bo_buf; update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); *(uint16_t*)(update_code->bcode_size) = patched_size; @@ -577,9 +588,6 @@ namespace buzz_update{ buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1)); - buzzvm_gstore(VM); return 1; } /*Unable to step something wrong*/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 82c665a..afff0c6 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -37,7 +37,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) cur_pos.longitude = 0; cur_pos.latitude = 0; cur_pos.altitude = 0; - + update=0; // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); @@ -106,7 +106,7 @@ void roscontroller::RosControllerRun() ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; // Intialize the update monitor - buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // DEBUG @@ -119,8 +119,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - buzz_update::update_routine(); - ROS_WARN("OUT OF UPDATE ROUTINE"); + if(update) buzz_update::update_routine(); if (time_step == BUZZRATE) { time_step = 0; @@ -152,7 +151,7 @@ void roscontroller::RosControllerRun() // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - buzz_update::updates_set_robots(no_of_robots); + if(update) buzz_update::updates_set_robots(no_of_robots); // get_xbee_status(); // commented out because it may slow down the node too much, to be tested ros::spinOnce(); @@ -570,7 +569,7 @@ with size......... | / delete[] out; delete[] payload_out_ptr; // Check for updater message if present send - if (buzz_update::is_msg_present()) + if (update && buzz_update::is_msg_present()) { uint8_t* buff_send = 0; uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); From 3154976e8a6e3b2fdbd10fc45cf306a54f3c1e2b Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Jun 2018 01:29:42 -0400 Subject: [PATCH 14/98] Added debug variable for prints within update files, corrected prints, beautified update files and incorporate a part of safe deployment state machine with standby.bzz. --- buzz_scripts/stand_by.bzz | 3 -- include/buzz_update.h | 2 +- src/buzz_update.cpp | 92 +++++++++++++++++++++------------------ src/roscontroller.cpp | 7 ++- 4 files changed, 53 insertions(+), 51 deletions(-) diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1b30b06..c605089 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -14,8 +14,6 @@ function init(){ if(r. data < l. data or (r. data == l. data )) return l else return r }) - - BVMSTATE = "UPDATESTANDBY" init_swarm() # start the swarm command listener nei_cmd_listen() @@ -27,7 +25,6 @@ function stand_by(){ neighbors.listen(updated, function(vid, value, rid) { - print(" got from",vid," ", " value = ",value," ","rid"," " ) if(value==update_no) barrier.put(rid,1) } ) diff --git a/include/buzz_update.h b/include/buzz_update.h index 13bd665..4b43e86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -136,7 +136,7 @@ namespace buzz_update /***************************************************/ /*Sets bzz file name*/ /***************************************************/ - void set_bzz_file(const char* in_bzz_file); + void set_bzz_file(const char* in_bzz_file, bool dbg); /****************************************************/ /*Tests the code from a buffer*/ diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 43ceb7f..8624c3a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -28,10 +28,9 @@ namespace buzz_update{ static int neigh = -1; static int updater_msg_ready; static uint16_t update_try_counter = 0; - static int updated = 0; static const uint16_t MAX_UPDATE_TRY = 5; - static int packet_id_ = 0; static size_t old_byte_code_size = 0; + static bool debug = false; /*Initialize updater*/ int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) @@ -45,7 +44,7 @@ namespace buzz_update{ dbgf_name = dbgfname; bcfname = bo_filename; ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); - ROS_INFO("Initializing file monitor..."); + if(debug) ROS_INFO("Initializing file monitor..."); fd = inotify_init1(IN_NONBLOCK); if (fd < 0) { @@ -140,23 +139,22 @@ namespace buzz_update{ while (i < len) { struct inotify_event* event = (struct inotify_event*)&buf[i]; - /* file was modified this flag is true in nano and self delet in gedit and other editors */ - // fprintf(stdout,"inside file monitor.\n"); + /*Report file changes and self deletes*/ if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) { - /*respawn watch if the file is self deleted */ + /*Respawn watch if the file is self deleted */ inotify_rm_watch(fd, wd); close(fd); fd = inotify_init1(IN_NONBLOCK); wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ + /*To mask multiple writes from editors*/ if (!old_update) { check = 1; old_update = 1; } } - /* update index to start of next event */ + /*Update index to start of next event*/ i += sizeof(struct inotify_event) + event->len; } if (!check) @@ -175,14 +173,14 @@ namespace buzz_update{ /*Patch the old bo with new patch*/ std::stringstream patch_writefile; patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ + /*Write the patch to a file and call bsdiff to patch*/ FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); fwrite(patch, update_patch_size, 1, fp); fclose(fp); std::stringstream patch_exsisting; patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 << "tmp_patch.bo"; - fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); if (system(patch_exsisting.str().c_str())) return 0; else @@ -197,7 +195,10 @@ namespace buzz_update{ /*Read the exsisting file to simulate the patching*/ std::stringstream read_patched; read_patched << path << name1 << ".bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + if(debug){ + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) @@ -227,7 +228,10 @@ namespace buzz_update{ /*Read the new patched file*/ std::stringstream read_patched; read_patched << path << name1 << "-patched.bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + if(debug) { + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } uint8_t* patched_bo_buf = 0; FILE* fp = fopen(read_patched.str().c_str(), "rb"); if (!fp) @@ -300,13 +304,13 @@ namespace buzz_update{ size += sizeof(uint16_t); *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; updater_msg_ready = 1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); + if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter); } void code_message_inqueue_append(uint8_t* msg, uint16_t size) { updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); memcpy(updater->inmsg_queue->queue, msg, size); @@ -321,19 +325,18 @@ namespace buzz_update{ { int size = 0; updater_code_t out_code = NULL; - - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - + if(debug) { + ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode)); + ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } if (*(int*)(updater->mode) == CODE_RUNNING) { // fprintf(stdout,"[debug]Inside inmsg code running"); if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) { - updated = 1; size += sizeof(uint8_t); if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) { @@ -360,7 +363,7 @@ namespace buzz_update{ if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) { - printf("TEST PASSED!\n"); + if(debug) ROS_WARN("Received patch PASSED tests!"); *(uint16_t*)updater->update_no = update_no; /*Clear exisiting patch if any*/ delete_p(updater->patch); @@ -378,7 +381,7 @@ namespace buzz_update{ } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); + ROS_ERROR("Patching the .bo file failed"); update_try_counter++; outqueue_append_code_request(update_no); } @@ -388,6 +391,7 @@ namespace buzz_update{ size = 0; if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) { + if(debug) ROS_WARN("Patch rebroadcast request received."); size += sizeof(uint8_t); if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { @@ -395,11 +399,11 @@ namespace buzz_update{ if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); + if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); code_message_outqueue_append(); } if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); } } // fprintf(stdout,"in queue freed\n"); @@ -421,12 +425,12 @@ namespace buzz_update{ std::string name2 = name1 + "-update"; - // CALL BSDIFF CMD + /*Launch bsdiff and create a patch*/ genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); system(genpatch.str().c_str()); - /* delete old files & rename new files */ + /*Delete old files & rename new files*/ remove((path + name1 + ".bo").c_str()); remove((path + name1 + ".bdb").c_str()); @@ -474,11 +478,11 @@ namespace buzz_update{ // Check for update if (check_update()) { - ROS_INFO("Update found \nUpdating script ...\n"); + ROS_INFO("Update found \tUpdating script ..."); if (compile_bzz(bzz_file)) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + ROS_ERROR("Error in compiling script, resuming old script."); } else { @@ -510,12 +514,12 @@ namespace buzz_update{ create_update_patch(); VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); + if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_gstore(VM); neigh = -1; - ROS_INFO("Sending code... \n"); + if(debug) ROS_INFO("Broadcasting patch ..."); code_message_outqueue_append(); } delete_p(BO_BUF); @@ -530,9 +534,10 @@ namespace buzz_update{ buzzvm_gload(VM); buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); if (tObj->i.value == no_of_robot) { + ROS_WARN("Patch deployment successful, rolling forward"); *(int*)(updater->mode) = CODE_RUNNING; gettimeofday(&t2, NULL); // collect_data(); @@ -543,7 +548,7 @@ namespace buzz_update{ } else if (timer_steps > TIMEOUT_FOR_ROLLBACK) { - ROS_ERROR("TIME OUT Reached, decided to roll back"); + ROS_ERROR("TIME OUT Reached, rolling back"); /*Time out hit deceided to roll back*/ *(int*)(updater->mode) = CODE_RUNNING; buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); @@ -568,13 +573,13 @@ namespace buzz_update{ { if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_WARN("Initializtion of script test passed\n"); + if(debug) ROS_WARN("Initializtion test passed"); if (buzz_utility::update_step_test()) { /*data logging*/ old_byte_code_size = *(size_t*)updater->bcode_size; /*data logging*/ - ROS_WARN("Step test passed\n"); + if(debug) ROS_WARN("Step test passed"); *(int*)(updater->mode) = CODE_STANDBY; // fprintf(stdout,"updater value = %i\n",updater->mode); delete_p(updater->bcode); @@ -595,13 +600,13 @@ namespace buzz_update{ { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); + ROS_ERROR("Step test failed, resuming old script"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); + ROS_ERROR("Step test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -616,13 +621,13 @@ namespace buzz_update{ { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("Initialization test failed, stick to old script\n"); + ROS_ERROR("Initialization test failed, resuming old script"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -675,8 +680,9 @@ namespace buzz_update{ close(fd); } - void set_bzz_file(const char* in_bzz_file) + void set_bzz_file(const char* in_bzz_file, bool dbg) { + debug=dbg; bzz_file = in_bzz_file; } @@ -686,7 +692,7 @@ namespace buzz_update{ } /*-------------------------------------------------------- - / Create Buzz bytecode from the bzz script inputed + / Create Buzz bytecode from the bzz script input /-------------------------------------------------------*/ int compile_bzz(std::string bzz_file) { diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index afff0c6..010107e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -26,7 +26,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) std::string fname = Compile_bzz(bzzfile_name); bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; - buzz_update::set_bzz_file(bzzfile_name.c_str()); + buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); // Initialize variables SetMode("LOITER", 0); @@ -576,8 +576,7 @@ with size......... | / ; int tot = 0; mavros_msgs::Mavlink update_packets; - fprintf(stdout, "Appending code into message ...\n"); - fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); + if(debug) ROS_INFO("Broadcasted Update packet Size: %u", updater_msgSize); // allocate mem and clear it buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize); memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); @@ -1008,7 +1007,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // Copy packet into temporary buffer neglecting update constant memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t*)(pl); - fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); + if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize); if (unMsgSize > 0) { buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); From 66cb2df693c475b3479ffad223dc79ec5c56b62a Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 12 Jun 2018 14:30:14 -0400 Subject: [PATCH 15/98] Fixed neighbours pos clearing. --- include/buzzuav_closures.h | 4 ++++ src/buzzuav_closures.cpp | 5 ++++- src/roscontroller.cpp | 1 + 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 6e31ec6..de887ea 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -101,6 +101,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) * update neighbors from in msgs */ void update_neighbors(buzzvm_t vm); +/* + *Clear neighbours struct + */ +void clear_neighbours_pos(); /* * closure to add a neighbor status */ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index abff74f..ce9fda9 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -651,7 +651,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) neighbors_map.erase(it); neighbors_map.insert(make_pair(id, pos_arr)); } - +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} // update at each step the VM table void update_neighbors(buzzvm_t vm) { diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 010107e..e460bd7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -732,6 +732,7 @@ void roscontroller::maintain_pos(int tim_step) if (timer_step >= BUZZRATE) { neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! timer_step = 0; From 6371581b66dc322699139d0b9323edbbab0ca1bf Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 19 Dec 2017 18:36:10 -0500 Subject: [PATCH 16/98] debug new matrix form for RRT and add blob check for intersection --- buzz_scripts/include/mapmatrix.bzz | 52 +++++-- buzz_scripts/include/rrtstar.bzz | 158 ++++++++++++-------- buzz_scripts/include/uavstates.bzz | 52 ++++--- buzz_scripts/{testalone.bzz => testRRT.bzz} | 12 +- buzz_scripts/testaloneWP.bzz | 2 - 5 files changed, 167 insertions(+), 109 deletions(-) rename buzz_scripts/{testalone.bzz => testRRT.bzz} (76%) diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index b99432a..79ad775 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -5,8 +5,8 @@ robot_marker = "X" # copy a full matrix row function mat_copyrow(out,ro,in,ri){ out[ro] = {} - var icr = 0 - while(icr < size(in[ri])) { + var icr = 1 + while(icr <= size(in[ri])) { out[ro][icr]=in[ri][icr] icr = icr + 1 } @@ -18,11 +18,11 @@ function getvec(t,row){ function init_test_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } @@ -33,22 +33,22 @@ function init_test_map(len){ map[6][5] = 0.0 map[4][5] = 0.0 - log("Occupancy grid initialized (",len,"x",len,") with obstacles.") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.") } function init_map(len){ map = {} - var indexR = 0 - while(indexR < len) { + var indexR = 1 + while(indexR <= len) { map[indexR] = {} - var indexC = 0 - while(indexC < len) { + var indexC = 1 + while(indexC <= len) { map[indexR][indexC] = 1.0 indexC = indexC + 1 } indexR = indexR + 1 } - log("Occupancy grid initialized (",len,"x",len,").") + log("Occupancy grid initialized (",size(map),"x",size(map[1]),").") } function add_obstacle(pos, off, inc_trust) { @@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { #log("Add obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old-inc_trust > 0.0) map[xi][yi] = old-inc_trust else @@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ #log("Remove obstacle in cell: ", xi, " ", yi) - old=map[xi][yi] + var old=map[xi][yi] if(old + dec_trust < 1.0) #x,y map[xi][yi] = old+dec_trust else @@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) { } } + +function get_occupied_cells(cur_cell){ + var i = 1 + var occupied_cells = {} + occupied_cells[0] = cur_cell + occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y) + occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y) + occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1) + occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1) + return occupied_cells +} + +function is_in(dictionary, x, y){ + var i = 0 + while(i < size(dictionary)){ + if(dictionary[i].x == x and dictionary[i].y == y){ + return 1 + } + i = i + 1 + } + return 0 +} + + function table_print(t) { foreach(t, function(key, value) { log(key, " -> ", value) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index ff0630c..b84fec9 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) { # create a map big enough for the goal init_map(2*math.round(math.vec2.length(m_navigation))+10) # center the robot on the grid - cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0)) + cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } function UAVpathPlanner(m_navigation, m_pos) { # place the robot on the grid - update_curcell(m_pos,0) + cur_cell = getcell(m_pos,0) # create the goal in the map grid mapgoal = math.vec2.add(m_navigation,cur_cell) mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) @@ -34,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) { m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) # place the robot on the grid - update_curcell(m_pos,1) + cur_cell = getcell(m_pos,1) log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) log("Going to cell: ", m_goal.x, " ", m_goal.y) @@ -44,26 +44,35 @@ function Kh4pathPlanner(m_goal, m_pos) { return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) } -function update_curcell(m_curpos, kh4) { +function getcell(m_curpos, kh4) { + var cell = {} if(kh4) { # for khepera playground - cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) + cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y)) } else { # for uav map relative to home - cur_cell = math.vec2.add(cur_cell, m_curpos) - cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + cell = math.vec2.add(cell, m_curpos) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) } - if(cur_cell.x > size(map)) - cur_cell.x = size(map) - if(cur_cell.y > size(map[1])) - cur_cell.y = size(map[1]) - if(cur_cell.x < 1) - cur_cell.x = 1 - if(cur_cell.y < 1) - cur_cell.y = 1 + if(cell.x > size(map)) + cell.x = size(map) + if(cell.y > size(map[1])) + cell.y = size(map[1]) + if(cell.x < 1) + cell.x = 1 + if(cell.y < 1) + cell.y = 1 + + return cell +} + +function updateMap(m_pos) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) } function UAVgetneiobs (m_curpos) { - update_curcell(m_curpos,0) + cur_cell = getcell(m_curpos,0) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) @@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -118,7 +127,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - update_curcell(m_curpos,1) + cur_cell = getcell(m_curpos,1) reduce(proximity, function(key, value, acc) { @@ -159,19 +168,21 @@ function getproxobs (m_curpos) { } function check_path(m_path, goal_l, m_curpos, kh4) { - pathisblocked = 0 - nb=goal_l - update_curcell(m_curpos,kh4) - Cvec = cur_cell - while(nb < size(m_path) and nb <= goal_l+1) { - Nvec = getvec(m_path, nb) - if(kh4==0) - Nvec=vec_from_gps(Nvec.x,Nvec.y) - if(doesItIntersect(Cvec, Nvec)){ - log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + var pathisblocked = 0 + var nb = goal_l + cur_cell = getcell(m_curpos,kh4) + var Cvec = cur_cell + while(nb <= size(m_path) and nb <= goal_l+1) { + var Nvec = getvec(m_path, nb) + if(kh4 == 0) { + Nvec = vec_from_gps(Nvec.x,Nvec.y,1) + Nvec = getcell(Nvec,kh4) + } + if(doesItIntersect(Cvec, Nvec)) { + log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") pathisblocked = 1 } - Cvec=Nvec + Cvec = Nvec nb = nb + 1 } @@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) { RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive - goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} #table_print(goalBoundary) - numberOfPoints = 1 - arrayOfPoints = {} + var numberOfPoints = 1 + var arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} Path = {} + mat_copyrow(Path,1,arrayOfPoints,1) + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} goalReached = 0; timeout = 0 @@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) { pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) #log("Point generated: ", pt.x, " ", pt.y) - pointList = findPointsInRadius(pt,Q,RADIUS); + var pointList = findPointsInRadius(pt,Q,RADIUS); # Find connection that provides the least cost to come nbCount = 0; @@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - #log("Found ", size(pointList), " close point:", pointList.mat) + log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) { nbCount = nbCount + 1; if(intersects != 1) { #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") - distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] if(distance < minCounted) { minCounted = distance; @@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) { } if(minCounter > 0) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointList[minCounter][4] @@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) { # Now check to see if any of the other points can be redirected nbCount = 0; ipt = 1 - while(ipt < size(pointList)) { + while(ipt <= size(pointList)) { pointNumber = {} mat_copyrow(pointNumber,1,pointList,ipt) + #log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4]) # Follow the line to see if it intersects anything intersects = doesItIntersect(pt,getvec(pointNumber,1)); @@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) { if(intersects != 1) { # If the alternative path is shorter than change it tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt)) + #log("Q: ", size(Q), "x", size(Q[1])) if(tmpdistance < Q[pointNumber[1][4]][5]) { Q[pointNumber[1][4]][3] = numberOfPoints Q[pointNumber[1][4]][5] = tmpdistance @@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) { # If there is no intersection we need to add to the tree if(intersects != 1) { numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][2]=pt.y + Q[numberOfPoints] = {} Q[numberOfPoints][1] = pt.x Q[numberOfPoints][2] = pt.y Q[numberOfPoints][3] = pointNum @@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) { function findClosestPoint(point,aPt) { # Go through each points and find the distances between them and the # target point - distance = 999 - pointNumber = -1 - ifcp=1 + var distance = 999 + var pointNb = -1 + var ifcp=1 while(ifcp <= size(aPt)) { - range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) if(range < distance) { distance = range; - pointNumber = ifcp; + pointNb = ifcp; } ifcp = ifcp + 1 } - return pointNumber + return pointNb } # Find the closest point in the tree function findPointsInRadius(point,q,r) { - pointList = {} + var ptList = {} var counted = 0; var iir = 1 while(iir <= size(q)) { - tmpvv = getvec(q,iir) + var tmpvv = getvec(q,iir) #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) - distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) if(distance < r) { counted = counted+1; - mat_copyrow(pointList,counted,q,iir) + mat_copyrow(ptList,counted,q,iir) } iir = iir + 1 } - return pointList + return ptList } # check if the line between 2 point intersect an obstacle function doesItIntersect(point,vector) { #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) - dif = math.vec2.sub(point,vector) - distance = math.vec2.length(dif) - if(distance==0.0){ + var dif = math.vec2.sub(point,vector) + var distance = math.vec2.length(dif) + if(distance == 0.0){ # Find what block we're in right now var xi = math.round(point.x) #+1 var yi = math.round(point.y) #+1 - if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) { if(map[xi][yi] > 0.5) return 1 else @@ -372,21 +393,22 @@ function doesItIntersect(point,vector) { } else return 0 } - vec = math.vec2.scale(dif,1.0/distance) - pathstep = size(map[1]) - 2 + var vec = math.vec2.scale(dif,1.0/distance) + var pathstep = size(map) - 2 idii = 1.0 while(idii <= pathstep) { - range = distance*idii/pathstep - pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + var range = distance*idii/pathstep + var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); # Find what block we're in right now var xi = math.round(pos_chk.x) #+1 var yi = math.round(pos_chk.y) #+1 - #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") - if(xi!=cur_cell.x and yi!=cur_cell.y){ - if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) { + if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ + #if(xi!=cur_cell.x and yi!=cur_cell.y) { + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { if(map[xi][yi] < 0.5) { # because obstacle use trust values #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") return 1; @@ -403,20 +425,28 @@ function doesItIntersect(point,vector) { } function getPathGPS(Q,nb){ + #log("-----------CONVERTING PATH!!!") path={} var npt=0 + var isrecursive = 0 # get the path from the point list - while(nb!=1){ + while(nb > 1 and isrecursive!=1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] path[npt][2]=Q[nb][2] - nb = Q[nb][3]; + if(nb != Q[Q[nb][3]][3]) + nb = Q[nb][3]; + else { + isrecursive = 1 + path={} + log("ERROR - Recursive path !!!") + } } # re-order the list and make the path points absolute pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) pathR[totpt-npt] = {} @@ -433,7 +463,7 @@ function getPath(Q,nb){ var npt=0 # log("get the path from the point list") - while(nb!=1){ + while(nb > 1){ npt=npt+1 path[npt] = {} path[npt][1]=Q[nb][1] @@ -444,7 +474,7 @@ function getPath(Q,nb){ # log("re-order the list") # table_print(path.mat) pathR={} - var totpt = npt + var totpt = npt + 1 while(npt > 0){ tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 843f34f..501416d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,6 +3,7 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## +include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. @@ -14,7 +15,6 @@ GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. cur_goal_l = 0 rc_State = 0 -homegps = {.lat=0, .long=0} function uav_initswarm() { s = swarm.create(1) @@ -34,6 +34,7 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff + homegps = {.lat=position.latitude, .long=position.longitude} if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) @@ -61,7 +62,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf) + gotoWPRRT(transf) } if(rc_goto.id==id){ @@ -93,10 +94,8 @@ function picture() { # still requires to be tuned, replaning takes too much time.. # DS 23/11/2017 function gotoWPRRT(transf) { - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) - homegps.lat = position.latitude - homegps.long = position.longitude - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) @@ -104,35 +103,36 @@ function gotoWPRRT(transf) { else { if(Path==nil){ # create the map - if(map==nil) + if(map==nil) { UAVinit_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(rc_goal, m_pos) } cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { - cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude - cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) - print(" heading to ", cur_pt.x,cur_pt.y) - if(math.vec2.length(cur_pt)>GOTODIST_TOL) { - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) - UAVgetneiobs(m_pos) - if(check_path(Path,cur_goal_l,m_pos,0)) { + var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude + var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) + print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) + if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { + updateMap(m_pos) + if(check_path(Path, cur_goal_l, m_pos, 0)) { uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { - #getproxobs(m_pos) - UAVgetneiobs (m_pos) + updateMap(m_pos) Path = UAVpathPlanner(rc_goal, m_pos) } cur_goal_l = 1 + } else { + cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) } - cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt)) - uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude) } else cur_goal_l = cur_goal_l + 1 @@ -262,14 +262,18 @@ function LimitAngle(angle){ return angle } -function vec_from_gps(lat, lon) { - d_lon = lon - position.longitude +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - position.longitude d_lat = lat - position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) + return math.vec2.new(ned_x,ned_y) } function gps_from_vec(vec) { diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testRRT.bzz similarity index 76% rename from buzz_scripts/testalone.bzz rename to buzz_scripts/testRRT.bzz index 7460de4..7590470 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testRRT.bzz @@ -1,4 +1,3 @@ -include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. @@ -6,15 +5,18 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5085,-73.1531935979886,5.0) - set_goto(picture) + uav_storegoal(45.5088103899,-73.1540826153,25.0) + set_goto(idle) } # Executed once at init time. function init() { - statef=turnedoff - UAVSTATE = "TURNEDOFF" uav_initstig() + uav_initswarm() + #statef=turnedoff + #UAVSTATE = "TURNEDOFF" + statef = takeoff + UAVSTATE = "TAKEOFF" } # Executed at each time step. diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 97b92bf..e438fb1 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -2,7 +2,6 @@ include "vec2.bzz" include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" function action() { uav_storegoal(-1.0,-1.0,-1.0) @@ -13,7 +12,6 @@ function action() { function init() { statef=turnedoff UAVSTATE = "TURNEDOFF" - uav_initstig() TARGET_ALTITUDE = 15.0 } From d532dd22512c94d709b5c47702df15e641df4ab7 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 12:20:43 -0500 Subject: [PATCH 17/98] generalized some parts of RRT and complete export occ_grid --- CMakeLists.txt | 1 + buzz_scripts/include/rrtstar.bzz | 124 ++++++++++------------------- buzz_scripts/include/uavstates.bzz | 8 +- include/buzzuav_closures.h | 4 + include/roscontroller.h | 5 ++ package.xml | 2 + src/buzzuav_closures.cpp | 35 ++++---- src/roscontroller.cpp | 44 +++++++++- 8 files changed, 125 insertions(+), 98 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b7e4b0..672d68d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs mavros_msgs sensor_msgs + nav_msgs message_generation ) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index b84fec9..9777dde 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,54 +6,41 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} -function UAVinit_map(m_navigation) { +# create a map with its length to fit the goal (rel. pos.) and the current position as the center +function dyn_init_map(m_navigation) { # create a map big enough for the goal - init_map(2*math.round(math.vec2.length(m_navigation))+10) + init_map(math.round(2*math.vec2.length(m_navigation))+10) # center the robot on the grid cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) } -function UAVpathPlanner(m_navigation, m_pos) { +# start the RRT* to reach the goal (rel. pos.) +function pathPlanner(m_goal, m_pos, kh4) { # place the robot on the grid - cur_cell = getcell(m_pos,0) + cur_cell = getcell(m_pos) # create the goal in the map grid - mapgoal = math.vec2.add(m_navigation,cur_cell) - mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) + mapgoal = getcell(math.vec2.add(m_goal, m_pos)) + + #print_map(map) + #export_map(map) # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5,5)) #size(map[1])/20.0,size(map[1])/20.0)) + return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) } -function Kh4pathPlanner(m_goal, m_pos) { - # move 0,0 to a corner instead of the center - m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) - - # place the robot on the grid - cur_cell = getcell(m_pos,1) - log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) - log("Going to cell: ", m_goal.x, " ", m_goal.y) - - # search for a path - print_map(map) - # print_map_argos(map) - return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) -} - -function getcell(m_curpos, kh4) { +function getcell(m_curpos) { var cell = {} - if(kh4) { # for khepera playground - cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y)) - } else { # for uav map relative to home - cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) - cell = math.vec2.add(cell, m_curpos) - cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) - } + # relative to center (start position) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y) + cell = math.vec2.add(cell, offset) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) + if(cell.x > size(map)) cell.x = size(map) if(cell.y > size(map[1])) @@ -72,7 +59,7 @@ function updateMap(m_pos) { } function UAVgetneiobs (m_curpos) { - cur_cell = getcell(m_curpos,0) + cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) @@ -81,7 +68,7 @@ function UAVgetneiobs (m_curpos) { function getneiobs (m_curpos) { var foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) var old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -127,7 +114,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - cur_cell = getcell(m_curpos,1) + cur_cell = getcell(m_curpos) reduce(proximity, function(key, value, acc) { @@ -170,13 +157,13 @@ function getproxobs (m_curpos) { function check_path(m_path, goal_l, m_curpos, kh4) { var pathisblocked = 0 var nb = goal_l - cur_cell = getcell(m_curpos,kh4) + cur_cell = getcell(m_curpos) var Cvec = cur_cell while(nb <= size(m_path) and nb <= goal_l+1) { var Nvec = getvec(m_path, nb) if(kh4 == 0) { Nvec = vec_from_gps(Nvec.x,Nvec.y,1) - Nvec = getcell(Nvec,kh4) + Nvec = getcell(Nvec) } if(doesItIntersect(Cvec, Nvec)) { log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") @@ -225,7 +212,7 @@ function RRTSTAR(GOAL,TOL) { minCounter = 0; if(size(pointList) != 0) { - log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) + #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) ipt=1 while(ipt <= size(pointList)) { pointNumber = {} @@ -329,8 +316,7 @@ function RRTSTAR(GOAL,TOL) { } if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPathGPS(Q,numberOfPoints) - print_pos(Path) + Path = getPath(Q,numberOfPoints, 1) } else { log("FAILED TO FIND A PATH!!!") Path = nil @@ -424,62 +410,40 @@ function doesItIntersect(point,vector) { return 0 } -function getPathGPS(Q,nb){ +# create a table with only the path's points in order +function getPath(Q,nb,gps){ #log("-----------CONVERTING PATH!!!") - path={} + var pathL={} var npt=0 - var isrecursive = 0 # get the path from the point list - while(nb > 1 and isrecursive!=1){ + while(nb > 1) { npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] + pathL[npt] = {} + pathL[npt][1]=Q[nb][1] + pathL[npt][2]=Q[nb][2] if(nb != Q[Q[nb][3]][3]) nb = Q[nb][3]; else { - isrecursive = 1 - path={} log("ERROR - Recursive path !!!") + return nil } } - # re-order the list and make the path points absolute - pathR={} - var totpt = npt + 1 - while(npt > 0){ - tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) - pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - npt = npt - 1 - } - return pathR -} - -# create a table with only the path's points in order -function getPath(Q,nb){ - path={} - var npt=0 - - # log("get the path from the point list") - while(nb > 1){ - npt=npt+1 - path[npt] = {} - path[npt][1]=Q[nb][1] - path[npt][2]=Q[nb][2] - nb = Q[nb][3]; - } - # log("re-order the list") # table_print(path.mat) - pathR={} + var pathR={} var totpt = npt + 1 while(npt > 0){ - tmpgoal = getvec(path,npt) pathR[totpt-npt] = {} - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y + if(gps) { + tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) + pathR[totpt-npt][1]=tmpgoal.latitude + pathR[totpt-npt][2]=tmpgoal.longitude + } else { + tmpgoal = getvec(path,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y + } npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 501416d..d3b015d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -104,15 +104,16 @@ function gotoWPRRT(transf) { if(Path==nil){ # create the map if(map==nil) { - UAVinit_map(rc_goal) + dyn_init_map(rc_goal) homegps.lat = position.latitude homegps.long = position.longitude } # add proximity sensor and neighbor obstacles to the map while(Path==nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else if(cur_goal_l <= size(Path)) { var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude @@ -126,8 +127,9 @@ function gotoWPRRT(transf) { rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) while(Path == nil) { updateMap(m_pos) - Path = UAVpathPlanner(rc_goal, m_pos) + Path = pathPlanner(rc_goal, m_pos) } + print_pos(Path) cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 56f0bd6..f14e1a7 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -94,6 +94,10 @@ void set_currentpos(double latitude, double longitude, double altitude); * returns the current go to position */ double* getgoto(); +/* + * returns the current grid + */ +std::map> getgrid(); /* * returns the current Buzz state */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 17533a4..0e9e547 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" @@ -124,6 +125,7 @@ private: ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; ros::Publisher uavstate_pub; + ros::Publisher grid_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; @@ -179,6 +181,9 @@ private: /*UAVState publisher*/ void uavstate_publisher(); + /*Grid publisher*/ + void grid_publisher(); + /*BVM message payload publisher*/ void send_MPpayload(); diff --git a/package.xml b/package.xml index 6f4fc98..0fa6866 100644 --- a/package.xml +++ b/package.xml @@ -48,6 +48,8 @@ mavros_msgs sensor_msgs sensor_msgs + nav_msgs + nav_msgs message_generation message_runtime diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6b380d6..980948b 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -36,6 +36,7 @@ std::map targets_map; std::map wplist_map; std::map neighbors_map; std::map neighbors_status_map; +std::map> grid; /****************************************/ /****************************************/ @@ -178,26 +179,24 @@ int buzz_exportmap(buzzvm_t vm) / Buzz closure to export a 2D map /----------------------------------------*/ { - /* Make sure one parameter has been passed */ buzzvm_lnum_assert(vm, 1); - /* Get the parameter */ + // Get the parameter buzzvm_lload(vm, 1); - buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // matrix - /* Get the table */ + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzobj_t t = buzzvm_stack_at(vm, 1); - /* Copy the values into a vector */ - std::vector mat; - for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i) - { - /* Duplicate the table */ + for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { buzzvm_dup(vm); - /* Push the index */ buzzvm_pushi(vm, i); - /* Get the value */ buzzvm_tget(vm); - /* Store it in the vector (assume all values are float, no mistake...) */ - mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value); - /* Get rid of the value, now useless */ + std::map row; + for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + buzzvm_dup(vm); + buzzvm_pushi(vm, j); + buzzvm_tget(vm); + row.insert(std::pair(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + buzzvm_pop(vm); + } + grid.insert(std::pair>(i,row)); buzzvm_pop(vm); } return buzzvm_ret0(vm); @@ -457,6 +456,14 @@ double* getgoto() return goto_pos; } +std::map> getgrid() +/* +/ return the grid +/-------------------------------------------------------------*/ +{ + return grid; +} + float* getgimbal() /* / return current gimbal commands diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9fe34f9..c5a4354 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -114,8 +114,10 @@ void roscontroller::RosControllerRun() // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { - // Neighbours of the robot published with id in respective topic + // Publish topics neighbours_pos_publisher(); + uavstate_publisher(); + grid_publisher(); send_MPpayload(); // Check updater state and step code update_routine(); @@ -337,6 +339,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); uavstate_pub = n_c.advertise("uavstate", 5); + grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); // Service Clients @@ -447,6 +450,45 @@ void roscontroller::uavstate_publisher() uavstate_pub.publish(uavstate_msg); } +void roscontroller::grid_publisher() +/* +/ Publish current Grid from Buzz script +/----------------------------------------------------*/ +{ + std::map> grid = buzzuav_closures::getgrid(); + std::map>::iterator itr = grid.begin(); + int g_w = itr->second.size(); + int g_h = grid.size(); + + if(g_w!=0 && g_h!=0) { + auto current_time = ros::Time::now(); + nav_msgs::OccupancyGrid grid_msg; + grid_msg.header.frame_id = "/world"; + grid_msg.header.stamp = current_time; + grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. + //grid_msg.info.resolution = gridMap.getResolution(); + grid_msg.info.width = g_w; + grid_msg.info.height = g_h; + grid_msg.info.origin.position.x = 0.0; + grid_msg.info.origin.position.y = 0.0; + grid_msg.info.origin.position.z = 0.0; + grid_msg.info.origin.orientation.x = 0.0; + grid_msg.info.origin.orientation.y = 0.0; + grid_msg.info.origin.orientation.z = 0.0; + grid_msg.info.origin.orientation.w = 1.0; + grid_msg.data.resize(g_w*g_h); + + for (itr=grid.begin(); itr!=grid.end(); ++itr) { + std::map::iterator itc = itr->second.begin(); + for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { + grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); + } + } + grid_pub.publish(grid_msg); + } +} + + void roscontroller::Arm() /* / Functions handling the local MAV ROS flight controller From 50368d84d33fd63833dfa6f57dad8ff6bb1354ff Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 13:45:22 -0500 Subject: [PATCH 18/98] fix rviz res --- buzz_scripts/include/rrtstar.bzz | 2 +- src/buzzuav_closures.cpp | 6 ++++-- src/roscontroller.cpp | 9 ++++++--- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 9777dde..2c2a6d5 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -27,7 +27,7 @@ function pathPlanner(m_goal, m_pos, kh4) { mapgoal = getcell(math.vec2.add(m_goal, m_pos)) #print_map(map) - #export_map(map) + export_map(map) # search for a path return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 980948b..a58c8f2 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -184,12 +184,12 @@ int buzz_exportmap(buzzvm_t vm) buzzvm_lload(vm, 1); buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary buzzobj_t t = buzzvm_stack_at(vm, 1); - for(int32_t i = 1; i < buzzdict_size(t->t.value); ++i) { + for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) { buzzvm_dup(vm); buzzvm_pushi(vm, i); buzzvm_tget(vm); std::map row; - for(int32_t j = 1; j < buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { buzzvm_dup(vm); buzzvm_pushi(vm, j); buzzvm_tget(vm); @@ -199,6 +199,8 @@ int buzz_exportmap(buzzvm_t vm) grid.insert(std::pair>(i,row)); buzzvm_pop(vm); } + // DEBUG + // ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value)); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c5a4354..2bf5cab 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -461,12 +461,14 @@ void roscontroller::grid_publisher() int g_h = grid.size(); if(g_w!=0 && g_h!=0) { + // DEBUG + //ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w); auto current_time = ros::Time::now(); nav_msgs::OccupancyGrid grid_msg; grid_msg.header.frame_id = "/world"; grid_msg.header.stamp = current_time; grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. - //grid_msg.info.resolution = gridMap.getResolution(); + grid_msg.info.resolution = 0.1;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; grid_msg.info.origin.position.x = 0.0; @@ -476,12 +478,13 @@ void roscontroller::grid_publisher() grid_msg.info.origin.orientation.y = 0.0; grid_msg.info.origin.orientation.z = 0.0; grid_msg.info.origin.orientation.w = 1.0; - grid_msg.data.resize(g_w*g_h); + grid_msg.data.resize(g_w * g_h); for (itr=grid.begin(); itr!=grid.end(); ++itr) { std::map::iterator itc = itr->second.begin(); for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { - grid_msg.data[itr->first*g_w+itc->first] = round(itc->second*100.0); + //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0)); + grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0); } } grid_pub.publish(grid_msg); From 63f9111d2f56636de8138d357630572660919412 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 14:33:23 -0500 Subject: [PATCH 19/98] made rrtstar a state --- buzz_scripts/include/barrier.bzz | 2 +- buzz_scripts/include/rrtstar.bzz | 156 ++++++++++++++--------------- buzz_scripts/include/uavstates.bzz | 117 +++++++++++----------- buzz_scripts/testRRT.bzz | 4 +- src/buzzuav_closures.cpp | 2 +- 5 files changed, 136 insertions(+), 145 deletions(-) diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index e280a66..31a452e 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -32,7 +32,7 @@ function barrier_set(threshold, transf, resumef, bdt) { statef = function() { barrier_wait(threshold, transf, resumef, bdt); } - UAVSTATE = "BARRIERWAIT" + BVMSTATE = "BARRIERWAIT" barrier_create() } diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 2c2a6d5..86d1ce0 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,10 +6,13 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +RRT_RUNSTEP = 3 +PROX_SENSOR_THRESHOLD = 0.11 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} +mapgoal = {} # create a map with its length to fit the goal (rel. pos.) and the current position as the center function dyn_init_map(m_navigation) { @@ -29,8 +32,25 @@ function pathPlanner(m_goal, m_pos, kh4) { #print_map(map) export_map(map) + # Initialize RRTStar var + log("--------Initialize RRTStar--------------") + HEIGHT = size(map) + WIDTH = size(map[1]) + RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive + goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0} + #table_print(goalBoundary) + numberOfPoints = 1 + arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance + Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} + goalReached = 0 + timeout = 0 + # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) + old_statef = statef + rrtstar() } function getcell(m_curpos) { @@ -53,12 +73,13 @@ function getcell(m_curpos) { return cell } -function updateMap(m_pos) { +function populateGrid(m_pos) { #getproxobs(m_pos) - UAVgetneiobs (m_pos) + getneiobs (m_pos) } -function UAVgetneiobs (m_curpos) { +# TODO: populate the map with a blob instead? +function getneiobs (m_curpos) { cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { @@ -66,54 +87,47 @@ function UAVgetneiobs (m_curpos) { }) } -function getneiobs (m_curpos) { - var foundobstacle = 0 - cur_cell = getcell(m_curpos) - var old_nei = table_copy(nei_cell) - #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) +# old function tested with the khepera for reference +# DS 20/11 +# function getneiobs (m_curpos) { +# var foundobstacle = 0 +# cur_cell = getcell(m_curpos) +# var old_nei = table_copy(nei_cell) - neighbors.foreach(function(rid, data) { - #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg") - Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) - #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") - Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) - nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} - #log("Neighbor in : ", Ncell.x, " ", Ncell.y) - if(old_nei[rid]!=nil) { - if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { - #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - } else { - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - }) - - #if(foundobstacle) { - #print_map(map) - #} - - return foundobstacle -} +# neighbors.foreach(function(rid, data) { +# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) +# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) +# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) +# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} +# if(old_nei[rid]!=nil) { +# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# } else { +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# }) +# return foundobstacle +# } +# populate a line in front of the sensor using plateform independant proximity sensing function getproxobs (m_curpos) { - foundobstacle = 0 + var foundobstacle = 0 cur_cell = getcell(m_curpos) reduce(proximity, @@ -125,7 +139,7 @@ function getproxobs (m_curpos) { obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value > IR_SENSOR_THRESHOLD) { + if(value.value > PROX_SENSOR_THRESHOLD) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) { add_obstacle(obs, 0, 0.25) add_obstacle(obs2, 0, 0.25) @@ -142,15 +156,6 @@ function getproxobs (m_curpos) { return acc }, math.vec2.new(0, 0)) - #if(foundobstacle) { - # reduce(proximity, - # function(key, value, acc){ - # log(value.value, ", ", value.angle) - # return acc - # }, math.vec2.new(0, 0)) - # print_map(map) - #} - return foundobstacle } @@ -176,29 +181,16 @@ function check_path(m_path, goal_l, m_curpos, kh4) { return pathisblocked } -function RRTSTAR(GOAL,TOL) { - HEIGHT = size(map) - WIDTH = size(map[1]) - RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive +function rrtstar() { + # update state machine variables + statef = rrtstar + BVMSTATE = "RRTSTAR" - - var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} - #table_print(goalBoundary) - var numberOfPoints = 1 - var arrayOfPoints = {} - arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} - Path = {} - mat_copyrow(Path,1,arrayOfPoints,1) - # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance - Q = {} - Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} - - goalReached = 0; - timeout = 0 ## # main search loop ## - while(goalReached == 0 and timeout < RRT_TIMEOUT) { + step_timeout = 0 + while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { # Point generation only as grid cell centers pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) @@ -313,15 +305,19 @@ function RRTSTAR(GOAL,TOL) { log(numberOfPoints, " points processed. Still looking for goal."); } timeout = timeout + 1 + step_timeout = step_timeout + 1 } + if(goalReached){ log("Goal found(",numberOfPoints,")!") Path = getPath(Q,numberOfPoints, 1) + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef } else { - log("FAILED TO FIND A PATH!!!") + #log("FAILED TO FIND A PATH!!!") Path = nil } - return Path } function findClosestPoint(point,aPt) { diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index d3b015d..10f1cfd 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -7,7 +7,7 @@ include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. -UAVSTATE = "TURNEDOFF" +BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 2 # m/steps GOTO_MAXDIST = 150 # m. @@ -23,16 +23,16 @@ function uav_initswarm() { function turnedoff() { statef=turnedoff - UAVSTATE = "TURNEDOFF" + BVMSTATE = "TURNEDOFF" } function idle() { statef=idle - UAVSTATE = "IDLE" + BVMSTATE = "IDLE" } function takeoff() { - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" statef=takeoff homegps = {.lat=position.latitude, .long=position.longitude} @@ -47,7 +47,7 @@ function takeoff() { } function land() { - UAVSTATE = "LAND" + BVMSTATE = "LAND" statef=land neighbors.broadcast("cmd", 21) @@ -60,7 +60,7 @@ function land() { } function set_goto(transf) { - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef=function() { gotoWPRRT(transf) } @@ -79,7 +79,7 @@ function set_goto(transf) { ptime=0 function picture() { statef=picture - UAVSTATE="PICTURE" + BVMSTATE="PICTURE" uav_setgimbal(0.0, 0.0, -90.0, 20.0) if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() @@ -98,50 +98,45 @@ function gotoWPRRT(transf) { m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) - if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) + if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) { log("Sorry this is too far.") - else { - if(Path==nil){ - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = position.latitude - homegps.long = position.longitude + return + } + # create the map + if(map==nil) { + dyn_init_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else if(cur_goal_l <= size(Path)) { + var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude + var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) + print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) + if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, cur_goal_l, m_pos, 0)) { + uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + Path = nil + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else { + cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) } - # add proximity sensor and neighbor obstacles to the map - while(Path==nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - updateMap(m_pos) - if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) - Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - while(Path == nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) - } - } - else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } + } else + cur_goal_l = cur_goal_l + 1 + } else { + Path = nil + transf() } } @@ -162,7 +157,7 @@ function gotoWP(transf) { function follow() { if(size(targets)>0) { - UAVSTATE = "FOLLOW" + BVMSTATE = "FOLLOW" statef=follow attractor=math.vec2.newp(0,0) foreach(targets, function(id, tab) { @@ -181,18 +176,18 @@ function uav_rccmd() { log("cmd 22") flight.rc_cmd=0 statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { log("cmd 21") log("To land") flight.rc_cmd=0 statef = land - UAVSTATE = "LAND" + BVMSTATE = "LAND" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 @@ -227,16 +222,16 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") - if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { statef=takeoff - UAVSTATE = "TAKEOFF" - } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { + BVMSTATE = "TAKEOFF" + } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { statef=land - UAVSTATE = "LAND" - } else if(value==400 and UAVSTATE=="TURNEDOFF") { + BVMSTATE = "LAND" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { uav_arm() - } else if(value==401 and UAVSTATE=="TURNEDOFF"){ + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ uav_disarm() } else if(value==900){ rc_State = 0 @@ -246,7 +241,7 @@ function uav_neicmd() { rc_State = 2 } else if(value==903){ rc_State = 3 - } else if(value==16 and UAVSTATE=="IDLE"){ + } else if(value==16 and BVMSTATE=="IDLE"){ # 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/testRRT.bzz b/buzz_scripts/testRRT.bzz index 7590470..7478529 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -16,7 +16,7 @@ function init() { #statef=turnedoff #UAVSTATE = "TURNEDOFF" statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -25,7 +25,7 @@ function step() { statef() - log("Current state: ", UAVSTATE) + log("Current state: ", BVMSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a58c8f2..c61d9a3 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -480,7 +480,7 @@ string getuavstate() ---------------------------------------*/ { static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); From df11060f84b269a57db13b359e853b46c969aa94 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 14:51:00 -0500 Subject: [PATCH 20/98] add comments and TODOs --- buzz_scripts/include/rrtstar.bzz | 43 +++++++++++++++++++------------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 86d1ce0..89fb106 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -15,6 +15,7 @@ nei_cell = {} mapgoal = {} # create a map with its length to fit the goal (rel. pos.) and the current position as the center +# TODO: test with khepera/argos function dyn_init_map(m_navigation) { # create a map big enough for the goal init_map(math.round(2*math.vec2.length(m_navigation))+10) @@ -23,6 +24,7 @@ function dyn_init_map(m_navigation) { } # start the RRT* to reach the goal (rel. pos.) +# TODO: test with khepera/argos function pathPlanner(m_goal, m_pos, kh4) { # place the robot on the grid cur_cell = getcell(m_pos) @@ -53,6 +55,7 @@ function pathPlanner(m_goal, m_pos, kh4) { rrtstar() } +# get the grid cell position (x,y) equivalent to the input position function getcell(m_curpos) { var cell = {} # relative to center (start position) @@ -78,7 +81,7 @@ function populateGrid(m_pos) { getneiobs (m_pos) } -# TODO: populate the map with a blob instead? +# TODO: populate the map with neighors as blobs instead ? function getneiobs (m_curpos) { cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid @@ -126,6 +129,7 @@ function getneiobs (m_curpos) { # } # populate a line in front of the sensor using plateform independant proximity sensing +# TODO: adapt to M100 stereo function getproxobs (m_curpos) { var foundobstacle = 0 cur_cell = getcell(m_curpos) @@ -159,6 +163,8 @@ function getproxobs (m_curpos) { return foundobstacle } +# check if any obstacle blocks the way +# TODO: remove the kh4 bool for retro-compatilibty function check_path(m_path, goal_l, m_curpos, kh4) { var pathisblocked = 0 var nb = goal_l @@ -181,14 +187,15 @@ function check_path(m_path, goal_l, m_curpos, kh4) { return pathisblocked } +## +# main search loop as a state +## function rrtstar() { # update state machine variables statef = rrtstar BVMSTATE = "RRTSTAR" - ## - # main search loop - ## + step_timeout = 0 while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { @@ -309,20 +316,23 @@ function rrtstar() { } if(goalReached){ - log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints, 1) - # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef - } else { - #log("FAILED TO FIND A PATH!!!") + log("Goal found(",numberOfPoints,")!") + Path = getPath(Q,numberOfPoints, 1) + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef + } else if(timeout >= RRT_TIMEOUT) { + log("FAILED TO FIND A PATH!!!") Path = nil + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef } } +# Go through each points and find the distances between them and the +# target point function findClosestPoint(point,aPt) { - # Go through each points and find the distances between them and the - # target point var distance = 999 var pointNb = -1 var ifcp=1 @@ -388,6 +398,7 @@ function doesItIntersect(point,vector) { var yi = math.round(pos_chk.y) #+1 #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") + # TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ #if(xi!=cur_cell.x and yi!=cur_cell.y) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { @@ -408,7 +419,6 @@ function doesItIntersect(point,vector) { # create a table with only the path's points in order function getPath(Q,nb,gps){ - #log("-----------CONVERTING PATH!!!") var pathL={} var npt=0 # get the path from the point list @@ -419,14 +429,13 @@ function getPath(Q,nb,gps){ pathL[npt][2]=Q[nb][2] if(nb != Q[Q[nb][3]][3]) nb = Q[nb][3]; - else { + else { # TODO: why is this happening? log("ERROR - Recursive path !!!") return nil } } - # log("re-order the list") - # table_print(path.mat) + # Re-Order the list. var pathR={} var totpt = npt + 1 while(npt > 0){ From b30bd6201830b383635e119b61f564db6ebae4fc Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 20 Dec 2017 16:10:30 -0500 Subject: [PATCH 21/98] fix proxmity topic --- buzz_scripts/testRRT.bzz | 14 ++++++++++---- launch/launch_config/topics.yaml | 1 + src/roscontroller.cpp | 9 +++++---- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 7478529..755bc4f 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -13,10 +13,10 @@ function action() { function init() { uav_initstig() uav_initswarm() - #statef=turnedoff - #UAVSTATE = "TURNEDOFF" - statef = takeoff - BVMSTATE = "TAKEOFF" + statef=turnedoff + BVMSTATE = "TURNEDOFF" + #statef = takeoff + #BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -26,6 +26,12 @@ function step() { statef() log("Current state: ", BVMSTATE) + log("Obstacles: ") + reduce(proximity, + function(key, value, acc) { + log(key, " - ", value.angle, value.value) + return acc + }, math.vec2.new(0, 0)) } # Executed once when the robot (or the simulator) is reset. diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index e7358cd..9e33d37 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -10,3 +10,4 @@ topics: localpos: local_position/pose stream: set_stream_rate altitude: global_position/rel_alt + obstacles: obstacles diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2bf5cab..11d5913 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -318,7 +318,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) system("rosnode kill rosbuzz_node"); } - node_handle.getParam("obstacles", obstacles_topic); + node_handle.getParam("topics/obstacles", obstacles_topic); } void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) @@ -468,7 +468,7 @@ void roscontroller::grid_publisher() grid_msg.header.frame_id = "/world"; grid_msg.header.stamp = current_time; grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. - grid_msg.info.resolution = 0.1;//gridMap.getResolution(); + grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; grid_msg.info.origin.position.x = 0.0; @@ -483,8 +483,9 @@ void roscontroller::grid_publisher() for (itr=grid.begin(); itr!=grid.end(); ++itr) { std::map::iterator itc = itr->second.begin(); for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { - //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, round(itc->second*100.0)); - grid_msg.data[(itr->first-1)*g_w+itc->first] = round(itc->second*100.0); + grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second; + // DEBUG + //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]); } } grid_pub.publish(grid_msg); From 292c4fda8a53446cd67cf34762c7ab5e442e9000 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 12:20:26 -0500 Subject: [PATCH 22/98] fix subscriber format, added NED yaw, changed position to absolute_position and fixed proximity obstacles detection --- buzz_scripts/include/rrtstar.bzz | 18 ++--- buzz_scripts/include/uavstates.bzz | 32 ++++----- buzz_scripts/testRRT.bzz | 24 ++++--- include/buzzuav_closures.h | 2 +- include/roscontroller.h | 56 +++++++-------- src/buzzuav_closures.cpp | 11 ++- src/roscontroller.cpp | 108 ++++++++++++++++------------- 7 files changed, 131 insertions(+), 120 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 89fb106..4e3c138 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -7,7 +7,7 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 -PROX_SENSOR_THRESHOLD = 0.11 +PROX_SENSOR_THRESHOLD_M = 10 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} @@ -77,7 +77,7 @@ function getcell(m_curpos) { } function populateGrid(m_pos) { - #getproxobs(m_pos) + getproxobs(m_pos) getneiobs (m_pos) } @@ -131,19 +131,19 @@ function getneiobs (m_curpos) { # populate a line in front of the sensor using plateform independant proximity sensing # TODO: adapt to M100 stereo function getproxobs (m_curpos) { - var foundobstacle = 0 + var updated_obstacle = 0 cur_cell = getcell(m_curpos) reduce(proximity, function(key, value, acc) { - obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) - obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw)) + obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw)) per = math.vec2.sub(obs,cur_cell) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value > PROX_SENSOR_THRESHOLD) { + if(value.value < PROX_SENSOR_THRESHOLD_M) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) { add_obstacle(obs, 0, 0.25) add_obstacle(obs2, 0, 0.25) @@ -151,16 +151,16 @@ function getproxobs (m_curpos) { add_obstacle(obsr2, 0, 0.25) add_obstacle(obsl, 0, 0.25) add_obstacle(obsl2, 0, 0.25) - foundobstacle = 1 + updated_obstacle = 1 } } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) { remove_obstacle(obs, 0, 0.05) - foundobstacle = 1 + updated_obstacle = 1 } return acc }, math.vec2.new(0, 0)) - return foundobstacle + return updated_obstacle } # check if any obstacle blocks the way diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 10f1cfd..8e347bf 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -34,13 +34,13 @@ function idle() { function takeoff() { BVMSTATE = "TAKEOFF" statef=takeoff - homegps = {.lat=position.latitude, .long=position.longitude} + homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude} - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", position.altitude) + log("Altitude: ", absolute_position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -105,8 +105,8 @@ function gotoWPRRT(transf) { # create the map if(map==nil) { dyn_init_map(rc_goal) - homegps.lat = position.latitude - homegps.long = position.longitude + homegps.lat = absolute_position.latitude + homegps.long = absolute_position.longitude } if(Path==nil) { @@ -122,7 +122,7 @@ function gotoWPRRT(transf) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { populateGrid(m_pos) if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude) Path = nil rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) # re-start the planner @@ -130,7 +130,7 @@ function gotoWPRRT(transf) { cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude) } } else cur_goal_l = cur_goal_l + 1 @@ -148,11 +148,11 @@ function gotoWP(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) } function follow() { @@ -260,8 +260,8 @@ function LimitAngle(angle){ } function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - position.longitude - d_lat = lat - position.latitude + d_lon = lon - absolute_position.longitude + d_lat = lat - absolute_position.latitude if(home_ref == 1) { d_lon = lon - homegps.long d_lat = lat - homegps.lat @@ -277,17 +277,17 @@ function gps_from_vec(vec) { Lgoal = {.latitude=0, .longitude=0} Vrange = math.vec2.length(vec) Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) - latR = position.latitude*math.pi/180.0; - lonR = position.longitude*math.pi/180.0; +# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude) + latR = absolute_position.latitude*math.pi/180.0; + lonR = absolute_position.longitude*math.pi/180.0; target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + position.latitude; + #goal.latitude = d_lat + absolute_position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + position.longitude; + #goal.longitude = d_lon + absolute_position.longitude; return Lgoal } diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 755bc4f..f586fe5 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -5,7 +5,7 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5088103899,-73.1540826153,25.0) + uav_storegoal(45.5088103899,-73.1540826153,5.0) set_goto(idle) } @@ -13,10 +13,12 @@ function action() { function init() { uav_initstig() uav_initswarm() - statef=turnedoff - BVMSTATE = "TURNEDOFF" - #statef = takeoff - #BVMSTATE = "TAKEOFF" + + TARGET_ALTITUDE = 5.0 # m. + # statef=turnedoff + # BVMSTATE = "TURNEDOFF" + statef = takeoff + BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -26,12 +28,12 @@ function step() { statef() log("Current state: ", BVMSTATE) - log("Obstacles: ") - reduce(proximity, - function(key, value, acc) { - log(key, " - ", value.angle, value.value) - return acc - }, math.vec2.new(0, 0)) + # log("Obstacles: ") + # reduce(proximity, + # function(key, value, acc) { + # log(key, " - ", value.angle, value.value) + # return acc + # }, math.vec2.new(0, 0)) } # Executed once when the robot (or the simulator) is reset. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f14e1a7..e6c3f48 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -89,7 +89,7 @@ void set_filtered_packet_loss(float value); /* * sets current position */ -void set_currentpos(double latitude, double longitude, double altitude); +void set_currentpos(double latitude, double longitude, float altitude, float yaw); /* * returns the current go to position */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 0e9e547..7599c58 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -1,5 +1,6 @@ #pragma once #include +#include #include #include #include @@ -51,7 +52,6 @@ class roscontroller public: roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); ~roscontroller(); - // void RosControllerInit(); void RosControllerRun(); static const string CAPTURE_SRV_DEFAULT_NAME; @@ -70,25 +70,27 @@ private: Num_robot_count count_robots; - struct gps + struct POSE { double longitude = 0.0; double latitude = 0.0; float altitude = 0.0; + // NED coordinates + float x = 0.0; + float y = 0.0; + float z = 0.0; + float yaw = 0.0; }; - typedef struct gps GPS; + typedef struct POSE ros_pose; - GPS target, home, cur_pos; - double cur_rel_altitude; + ros_pose target, home, cur_pos; uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; - // std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; int timer_step = 0; int robot_id = 0; std::string robot_name = ""; - // int oldcmdID=0; int rc_cmd; float fcu_timeout; @@ -96,7 +98,9 @@ private: int barrier; int message_number = 0; uint8_t no_of_robots = 0; - /*tmp to be corrected*/ + bool rcclient; + bool xbeeplugged = false; + bool multi_msg; uint8_t no_cnt = 0; uint8_t old_val = 0; std::string bzzfile_name; @@ -107,20 +111,18 @@ private: std::string bcfname, dbgfname; std::string out_payload; std::string in_payload; - std::string obstacles_topic; std::string stand_by; std::string xbeesrv_name; std::string capture_srv_name; std::string setpoint_name; std::string stream_client_name; - std::string relative_altitude_sub_name; std::string setpoint_nonraw; - bool rcclient; - bool xbeeplugged = false; - bool multi_msg; + + // ROS service, publishers and subscribers ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; ros::ServiceClient capture_srv; + ros::ServiceClient stream_client; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; @@ -137,23 +139,16 @@ private: ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; - - std::string local_pos_sub_name; ros::Subscriber local_pos_sub; - double local_pos_new[3]; - ros::ServiceClient stream_client; + std::map m_smTopic_infos; int setpoint_counter; - double my_x = 0, my_y = 0; std::ofstream log; - /*Commands for flight controller*/ - // mavros_msgs::CommandInt cmd_srv; + // Commands for flight controller mavros_msgs::CommandLong cmd_srv; - std::vector m_sMySubscriptions; - std::map m_smTopic_infos; mavros_msgs::CommandBool m_cmdBool; ros::ServiceClient arm_client; @@ -161,7 +156,7 @@ private: mavros_msgs::SetMode m_cmdSetMode; ros::ServiceClient mode_client; - /*Initialize publisher and subscriber, done in the constructor*/ + // Initialize publisher and subscriber, done in the constructor void Initialize_pub_sub(ros::NodeHandle& n_c); std::string current_mode; @@ -204,8 +199,8 @@ private: /*convert from spherical to cartesian coordinate system callback */ float constrainAngle(float x); - void gps_rb(GPS nei_pos, double out[]); - void gps_ned_cur(float& ned_x, float& ned_y, GPS t); + void gps_rb(POSE nei_pos, double out[]); + void gps_ned_cur(float& ned_x, float& ned_y, POSE t); void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat); @@ -219,10 +214,10 @@ private: void flight_status_update(const mavros_msgs::State::ConstPtr& msg); /*current position callback*/ - void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); + void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg); /*current relative altitude callback*/ - void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); + void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg); /*payload callback callback*/ void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); @@ -234,7 +229,7 @@ private: void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); /*Obstacle distance table callback*/ - void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); + void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg); /*Get publisher and subscriber from YML file*/ void GetSubscriptionParameters(ros::NodeHandle& node_handle); @@ -250,8 +245,6 @@ private: void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); - // void WaypointMissionSetup(float lat, float lng, float alt); - void fc_command_setup(); void SetLocalPosition(float x, float y, float z, float yaw); @@ -262,6 +255,7 @@ private: void get_number_of_robots(); + // functions related to Xbee modules information void GetRobotId(); bool GetDequeFull(bool& result); bool GetRssi(float& result); @@ -269,7 +263,7 @@ private: bool GetAPIRssi(const uint8_t short_id, float& result); bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); - void get_xbee_status(); + }; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index c61d9a3..425c7f1 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -18,7 +18,7 @@ static double rc_goto_pos[3]; static float rc_gimbal[4]; static float batt[3]; static float obst[5] = { 0, 0, 0, 0, 0 }; -static double cur_pos[3]; +static double cur_pos[4]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd = 0; @@ -637,7 +637,7 @@ int buzzuav_update_xbee_status(buzzvm_t vm) return vm->state; } -void set_currentpos(double latitude, double longitude, double altitude) +void set_currentpos(double latitude, double longitude, float altitude, float yaw) /* / update interface position array -----------------------------------*/ @@ -645,6 +645,7 @@ void set_currentpos(double latitude, double longitude, double altitude) cur_pos[0] = latitude; cur_pos[1] = longitude; cur_pos[2] = altitude; + cur_pos[3] = yaw; } // adds neighbours position void neighbour_pos_callback(int id, float range, float bearing, float elevation) @@ -677,7 +678,7 @@ int buzzuav_update_currentpos(buzzvm_t vm) / Update the BVM position table /------------------------------------------------------*/ { - buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); @@ -691,6 +692,10 @@ int buzzuav_update_currentpos(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1)); + buzzvm_pushf(vm, cur_pos[3]); + buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 11d5913..a59aac4 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -249,30 +249,37 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) /Obtains publisher, subscriber and services from yml config file /-----------------------------------------------------------------------------------*/ { - m_sMySubscriptions.clear(); - std::string gps_topic; - if (node_handle.getParam("topics/gps", gps_topic)) + std::string topic; + if (node_handle.getParam("topics/gps", topic)) ; else { - ROS_ERROR("Provide a gps topic in Launch file"); + ROS_ERROR("Provide a gps topic in YAML file"); system("rosnode kill rosbuzz_node"); } - m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/NavSatFix")); + if (node_handle.getParam("topics/localpos", topic)) + ; + else + { + ROS_ERROR("Provide a localpos name in YAML file"); + system("rosnode kill rosbuzz_node"); + } + m_smTopic_infos.insert(pair(topic, "geometry_msgs/PoseStamped")); - std::string battery_topic; - node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); + node_handle.getParam("topics/obstacles", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/LaserScan")); - std::string status_topic; - node_handle.getParam("topics/status", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); - node_handle.getParam("topics/estatus", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); + node_handle.getParam("topics/battery", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); - std::string altitude_topic; - node_handle.getParam("topics/altitude", altitude_topic); - m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); + node_handle.getParam("topics/status", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/ExtendedState")); + + node_handle.getParam("topics/altitude", topic); + m_smTopic_infos.insert(pair(topic, "std_msgs/Float64")); // Obtain required topic and service names from the parameter server if (node_handle.getParam("topics/fcclient", fcclient_name)) @@ -310,15 +317,6 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } - if (node_handle.getParam("topics/localpos", local_pos_sub_name)) - ; - else - { - ROS_ERROR("Provide a localpos name in YAML file"); - system("rosnode kill rosbuzz_node"); - } - - node_handle.getParam("topics/obstacles", obstacles_topic); } void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) @@ -332,8 +330,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); - // publishers payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); @@ -353,8 +349,6 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) capture_srv = n_c.serviceClient(capture_srv_name); stream_client = n_c.serviceClient(stream_client_name); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); - multi_msg = true; } @@ -379,11 +373,19 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c) } else if (it->second == "sensor_msgs/NavSatFix") { - current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::global_gps_callback, this); } else if (it->second == "std_msgs/Float64") { - relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::rel_alt_callback, this); + } + else if (it->second == "geometry_msgs/PoseStamped") + { + local_pos_sub = n_c.subscribe(it->first, 5, &roscontroller::local_pos_callback, this); + } + else if (it->second == "sensor_msgs/LaserScan") + { + obstacle_sub = n_c.subscribe(it->first, 5, &roscontroller::obstacle_dist_callback, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -798,7 +800,7 @@ float roscontroller::constrainAngle(float x) return x; } -void roscontroller::gps_rb(GPS nei_pos, double out[]) +void roscontroller::gps_rb(POSE nei_pos, double out[]) /* / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates @@ -814,7 +816,7 @@ void roscontroller::gps_rb(GPS nei_pos, double out[]) out[2] = 0.0; } -void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, GPS t) +void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) /* / Get GPS from NED and a reference GPS point (struct input) ----------------------------------------------------------- */ @@ -868,39 +870,47 @@ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedSta buzzuav_closures::flight_status_update(msg->landed_state); } -void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) +void roscontroller::global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) /* / Update current GPS position into BVM from subscriber /-------------------------------------------------------------*/ { - // DEBUG - // ROS_INFO("Altitude out: %f", cur_rel_altitude); + // reset timeout counter fcu_timeout = TIMEOUT; - set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); - buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); + set_cur_pos(msg->latitude, msg->longitude, cur_pos.z); + buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_pos.z, cur_pos.yaw); } -void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose) +void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) /* / Update current position for flight controller NED offset /-------------------------------------------------------------*/ { - local_pos_new[0] = pose->pose.position.x; - local_pos_new[1] = pose->pose.position.y; - local_pos_new[2] = pose->pose.position.z; + cur_pos.x = msg->pose.position.x; + cur_pos.y = msg->pose.position.y; + // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead + tf::Quaternion q( + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z, + msg->pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + cur_pos.yaw = yaw; } -void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg) +void roscontroller::rel_alt_callback(const std_msgs::Float64::ConstPtr& msg) /* / Update altitude into BVM from subscriber /-------------------------------------------------------------*/ { // DEBUG // ROS_INFO("Altitude in: %f", msg->data); - cur_rel_altitude = (double)msg->data; + cur_pos.z = (double)msg->data; } -void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg) +void roscontroller::obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg) /* /Set obstacle Obstacle distance table into BVM from subscriber /-------------------------------------------------------------*/ @@ -925,9 +935,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.header.frame_id = 1; // DEBUG - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); - moveMsg.pose.position.x = local_pos_new[0] + y; - moveMsg.pose.position.y = local_pos_new[1] + x; + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y); + moveMsg.pose.position.x = cur_pos.x + y; + moveMsg.pose.position.y = cur_pos.y + x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -1038,7 +1048,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1], neighbours_pos_payload[2]); - GPS nei_pos; + POSE nei_pos; nei_pos.latitude = neighbours_pos_payload[0]; nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; From 4e4bacb25d40e490639036d921c75f036afc734d Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 13:28:06 -0500 Subject: [PATCH 23/98] changed absolute_position to fit argos implementation --- buzz_scripts/include/rrtstar.bzz | 4 +-- buzz_scripts/include/uavstates.bzz | 32 +++++++++++----------- src/buzzuav_closures.cpp | 44 +++++++++++++++++++++++------- 3 files changed, 52 insertions(+), 28 deletions(-) diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 4e3c138..7cc5235 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -136,8 +136,8 @@ function getproxobs (m_curpos) { reduce(proximity, function(key, value, acc) { - obs = getcell(math.vec2.newp(value.value, value.angle + absolute_position.yaw)) - obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + absolute_position.yaw)) + obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw)) + obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw)) per = math.vec2.sub(obs,cur_cell) obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 8e347bf..316d281 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -34,13 +34,13 @@ function idle() { function takeoff() { BVMSTATE = "TAKEOFF" statef=takeoff - homegps = {.lat=absolute_position.latitude, .long=absolute_position.longitude} + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} - if( flight.status == 2 and absolute_position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", absolute_position.altitude) + log("Altitude: ", pose.position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -105,8 +105,8 @@ function gotoWPRRT(transf) { # create the map if(map==nil) { dyn_init_map(rc_goal) - homegps.lat = absolute_position.latitude - homegps.long = absolute_position.longitude + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude } if(Path==nil) { @@ -122,7 +122,7 @@ function gotoWPRRT(transf) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { populateGrid(m_pos) if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude - absolute_position.altitude) + uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude) Path = nil rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) # re-start the planner @@ -130,7 +130,7 @@ function gotoWPRRT(transf) { cur_goal_l = 1 } else { cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude) } } else cur_goal_l = cur_goal_l + 1 @@ -148,11 +148,11 @@ function gotoWP(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - absolute_position.altitude) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } function follow() { @@ -260,8 +260,8 @@ function LimitAngle(angle){ } function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - absolute_position.longitude - d_lat = lat - absolute_position.latitude + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude if(home_ref == 1) { d_lon = lon - homegps.long d_lat = lat - homegps.lat @@ -277,17 +277,17 @@ function gps_from_vec(vec) { Lgoal = {.latitude=0, .longitude=0} Vrange = math.vec2.length(vec) Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing, absolute_position.latitude, absolute_position.longitude) - latR = absolute_position.latitude*math.pi/180.0; - lonR = absolute_position.longitude*math.pi/180.0; +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); Lgoal.latitude = target_lat*180.0/math.pi; Lgoal.longitude = target_lon*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + absolute_position.latitude; + #goal.latitude = d_lat + pose.position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + absolute_position.longitude; + #goal.longitude = d_lon + pose.position.longitude; return Lgoal } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 425c7f1..3c58403 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -678,25 +678,49 @@ int buzzuav_update_currentpos(buzzvm_t vm) / Update the BVM position table /------------------------------------------------------*/ { - buzzvm_pushs(vm, buzzvm_string_register(vm, "absolute_position", 1)); + buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1)); buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzobj_t tPoseTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tPosition = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 0)); buzzvm_pushf(vm, cur_pos[0]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0)); buzzvm_pushf(vm, cur_pos[1]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 1)); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); + buzzvm_push(vm, tPosition); + buzzvm_tput(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tOrientation = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tOrientation); + buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 0)); buzzvm_pushf(vm, cur_pos[3]); buzzvm_tput(vm); - buzzvm_gstore(vm); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "orientation", 0)); + buzzvm_push(vm, tOrientation); + buzzvm_tput(vm); + return vm->state; } From a626c556d99642191d9e311a9574ed27fb6bc370 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 21 Dec 2017 21:41:57 -0500 Subject: [PATCH 24/98] add collision boxes for gazebo buildings and debugged collision detection for rrt --- buzz_scripts/include/mapmatrix.bzz | 6 +-- buzz_scripts/include/rrtstar.bzz | 76 +++++++++++++++++++----------- buzz_scripts/include/uavstates.bzz | 2 +- buzz_scripts/testRRT.bzz | 4 +- src/buzzuav_closures.cpp | 3 +- src/roscontroller.cpp | 4 +- 6 files changed, 58 insertions(+), 37 deletions(-) diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index 79ad775..c3d3393 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -56,7 +56,7 @@ function add_obstacle(pos, off, inc_trust) { var yi = math.round(pos.y) if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { - #log("Add obstacle in cell: ", xi, " ", yi) + # log("Add obstacle in cell: ", xi, " ", yi) var old=map[xi][yi] if(old-inc_trust > 0.0) map[xi][yi] = old-inc_trust @@ -69,8 +69,8 @@ function remove_obstacle(pos, off, dec_trust) { var xi = math.round(pos.x) var yi = math.round(pos.y) - if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ - #log("Remove obstacle in cell: ", xi, " ", yi) + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { + # log("Remove obstacle in cell: ", xi, " ", yi) var old=map[xi][yi] if(old + dec_trust < 1.0) #x,y map[xi][yi] = old+dec_trust diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 7cc5235..3fa686c 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -7,7 +7,7 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 -PROX_SENSOR_THRESHOLD_M = 10 +PROX_SENSOR_THRESHOLD_M = 8.0 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} @@ -46,7 +46,7 @@ function pathPlanner(m_goal, m_pos, kh4) { arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance Q = {} - Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0} goalReached = 0 timeout = 0 @@ -56,6 +56,7 @@ function pathPlanner(m_goal, m_pos, kh4) { } # get the grid cell position (x,y) equivalent to the input position +# input should be relative to home position (planing start point) function getcell(m_curpos) { var cell = {} # relative to center (start position) @@ -79,6 +80,7 @@ function getcell(m_curpos) { function populateGrid(m_pos) { getproxobs(m_pos) getneiobs (m_pos) + export_map(map) } # TODO: populate the map with neighors as blobs instead ? @@ -136,26 +138,43 @@ function getproxobs (m_curpos) { reduce(proximity, function(key, value, acc) { - obs = getcell(math.vec2.newp(value.value, value.angle + pose.orientation.yaw)) - obs2 = getcell(math.vec2.newp(value.value + 1.0, value.angle + pose.orientation.yaw)) - per = math.vec2.sub(obs,cur_cell) - obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) - obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) - obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) - obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value < PROX_SENSOR_THRESHOLD_M) { - if(map[math.round(obs.x)][math.round(obs.y)]!=0) { - add_obstacle(obs, 0, 0.25) - add_obstacle(obs2, 0, 0.25) - add_obstacle(obsr, 0, 0.25) - add_obstacle(obsr2, 0, 0.25) - add_obstacle(obsl, 0, 0.25) - add_obstacle(obsl2, 0, 0.25) - updated_obstacle = 1 + if (value.angle != -1) { # down sensor of M100 + if(value.value < PROX_SENSOR_THRESHOLD_M) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs))) + # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) + # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) + if(map[math.round(obs.x)][math.round(obs.y)]!=0) { + add_obstacle(obs, 0, 0.25) + add_obstacle(obsr, 0, 0.25) + add_obstacle(obsl, 0, 0.25) + add_obstacle(obsr2, 0, 0.25) + add_obstacle(obsl2, 0, 0.25) + # add_obstacle(obs2, 0, 0.25) + # add_obstacle(obsr2, 0, 0.25) + # add_obstacle(obsl2, 0, 0.25) + updated_obstacle = 1 + } + } else { + var line_length = PROX_SENSOR_THRESHOLD_M; + while(line_length > 0) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + remove_obstacle(obs, 0, 0.05) + remove_obstacle(obsr, 0, 0.05) + remove_obstacle(obsl, 0, 0.05) + line_length = line_length - 1; + } } - } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) { - remove_obstacle(obs, 0, 0.05) - updated_obstacle = 1 } return acc }, math.vec2.new(0, 0)) @@ -225,7 +244,7 @@ function rrtstar() { nbCount = nbCount + 1; if(intersects != 1) { #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") - var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5] if(distance < minCounted) { minCounted = distance; @@ -422,17 +441,18 @@ function getPath(Q,nb,gps){ var pathL={} var npt=0 # get the path from the point list - while(nb > 1) { + while(nb > 0) { npt=npt+1 pathL[npt] = {} pathL[npt][1]=Q[nb][1] pathL[npt][2]=Q[nb][2] - if(nb != Q[Q[nb][3]][3]) - nb = Q[nb][3]; - else { # TODO: why is this happening? - log("ERROR - Recursive path !!!") - return nil + if( nb > 1) { + if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening? + log("ERROR - Recursive path !!!") + return nil + } } + nb = Q[nb][3]; } # Re-Order the list. diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 316d281..8a27c38 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -9,7 +9,7 @@ include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2 # m/steps +GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index f586fe5..3ad4cc6 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -5,7 +5,7 @@ include "vstigenv.bzz" function action() { statef=action - uav_storegoal(45.5088103899,-73.1540826153,5.0) + uav_storegoal(45.5088103899,-73.1540826153,2.5) set_goto(idle) } @@ -14,7 +14,7 @@ function init() { uav_initstig() uav_initswarm() - TARGET_ALTITUDE = 5.0 # m. + TARGET_ALTITUDE = 2.5 # m # statef=turnedoff # BVMSTATE = "TURNEDOFF" statef = takeoff diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 3c58403..20e0456 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -179,6 +179,7 @@ int buzz_exportmap(buzzvm_t vm) / Buzz closure to export a 2D map /----------------------------------------*/ { + grid.clear(); buzzvm_lnum_assert(vm, 1); // Get the parameter buzzvm_lload(vm, 1); @@ -193,7 +194,7 @@ int buzz_exportmap(buzzvm_t vm) buzzvm_dup(vm); buzzvm_pushi(vm, j); buzzvm_tget(vm); - row.insert(std::pair(j,round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + row.insert(std::pair(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0))); buzzvm_pop(vm); } grid.insert(std::pair>(i,row)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index a59aac4..cfd47c0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -473,8 +473,8 @@ void roscontroller::grid_publisher() grid_msg.info.resolution = 0.01;//gridMap.getResolution(); grid_msg.info.width = g_w; grid_msg.info.height = g_h; - grid_msg.info.origin.position.x = 0.0; - grid_msg.info.origin.position.y = 0.0; + grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution; + grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution; grid_msg.info.origin.position.z = 0.0; grid_msg.info.origin.orientation.x = 0.0; grid_msg.info.origin.orientation.y = 0.0; From 6cd162bff4be2c7aca47e2e692437c89082ff0eb Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 22 Dec 2017 16:48:39 -0500 Subject: [PATCH 25/98] restructured bzz for path planning --- buzz_scripts/include/{ => act}/barrier.bzz | 28 +- buzz_scripts/include/act/states.bzz | 234 ++++++++++++++ buzz_scripts/include/{ => plan}/mapmatrix.bzz | 0 buzz_scripts/include/{ => plan}/rrtstar.bzz | 29 +- buzz_scripts/include/shapes.bzz | 111 ------- .../taskallocate}/graphformGPS.bzz | 0 .../graphs/images}/Graph_droneL.graph | 0 .../graphs/images}/Graph_droneO.graph | 0 .../graphs/images}/Graph_droneP.graph | 0 .../graphs/images}/Graph_droneY.graph | 0 .../graphs/images}/frame_01186.png | Bin .../graphs/images}/frame_01207.png | Bin .../graphs/images}/frame_01394.png | Bin .../graphs/images}/frame_01424.png | Bin .../{ => taskallocate}/graphs/shapes_L.bzz | 0 .../{ => taskallocate}/graphs/shapes_O.bzz | 0 .../{ => taskallocate}/graphs/shapes_P.bzz | 0 .../{ => taskallocate}/graphs/shapes_Y.bzz | 0 .../graphs/shapes_square.bzz | 0 .../{graphs => taskallocate}/waypointlist.csv | 0 buzz_scripts/include/uavstates.bzz | 293 ------------------ buzz_scripts/include/utilities.bzz | 152 --------- buzz_scripts/include/utils/conversions.bzz | 63 ++++ buzz_scripts/include/{ => utils}/string.bzz | 0 buzz_scripts/include/{ => utils}/vec2.bzz | 0 buzz_scripts/include/vstigenv.bzz | 8 +- buzz_scripts/main.bzz | 81 +++++ buzz_scripts/testRRT.bzz | 45 --- src/buzz_utility.cpp | 4 +- 29 files changed, 402 insertions(+), 646 deletions(-) rename buzz_scripts/include/{ => act}/barrier.bzz (68%) create mode 100644 buzz_scripts/include/act/states.bzz rename buzz_scripts/include/{ => plan}/mapmatrix.bzz (100%) rename buzz_scripts/include/{ => plan}/rrtstar.bzz (96%) delete mode 100644 buzz_scripts/include/shapes.bzz rename buzz_scripts/{ => include/taskallocate}/graphformGPS.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneL.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneO.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneP.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/Graph_droneY.graph (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01186.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01207.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01394.png (100%) rename buzz_scripts/include/{graphs => taskallocate/graphs/images}/frame_01424.png (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_L.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_O.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_P.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_Y.bzz (100%) rename buzz_scripts/include/{ => taskallocate}/graphs/shapes_square.bzz (100%) rename buzz_scripts/include/{graphs => taskallocate}/waypointlist.csv (100%) delete mode 100644 buzz_scripts/include/uavstates.bzz delete mode 100644 buzz_scripts/include/utilities.bzz create mode 100644 buzz_scripts/include/utils/conversions.bzz rename buzz_scripts/include/{ => utils}/string.bzz (100%) rename buzz_scripts/include/{ => utils}/vec2.bzz (100%) create mode 100644 buzz_scripts/main.bzz delete mode 100644 buzz_scripts/testRRT.bzz diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/act/barrier.bzz similarity index 68% rename from buzz_scripts/include/barrier.bzz rename to buzz_scripts/include/act/barrier.bzz index 31a452e..8b56f3c 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -28,9 +28,9 @@ function barrier_create() { barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef, bdt) { +function barrier_set(threshold, transf, resumef) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -48,7 +48,7 @@ function barrier_ready() { # # Executes the barrier # -function barrier_wait(threshold, transf, resumef, bdt) { +function barrier_wait(threshold, transf, resumef) { barrier.put(id, 1) barrier.get(id) @@ -56,31 +56,13 @@ function barrier_wait(threshold, transf, resumef, bdt) { if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) timeW = 0 - transf() + BVMSTATE = transf } else if(timeW >= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") barrier=nil timeW = 0 - resumef() + BVMSTATE = resumef } - if(bdt!=-1) - neighbors.broadcast("cmd", bdt) - timeW = timeW+1 } - -# get the lowest id of the fleet, but requires too much bandwidth... -function getlowest(){ - Lid = 15; - u=15 - while(u>=0){ - tab = barrier.get(u) - if(tab!=nil){ - if(tab LOWEST ID:",Lid) -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz new file mode 100644 index 0000000..d8e3790 --- /dev/null +++ b/buzz_scripts/include/act/states.bzz @@ -0,0 +1,234 @@ +######################################## +# +# FLIGHT-RELATED FUNCTIONS +# +######################################## +include "utils/vec2.bzz" +include "plan/rrtstar.bzz" +include "act/barrier.bzz" +include "utils/conversions.bzz" + +TARGET_ALTITUDE = 15.0 # m. +BVMSTATE = "TURNEDOFF" +PICTURE_WAIT = 20 # steps +GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. +path_it = 0 +rc_State = 0 +pic_time = 0 +g_it = 0 + +function turnedoff() { + BVMSTATE = "TURNEDOFF" +} + +function idle() { + BVMSTATE = "IDLE" +} + +function launch() { + BVMSTATE = "LAUNCH" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } else { + log("Altitude: ", pose.position.altitude) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } + } else { + barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_ready() + } +} + +function stop() { + BVMSTATE = "STOP" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + neighbors.broadcast("cmd", 21) + uav_land() + if(flight.status != 2 and flight.status != 3) { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } + } else { + barrier_set(ROBOTS,"TURNEDOFF","STOP") + barrier_ready() + } +} + +function take_picture() { + BVMSTATE="PICTURE" + uav_setgimbal(0.0, 0.0, -90.0, 20.0) + if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize + uav_takepicture() + } else if(pic_time>=PICTURE_WAIT) { # wait for the picture + BVMSTATE="IDLE" + pic_time=0 + } + pic_time=pic_time+1 +} + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + +# function goto_direct(transf) { +# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) +# print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + +# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) +# log("Sorry this is too far.") +# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity +# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination +# transf() +# else +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) +# } + +function follow() { + if(size(targets)>0) { + BVMSTATE = "FOLLOW" + attractor=math.vec2.newp(0,0) + foreach(targets, function(id, tab) { + force=(0.05)*(tab.range)^4 + attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) + }) + uav_moveto(attractor.x, attractor.y, 0.0) + } else { + log("No target in local table!") + BVMSTATE = "IDLE" + } +} + +function rc_cmd_listen() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + flight.rc_cmd=0 + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + BVMSTATE = "PATHPLAN" + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + rc_State = 0 + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + rc_State = 1 + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + rc_State = 2 + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + rc_State = 3 + neighbors.broadcast("cmd", 903) + } +} + +function nei_cmd_listen() { + neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "LAUNCH" + } else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") { + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + uav_arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + uav_disarm() + } else if(value==900){ + rc_State = 0 + } else if(value==901){ + rc_State = 1 + } else if(value==902){ + rc_State = 2 + } else if(value==903){ + rc_State = 3 + } else if(value==16 and BVMSTATE=="IDLE"){ + # 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/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz similarity index 100% rename from buzz_scripts/include/mapmatrix.bzz rename to buzz_scripts/include/plan/mapmatrix.bzz diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz similarity index 96% rename from buzz_scripts/include/rrtstar.bzz rename to buzz_scripts/include/plan/rrtstar.bzz index 3fa686c..6d40626 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -3,7 +3,7 @@ # # map table-based matrix ##### -include "mapmatrix.bzz" +include "plan/mapmatrix.bzz" RRT_TIMEOUT = 500 RRT_RUNSTEP = 3 @@ -78,7 +78,7 @@ function getcell(m_curpos) { } function populateGrid(m_pos) { - getproxobs(m_pos) + # getproxobs(m_pos) getneiobs (m_pos) export_map(map) } @@ -211,8 +211,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) { ## function rrtstar() { # update state machine variables - statef = rrtstar - BVMSTATE = "RRTSTAR" + BVMSTATE = "PATHPLAN" step_timeout = 0 @@ -336,16 +335,14 @@ function rrtstar() { if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints, 1) + Path = convert_path(getPath(Q,numberOfPoints)) # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } else if(timeout >= RRT_TIMEOUT) { log("FAILED TO FIND A PATH!!!") Path = nil # done. resume the state machine - BVMSTATE = "GOTOGPS" - statef = old_statef + BVMSTATE = "NAVIGATE" } } @@ -437,7 +434,7 @@ function doesItIntersect(point,vector) { } # create a table with only the path's points in order -function getPath(Q,nb,gps){ +function getPath(Q,nb){ var pathL={} var npt=0 # get the path from the point list @@ -460,15 +457,9 @@ function getPath(Q,nb,gps){ var totpt = npt + 1 while(npt > 0){ pathR[totpt-npt] = {} - if(gps) { - tmpgoal = gps_from_vec(math.vec2.sub(getvec(pathL,npt),cur_cell)) - pathR[totpt-npt][1]=tmpgoal.latitude - pathR[totpt-npt][2]=tmpgoal.longitude - } else { - tmpgoal = getvec(path,npt) - pathR[totpt-npt][1]=tmpgoal.x - pathR[totpt-npt][2]=tmpgoal.y - } + var tmpgoal = getvec(pathL,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y npt = npt - 1 } #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz deleted file mode 100644 index 53cdd3b..0000000 --- a/buzz_scripts/include/shapes.bzz +++ /dev/null @@ -1,111 +0,0 @@ -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} -m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = 0, # Lable of the point - .Pred = -1, # Lable of its predecessor - .distance = -1, # distance to the predecessor - .bearing = -1, # bearing form the predecessor to this dot - .height = 3000, # height of this dot - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[1] = { - .Lable = 1, - .Pred = 0, - .distance = 1000, - .bearing = 0.0, - .height = 5000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[2] = { - .Lable = 2, - .Pred = 0, - .distance = 1000, - .bearing = 1.57, - .height = 7000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[3] = { - .Lable = 3, - .Pred = 0, - .distance = 1000, - .bearing = 3.14, - .height = 9000, - .State="UNASSIGNED", - .StateAge=0 -} -m_vecNodes[4] = { - .Lable = 4, - .Pred = 0, - .distance = 1000, - .bearing = 4.71, - .height = 11000, - .State="UNASSIGNED", - .StateAge=0 -} - -# -# Graph parsing -# -function parse_graph(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = string.toint(rrec[0]), # Lable of the point - .Pred = string.toint(rrec[1]), # Lable of its predecessor - .distance = string.tofloat(rrec[2]), # distance to the predecessor - .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot - .height = string.tofloat(rrec[4]), # height of this dot - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} - -function parse_graph_fixed(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2 - .Pred1 = string.toint(rrec[1]), # Pred 1 lable - .Pred2 = string.toint(rrec[3]), # Pred 2 lable - .d1 = string.tofloat(rrec[2]), # Pred 1 distance - .d2 = string.tofloat(rrec[4]), # Pred 2 distance - .Lable=string.toint(rrec[0]), - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} \ No newline at end of file diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz similarity index 100% rename from buzz_scripts/graphformGPS.bzz rename to buzz_scripts/include/taskallocate/graphformGPS.bzz diff --git a/buzz_scripts/include/graphs/Graph_droneL.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneL.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph diff --git a/buzz_scripts/include/graphs/Graph_droneO.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneO.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph diff --git a/buzz_scripts/include/graphs/Graph_droneP.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneP.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph diff --git a/buzz_scripts/include/graphs/Graph_droneY.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph similarity index 100% rename from buzz_scripts/include/graphs/Graph_droneY.graph rename to buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph diff --git a/buzz_scripts/include/graphs/frame_01186.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01186.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01186.png diff --git a/buzz_scripts/include/graphs/frame_01207.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01207.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01207.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01207.png diff --git a/buzz_scripts/include/graphs/frame_01394.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01394.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01394.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01394.png diff --git a/buzz_scripts/include/graphs/frame_01424.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png similarity index 100% rename from buzz_scripts/include/graphs/frame_01424.png rename to buzz_scripts/include/taskallocate/graphs/images/frame_01424.png diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_L.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_L.bzz diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_O.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_O.bzz diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_P.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_P.bzz diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_Y.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz diff --git a/buzz_scripts/include/graphs/shapes_square.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz similarity index 100% rename from buzz_scripts/include/graphs/shapes_square.bzz rename to buzz_scripts/include/taskallocate/graphs/shapes_square.bzz diff --git a/buzz_scripts/include/graphs/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv similarity index 100% rename from buzz_scripts/include/graphs/waypointlist.csv rename to buzz_scripts/include/taskallocate/waypointlist.csv diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz deleted file mode 100644 index 8a27c38..0000000 --- a/buzz_scripts/include/uavstates.bzz +++ /dev/null @@ -1,293 +0,0 @@ -######################################## -# -# FLIGHT-RELATED FUNCTIONS -# -######################################## -include "vec2.bzz" -include "rrtstar.bzz" - -TARGET_ALTITUDE = 15.0 # m. -BVMSTATE = "TURNEDOFF" -PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps -GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.5 # m. -GOTOANG_TOL = 0.1 # rad. -cur_goal_l = 0 -rc_State = 0 - -function uav_initswarm() { - s = swarm.create(1) - s.join() -} - -function turnedoff() { - statef=turnedoff - BVMSTATE = "TURNEDOFF" -} - -function idle() { - statef=idle - BVMSTATE = "IDLE" -} - -function takeoff() { - BVMSTATE = "TAKEOFF" - statef=takeoff - homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} - - if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, action, land, -1) - barrier_ready() - } else { - log("Altitude: ", pose.position.altitude) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } -} - -function land() { - BVMSTATE = "LAND" - statef=land - - neighbors.broadcast("cmd", 21) - uav_land() - - if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,turnedoff,land, 21) - barrier_ready() - } -} - -function set_goto(transf) { - BVMSTATE = "GOTOGPS" - statef=function() { - gotoWPRRT(transf) - } - - if(rc_goto.id==id){ - if(s!=nil){ - if(s.in()) - s.leave() - } - } else { - neighbors.broadcast("cmd", 16) - neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - } -} - -ptime=0 -function picture() { - statef=picture - BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) - if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() - } else if(ptime>=PICTURE_WAIT) { # wait for the picture - statef=action - ptime=0 - } - ptime=ptime+1 -} - -# -# still requires to be tuned, replaning takes too much time.. -# DS 23/11/2017 -function gotoWPRRT(transf) { - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) - print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) - - if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) { - log("Sorry this is too far.") - return - } - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = pose.position.latitude - homegps.long = pose.position.longitude - } - - if(Path==nil) { - # add proximity sensor and neighbor obstacles to the map - populateGrid(m_pos) - # start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - populateGrid(m_pos) - if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude - pose.position.altitude) - Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - # re-start the planner - pathPlanner(rc_goal, m_pos) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude - pose.position.altitude) - } - } else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } -} - -function gotoWP(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - - if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination - transf() - else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -} - -function follow() { - if(size(targets)>0) { - BVMSTATE = "FOLLOW" - statef=follow - attractor=math.vec2.newp(0,0) - foreach(targets, function(id, tab) { - force=(0.05)*(tab.range)^4 - attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) - }) - uav_moveto(attractor.x, attractor.y, 0.0) - } else { - log("No target in local table!") - #statef=idle - } -} - -function uav_rccmd() { - if(flight.rc_cmd==22) { - log("cmd 22") - flight.rc_cmd=0 - statef = takeoff - BVMSTATE = "TAKEOFF" - neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21) { - log("cmd 21") - log("To land") - flight.rc_cmd=0 - statef = land - BVMSTATE = "LAND" - neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16) { - flight.rc_cmd=0 - BVMSTATE = "GOTOGPS" - statef = goto - } else if(flight.rc_cmd==400) { - flight.rc_cmd=0 - uav_arm() - neighbors.broadcast("cmd", 400) - } else if (flight.rc_cmd==401){ - flight.rc_cmd=0 - uav_disarm() - neighbors.broadcast("cmd", 401) - } else if (flight.rc_cmd==666){ - flight.rc_cmd=0 - stattab_send() - } else if (flight.rc_cmd==900){ - flight.rc_cmd=0 - rc_State = 0 - neighbors.broadcast("cmd", 900) - } else if (flight.rc_cmd==901){ - flight.rc_cmd=0 - rc_State = 1 - neighbors.broadcast("cmd", 901) - } else if (flight.rc_cmd==902){ - flight.rc_cmd=0 - rc_State = 2 - neighbors.broadcast("cmd", 902) - } else if (flight.rc_cmd==903){ - flight.rc_cmd=0 - rc_State = 3 - neighbors.broadcast("cmd", 903) - } -} - -function uav_neicmd() { - neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { - statef=takeoff - BVMSTATE = "TAKEOFF" - } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { - statef=land - BVMSTATE = "LAND" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ - rc_State = 0 - } else if(value==901){ - rc_State = 1 - } else if(value==902){ - rc_State = 2 - } else if(value==903){ - rc_State = 3 - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) - } - }) -} - -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 -} - -function vec_from_gps(lat, lon, home_ref) { - d_lon = lon - pose.position.longitude - d_lat = lat - pose.position.latitude - if(home_ref == 1) { - d_lon = lon - homegps.long - d_lat = lat - homegps.lat - } - ned_x = d_lat/180*math.pi * 6371000.0; - ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) -} - -function gps_from_vec(vec) { - Lgoal = {.latitude=0, .longitude=0} - Vrange = math.vec2.length(vec) - Vbearing = LimitAngle(math.atan(vec.y, vec.x)) -# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) - latR = pose.position.latitude*math.pi/180.0; - lonR = pose.position.longitude*math.pi/180.0; - target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); - target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); - Lgoal.latitude = target_lat*180.0/math.pi; - Lgoal.longitude = target_lon*180.0/math.pi; - #d_lat = (vec.x / 6371000.0)*180.0/math.pi; - #goal.latitude = d_lat + pose.position.latitude; - #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; - #goal.longitude = d_lon + pose.position.longitude; - - return Lgoal -} diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz deleted file mode 100644 index 6c4dab6..0000000 --- a/buzz_scripts/include/utilities.bzz +++ /dev/null @@ -1,152 +0,0 @@ -# Utilities - -# Rads to degrees -function rtod(r) { - return (r*(180.0/math.pi)) -} - -# Copy a table -function table_deep_copy(new_t, old_t, depth) { - depth = depth - 1 - if (old_t != nil) { - foreach(old_t, function(key, value) { - new_t[key] = value - if(depth != 0) { - new_t[key] = {} - table_deep_copy(new_t[key], value, depth) - } - }) - } -} - -function table_copy(old_t, depth) { - var t = {}; - table_deep_copy(t, old_t, depth); - return t; -} - -# Print the contents of a table -function table_print(t, depth) { - depth = depth - 1 - if (t != nil) { - foreach(t, function(key, value) { - log(key, " -> ", value) - if(depth != 0) { - table_print(t[key], depth) - } - }) - } -} - -# Write a table as if it was a vector -function write_vector(k, index, val) { - var key = string.tostring(index) - k[key] = val -} - -# Read a table as if it was a vector -function read_vector(k, index) { - var key = string.tostring(index) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Write a table as if it was a matrix -function write_matrix(k, row, col, val) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - k[key] = val -} - -# Read a table as if it was a matrix -function read_matrix(k, row, col) { - var key = string.concat(string.tostring(row),"-",string.tostring(col)) - if (k[key] == nil) { - return -1 - } else { - return k[key] - } -} - -# Int to String -function itos(i) { - - log("Use 'string.tostring(OJB)' instead") - - if (i==0) { return "0" } - if (i==1) { return "1" } - if (i==2) { return "2" } - if (i==3) { return "3" } - if (i==4) { return "4" } - if (i==5) { return "5" } - if (i==6) { return "6" } - if (i==7) { return "7" } - if (i==8) { return "8" } - if (i==9) { return "9" } - - log("Function 'itos' out of bounds, returning the answer (42)") - return "42" -} - -# String to Int -function stoi(s) { - if (s=='0') { return 0 } - if (s=='1') { return 1 } - if (s=='2') { return 2 } - if (s=='3') { return 3 } - if (s=='4') { return 4 } - if (s=='5') { return 5 } - if (s=='6') { return 6 } - if (s=='7') { return 7 } - if (s=='8') { return 8 } - if (s=='9') { return 9 } - - log("Function 'stoi' out of bounds, returning the answer (42)") - return 42 - -} - -# Force angles in the (-pi,pi) interval -function radians_interval(a) { - var temp = a - while ((temp>2.0*math.pi) or (temp<0.0)) { - if (temp > 2.0*math.pi) { - temp = temp - 2.0*math.pi - } else if (temp < 0.0){ - temp = temp + 2.0*math.pi - } - } - if (temp > math.pi) { - temp = temp - 2.0*math.pi - } - return temp -} - -############################################ - -#base = {} - -#base.create = function() { -# return { -# .method = function(a,b) { -# return a + b -# } -# } -# } - -#x = base.create() -#x.method(3,4) # returns 7 - -#derived = {} - -#derived.create = function() { -# b = base.create() -# b.method = function(a,b) { -# return a * b -# } -#} - -#x = derived.create() -#x.method(3,4) # returns 12 diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz new file mode 100644 index 0000000..144cabf --- /dev/null +++ b/buzz_scripts/include/utils/conversions.bzz @@ -0,0 +1,63 @@ +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 +} + +# TODO: add other conversions.... +function convert_path(P) { + var pathR={} + if(V_TYPE == 0) { + var n = 1 + while(n <= size(P)){ + pathR[n] = {} + var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell)) + pathR[n][1]=tmpgoal.latitude + pathR[n][2]=tmpgoal.longitude + n = n + 1 + } + } + return pathR +} + +# TODO: add other conversions.... +function convert_pt(in) { + if(V_TYPE == 0) + return vec_from_gps(in.x, in.y, 0) +} + +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) +} + +function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + pose.position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + pose.position.longitude; + + return Lgoal +} \ No newline at end of file diff --git a/buzz_scripts/include/string.bzz b/buzz_scripts/include/utils/string.bzz similarity index 100% rename from buzz_scripts/include/string.bzz rename to buzz_scripts/include/utils/string.bzz diff --git a/buzz_scripts/include/vec2.bzz b/buzz_scripts/include/utils/vec2.bzz similarity index 100% rename from buzz_scripts/include/vec2.bzz rename to buzz_scripts/include/utils/vec2.bzz diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index 0a7b01f..4c0e06b 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -19,7 +19,13 @@ var v_ground = {} b_updating = 0 vstig_counter = WAIT4STEP -function uav_initstig() { + +function init_swarm() { + s = swarm.create(1) + s.join() +} + +function init_stig() { v_status = stigmergy.create(STATUS_VSTIG) v_ground = stigmergy.create(GROUND_VSTIG) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz new file mode 100644 index 0000000..6bf2856 --- /dev/null +++ b/buzz_scripts/main.bzz @@ -0,0 +1,81 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +function action() { + statef=action + set_goto(idle) +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on navigate + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz deleted file mode 100644 index 3ad4cc6..0000000 --- a/buzz_scripts/testRRT.bzz +++ /dev/null @@ -1,45 +0,0 @@ -include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" - -function action() { - statef=action - uav_storegoal(45.5088103899,-73.1540826153,2.5) - set_goto(idle) -} - -# Executed once at init time. -function init() { - uav_initstig() - uav_initswarm() - - TARGET_ALTITUDE = 2.5 # m - # statef=turnedoff - # BVMSTATE = "TURNEDOFF" - statef = takeoff - BVMSTATE = "TAKEOFF" -} - -# Executed at each time step. -function step() { - uav_rccmd() - - statef() - - log("Current state: ", BVMSTATE) - # log("Obstacles: ") - # reduce(proximity, - # function(key, value, acc) { - # log(key, " - ", value.angle, value.value) - # return acc - # }, math.vec2.new(0, 0)) -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index db43d6f..507e70c 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -201,7 +201,7 @@ static int buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + 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)); @@ -255,7 +255,7 @@ static int testing_buzz_register_hooks() buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + 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)); From da86b4be817452a324a2cc0484774ca0267ab4fa Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 26 Feb 2018 13:26:33 -0500 Subject: [PATCH 26/98] added minimal ROSBuzz example script --- buzz_scripts/empty.bzz | 17 ----- buzz_scripts/include/act/states.bzz | 89 +++++---------------------- buzz_scripts/include/plan/rrtstar.bzz | 58 +++++++++++++++++ buzz_scripts/main.bzz | 17 +++-- buzz_scripts/minimal.bzz | 64 +++++++++++++++++++ 5 files changed, 145 insertions(+), 100 deletions(-) delete mode 100644 buzz_scripts/empty.bzz create mode 100644 buzz_scripts/minimal.bzz diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz deleted file mode 100644 index 3bb5b12..0000000 --- a/buzz_scripts/empty.bzz +++ /dev/null @@ -1,17 +0,0 @@ -include "update.bzz" - -# Executed once at init time. -function init() { -} - -# Executed at each time step. -function step() { -} - -# Executed once when the robot (or the simulator) is reset. -function reset() { -} - -# Executed once at the end of experiment. -function destroy() { -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d8e3790..e987b60 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,7 +4,6 @@ # ######################################## include "utils/vec2.bzz" -include "plan/rrtstar.bzz" include "act/barrier.bzz" include "utils/conversions.bzz" @@ -33,7 +32,7 @@ function launch() { if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_ready() } else { log("Altitude: ", pose.position.altitude) @@ -41,7 +40,7 @@ function launch() { uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, "NAVIGATE", "LAUNCH") + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_ready() } } @@ -73,78 +72,20 @@ function take_picture() { pic_time=pic_time+1 } -# -# Work in progress.... -function navigate() { - BVMSTATE="NAVIGATE" - if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) - cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) - m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) - } - - print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) - if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { +function goto_gps(transf) { + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") - return - } - - # create the map - if(map==nil) { - dyn_init_map(cur_goal) - if(V_TYPE == 0) { - homegps.lat = pose.position.latitude - homegps.long = pose.position.longitude - } - } - - if(Path==nil) { - # add proximity sensor and neighbor obstacles to the map - populateGrid(m_pos) - # start the planner - path_it = 1 - pathPlanner(cur_goal, m_pos) - } else if(path_it <= size(Path)) { - var cur_path_pt = convert_pt(getvec(Path, path_it)) - print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) - if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { - populateGrid(m_pos) - if(check_path(Path, path_it, m_pos, 0)) { - # stop - goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) - Path = nil - if(V_TYPE == 0) - cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - # re-start the planner - path_it = 1 - pathPlanner(cur_goal, m_pos) - } else { - cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) - goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) - } - } else - path_it = path_it + 1 - } else { - Path = nil - BVMSTATE="IDLE" - } + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } -# function goto_direct(transf) { -# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) -# print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) - -# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) -# log("Sorry this is too far.") -# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity -# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -# } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination -# transf() -# else -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) -# } - function follow() { if(size(targets)>0) { BVMSTATE = "FOLLOW" @@ -208,9 +149,9 @@ function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") { + if(value==22 and BVMSTATE!="BARRIERWAIT") { BVMSTATE = "LAUNCH" - } else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") { + } else if(value==21 and BVMSTATE!="BARRIERWAIT") { BVMSTATE = "STOP" } else if(value==400 and BVMSTATE=="TURNEDOFF") { uav_arm() diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 6d40626..bfdd065 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -14,6 +14,64 @@ cur_cell = {} nei_cell = {} mapgoal = {} + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + # create a map with its length to fit the goal (rel. pos.) and the current position as the center # TODO: test with khepera/argos function dyn_init_map(m_navigation) { diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 6bf2856..2563c3e 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -2,8 +2,12 @@ include "update.bzz" # don't use a stigmergy id=11 with this header, for barrier # it requires an 'action' function to be defined here. include "act/states.bzz" +include "plan/rrtstar.bzz" include "vstigenv.bzz" +#State launched after takeoff +AUTO_LAUNCH_STATE = "IDLE" + ##### # Vehicule type: # 0 -> outdoor flying vehicle @@ -16,11 +20,6 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } -function action() { - statef=action - set_goto(idle) -} - # Executed once at init time. function init() { init_stig() @@ -31,7 +30,7 @@ function init() { # start the swarm command listener # nei_cmd_listen() - # Starting state + # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup. # BVMSTATE = "TURNEDOFF" BVMSTATE = "LAUNCHED" } @@ -51,15 +50,15 @@ function step() { statef=turnedoff else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop - else if(BVMSTATE=="LAUNCHED") # ends on navigate + else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE statef=launch else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure statef=makegraph # or bidding - else if(BVMSTATE=="PATHPLAN") # ends on navigate + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar statef=rrtstar - else if(BVMSTATE=="NAVIGATE") # ends on idle + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar statef=navigate else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure statef=follow diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz new file mode 100644 index 0000000..8f0494b --- /dev/null +++ b/buzz_scripts/minimal.bzz @@ -0,0 +1,64 @@ +include "update.bzz" +include "act/states.bzz" +include "vstigenv.bzz" + +# State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +# Executed once at init time. +function init() { + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +function action() { + BVMSTATE = "ACTION" + # do some actions.... +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action + + statef() + + log("Current state: ", BVMSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From ed59c6de7c0653e736be4f31bf67dedf3a652a1d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 2 Mar 2018 15:35:06 -0500 Subject: [PATCH 27/98] ported AGF to new rosbuzz file structure --- buzz_scripts/include/act/states.bzz | 8 +- .../include/taskallocate/graphformGPS.bzz | 188 +++++++----------- buzz_scripts/main.bzz | 33 ++- buzz_scripts/mainRRT.bzz | 80 ++++++++ 4 files changed, 182 insertions(+), 127 deletions(-) create mode 100644 buzz_scripts/mainRRT.bzz diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index e987b60..9dfc8fc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -73,17 +73,17 @@ function take_picture() { } function goto_gps(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } function follow() { @@ -94,7 +94,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index d4e5aae..e35fe37 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -1,18 +1,11 @@ # # Include files # -include "string.bzz" -include "vec2.bzz" -include "update.bzz" -#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. -include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. -include "uavstates.bzz" # require an 'action' function to be defined here. - -include "graphs/shapes_P.bzz" -include "graphs/shapes_O.bzz" -include "graphs/shapes_L.bzz" -include "graphs/shapes_Y.bzz" +include "taskallocate/graphs/shapes_P.bzz" +include "taskallocate/graphs/shapes_O.bzz" +include "taskallocate/graphs/shapes_L.bzz" +include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -20,9 +13,6 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 old_state = -1 -# max velocity in cm/step -ROBOT_MAXVEL = 150.0 - # # Global variables # @@ -40,12 +30,12 @@ m_MessageBearing={}#store received neighbour message m_neighbourCount=0#used to cunt neighbours #Save message from one neighbour #the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing -m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} +m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} # #Save the message to send #The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #navigation vector m_navigation={.x=0,.y=0} @@ -117,38 +107,38 @@ function count(table,value){ # function i2s(value){ if(value==1){ - return "STATE_FREE" + return "GRAPH_FREE" } else if(value==2){ - return "STATE_ASKING" + return "GRAPH_ASKING" } else if(value==3){ - return "STATE_JOINING" + return "GRAPH_JOINING" } else if(value==4){ - return "STATE_JOINED" + return "GRAPH_JOINED" } else if(value==5){ - return "STATE_LOCK" + return "GRAPH_LOCK" } } # #map from state to int # function s2i(value){ - if(value=="STATE_FREE"){ + if(value=="GRAPH_FREE"){ return 1 } - else if(value=="STATE_ASKING"){ + else if(value=="GRAPH_ASKING"){ return 2 } - else if(value=="STATE_JOINING"){ + else if(value=="GRAPH_JOINING"){ return 3 } - else if(value=="STATE_JOINED"){ + else if(value=="GRAPH_JOINED"){ return 4 } - else if(value=="STATE_LOCK"){ + else if(value=="GRAPH_LOCK"){ return 5 } } @@ -344,11 +334,11 @@ function UpdateNodeInfo(){ var i=0 while(i0) @@ -455,7 +446,7 @@ function DoFree() { var i=0 var j=0 while(i outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup. + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From b798abf2834b74c3eebb4164684aed3dd42c1002 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 3 Mar 2018 19:41:54 -0500 Subject: [PATCH 28/98] added yaw control and states pursuit, agreggation and formation (still to tweak) --- buzz_scripts/include/act/states.bzz | 75 ++++++++++++++++++- buzz_scripts/include/plan/rrtstar.bzz | 4 +- .../include/taskallocate/graphformGPS.bzz | 2 +- buzz_scripts/main.bzz | 10 ++- src/buzzuav_closures.cpp | 18 +++-- src/roscontroller.cpp | 14 ++-- 6 files changed, 101 insertions(+), 22 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9dfc8fc..02d1fdc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -66,7 +66,7 @@ function take_picture() { if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture - BVMSTATE="IDLE" + BVMSTATE="IDLE" pic_time=0 } pic_time=pic_time+1 @@ -79,11 +79,11 @@ function goto_gps(transf) { log("Sorry this is too far.") else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } function follow() { @@ -94,13 +94,80 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - goto_abs(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" } } +# converge to centroid +function aggregate() { + BVMSTATE="AGGREGATE" + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + if(math.vec2.length(centroid)>GOTO_MAXVEL) + centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) + goto_abs(centroid.x, centroid.y, 0.0, 0.0) +} + +# follow one another +rotang = 0.0 +function pursuit() { + BVMSTATE="PURSUIT" + insight = 0 + leader = math.vec2.newp(0.0, 0.0) + var cmdbin = math.vec2.newp(0.0, 0.0) + neighbors.foreach(function(rid, data) { + if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { + insight = 1 + leader = math.vec2.newp(data.distance, data.azimuth) + } + }) + if(insight == 1) { + log("Leader in sight !") + #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) + cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) + } else { + rotang = rotang + math.pi/60 + cmdbin = math.vec2.newp(2.0, rotang) + } + goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) +} + +# Lennard-Jones interaction magnitude +TARGET = 8.0 +EPSILON = 0.000001 +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# 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 +function formation() { + BVMSTATE="FORMATION" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + if(math.vec2.length(accum)>GOTO_MAXVEL*15) + accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) + goto_abs(accum.x, accum.y, 0.0, 0.0) +} + function rc_cmd_listen() { if(flight.rc_cmd==22) { log("cmd 22") diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index bfdd065..54c69ab 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -53,7 +53,7 @@ function navigate() { populateGrid(m_pos) if(check_path(Path, path_it, m_pos, 0)) { # stop - goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude) + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0) Path = nil if(V_TYPE == 0) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) @@ -62,7 +62,7 @@ function navigate() { pathPlanner(cur_goal, m_pos) } else { cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) - goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0) } } else path_it = path_it + 1 diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index e35fe37..0252971 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -694,7 +694,7 @@ function DoLock(){ m_navigation=motion_vector() } #move - goto_abs(m_navigation.x, m_navigation.y, 0.0) + goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() } # diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1cfd45c..d898bb3 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "PURSUIT" ##### # Vehicule type: @@ -26,7 +26,7 @@ function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 25.0 # m + TARGET_ALTITUDE = 10 + id*2.0 # m # start the swarm command listener nei_cmd_listen() @@ -54,6 +54,12 @@ function step() { statef=launch else if(BVMSTATE=="IDLE") statef=idle + else if(BVMSTATE=="AGGREGATE") + statef=aggregate + else if(BVMSTATE=="FORMATION") + statef=formation + else if(BVMSTATE=="PURSUIT") + statef=pursuit else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? startGraph() } else if(BVMSTATE=="GRAPH_FREE") { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 20e0456..b55f9eb 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -13,7 +13,7 @@ namespace buzzuav_closures { // TODO: Minimize the required global variables and put them in the header // static const rosbzz_node::roscontroller* roscontroller_ptr; -static double goto_pos[3]; +static double goto_pos[4]; static double rc_goto_pos[3]; static float rc_gimbal[4]; static float batt[3]; @@ -207,22 +207,26 @@ int buzz_exportmap(buzzvm_t vm) int buzzuav_moveto(buzzvm_t vm) /* -/ Buzz closure to move following a 2D vector +/ Buzz closure to move following a 3D vector + Yaw /----------------------------------------*/ { - buzzvm_lnum_assert(vm, 3); + buzzvm_lnum_assert(vm, 4); buzzvm_lload(vm, 1); // dx buzzvm_lload(vm, 2); // dy - buzzvm_lload(vm, 3); //* dheight + buzzvm_lload(vm, 3); // dheight + buzzvm_lload(vm, 4); // dyaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dh = buzzvm_stack_at(vm, 1)->f.value; - float dy = buzzvm_stack_at(vm, 2)->f.value; - float dx = buzzvm_stack_at(vm, 3)->f.value; + float dyaw = buzzvm_stack_at(vm, 1)->f.value; + float dh = buzzvm_stack_at(vm, 2)->f.value; + float dy = buzzvm_stack_at(vm, 3)->f.value; + float dx = buzzvm_stack_at(vm, 4)->f.value; goto_pos[0] = dx; goto_pos[1] = dy; 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]); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cfd47c0..cf54154 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -711,7 +711,7 @@ script case buzzuav_closures::COMMAND_MOVETO: goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; case buzzuav_closures::COMMAND_GIMBAL: @@ -935,15 +935,17 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.header.frame_id = 1; // DEBUG - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y); + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f, %.3f", cur_pos.x, cur_pos.y, x, y, yaw); moveMsg.pose.position.x = cur_pos.x + y; moveMsg.pose.position.y = cur_pos.y + x; moveMsg.pose.position.z = z; - moveMsg.pose.orientation.x = 0; - moveMsg.pose.orientation.y = 0; - moveMsg.pose.orientation.z = 0; - moveMsg.pose.orientation.w = 1; + tf::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + moveMsg.pose.orientation.x = q[0]; + moveMsg.pose.orientation.y = q[1]; + moveMsg.pose.orientation.z = q[2]; + moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment // if(fabs(x)>0.005 || fabs(y)>0.005) { From c9364666e651da88c56e30711aa7aaad8f29fcc2 Mon Sep 17 00:00:00 2001 From: dave Date: Sat, 3 Mar 2018 22:43:12 -0500 Subject: [PATCH 29/98] enhanced states switch and zooids rate --- buzz_scripts/include/act/states.bzz | 38 ++++++++------ .../include/taskallocate/graphformGPS.bzz | 52 +++++++++---------- buzz_scripts/main.bzz | 21 ++++---- 3 files changed, 56 insertions(+), 55 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 02d1fdc..fa6cffc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,12 +10,12 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 1.5 # m/steps +GOTO_MAXVEL = 2.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. path_it = 0 -rc_State = 0 +graphid = 0 pic_time = 0 g_it = 0 @@ -163,8 +163,8 @@ function formation() { var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) math.vec2.scale(accum, 1.0 / neighbors.count()) - if(math.vec2.length(accum)>GOTO_MAXVEL*15) - accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum)) + if(math.vec2.length(accum)>GOTO_MAXVEL*10) + accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum)) goto_abs(accum.x, accum.y, 0.0, 0.0) } @@ -195,19 +195,22 @@ function rc_cmd_listen() { stattab_send() } else if (flight.rc_cmd==900){ flight.rc_cmd=0 - rc_State = 0 + BVMSTATE = "TASK_ALLOCATE" neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 - rc_State = 1 + destroyGraph() + BVMSTATE = "PURSUIT" neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 - rc_State = 2 + destroyGraph() + BVMSTATE = "AGGREGATE" neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 - rc_State = 3 + destroyGraph() + BVMSTATE = "FORMATION" neighbors.broadcast("cmd", 903) } } @@ -224,14 +227,17 @@ function nei_cmd_listen() { uav_arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ uav_disarm() - } else if(value==900){ - rc_State = 0 - } else if(value==901){ - rc_State = 1 - } else if(value==902){ - rc_State = 2 - } else if(value==903){ - rc_State = 3 + } else if(value==900){ # Shapes + BVMSTATE = "TASK_ALLOCATE" + } else if(value==901){ # Pursuit + destroyGraph() + BVMSTATE = "PURSUIT" + } else if(value==902){ # Agreggate + destroyGraph() + BVMSTATE = "AGGREGATE" + } else if(value==903){ # Formation + destroyGraph() + BVMSTATE = "FORMATION" } else if(value==16 and BVMSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 0252971..826ee01 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -11,7 +11,6 @@ ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 -old_state = -1 # # Global variables @@ -401,7 +400,7 @@ function TransitionToJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) } # #Transistion to state Lock, lock the current formation @@ -422,10 +421,8 @@ function TransitionToLock(){ } m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x, m_navigation.y, 0.0) + goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) - # prepare to restart a new shape - old_state = rc_State #stop listening neighbors.ignore("m") } @@ -536,7 +533,7 @@ function DoAsking(){ m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) BroadcastGraph() } # @@ -666,20 +663,14 @@ function DoJoined(){ } } - #check if should to transists to lock - #write statues - #v_tag.get(m_nLabel) - #log(v_tag.size(), " of ", ROBOTS, " ready to lock") - #if(v_tag.size()==ROBOTS){ - # TransitionToLock() - #} barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED") BroadcastGraph() } # #Do Lock # -function DoLock(){ +timeout_graph = 40 +function DoLock() { UpdateGraph() m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.Label=m_nLabel @@ -693,19 +684,23 @@ function DoLock(){ if(m_nLabel!=0){ m_navigation=motion_vector() } - #move +# #move goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() -} -# -# Executed after takeoff -# -function startGraph(){ - BVMSTATE="GRAPH_FREE" - # reset the graph - statef=resetGraph + if(loop) { + if(timeout_graph==0) { + if(graphid < 3) + graphid = graphid + 1 + else + graphid = 0 + timeout_graph = 40 + BVMSTATE="TASK_ALLOCATE" + } + timeout_graph = timeout_graph - 1 + } } + # # Executed at init # @@ -750,6 +745,7 @@ function BroadcastGraph() { # Executed when reset # function resetGraph(){ + BVMSTATE="GRAPH_FREE" m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} m_navigation={.x=0,.y=0} @@ -766,16 +762,16 @@ function resetGraph(){ assign_id=-1 m_gotjoinedparent = 0 - if(rc_State==0){ + if(graphid==0){ log("Loading P graph") Read_GraphP() - } else if(rc_State==1) { + } else if(graphid==1) { log("Loading O graph") Read_GraphO() - } else if(rc_State==2) { + } else if(graphid==2) { log("Loading L graph") Read_GraphL() - } else if(rc_State==3) { + } else if(graphid==3) { log("Loading Y graph") Read_GraphY() } @@ -802,7 +798,7 @@ function destroyGraph() { #clear neighbour message m_navigation.x=0.0 m_navigation.y=0.0 - goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) m_vecNodes={} #stop listening neighbors.ignore("m") diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d898bb3..7191155 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" ##### # Vehicule type: @@ -27,6 +27,7 @@ function init() { init_swarm() TARGET_ALTITUDE = 10 + id*2.0 # m + loop = 1 # start the swarm command listener nei_cmd_listen() @@ -60,21 +61,19 @@ function step() { statef=formation else if(BVMSTATE=="PURSUIT") statef=pursuit - else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ? - startGraph() - } else if(BVMSTATE=="GRAPH_FREE") { + else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? + statef=resetGraph + else if(BVMSTATE=="GRAPH_FREE") statef=DoFree - } else if(BVMSTATE=="GRAPH_ASKING") { + else if(BVMSTATE=="GRAPH_ASKING") statef=DoAsking - } else if(BVMSTATE=="GRAPH_JOINING") { + else if(BVMSTATE=="GRAPH_JOINING") statef=DoJoining - } else if(BVMSTATE=="GRAPH_JOINED") { + else if(BVMSTATE=="GRAPH_JOINED") statef=DoJoined - } else if(BVMSTATE=="GRAPH_TRANSTOLOCK") + else if(BVMSTATE=="GRAPH_TRANSTOLOCK") statef=TransitionToLock - else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph - startGraph() - else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested) + else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list statef=DoLock else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar statef=rrtstar From a1d13d312a3d97f4ab5a5ac020ad280c8c28eb29 Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 4 Mar 2018 21:44:39 -0500 Subject: [PATCH 30/98] fixed formation and enhanced aggregation --- buzz_scripts/include/act/states.bzz | 59 ++++++++----------- .../include/taskallocate/graphformGPS.bzz | 11 +--- buzz_scripts/include/utils/conversions.bzz | 6 ++ buzz_scripts/main.bzz | 2 +- 4 files changed, 35 insertions(+), 43 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index fa6cffc..3e27726 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -77,13 +77,12 @@ function goto_gps(transf) { print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) - } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() - else + else { + LimitSpeed(m_navigation, 1.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } } function follow() { @@ -110,40 +109,34 @@ function aggregate() { }, {.x=0, .y=0}) if(neighbors.count() > 0) math.vec2.scale(centroid, 1.0 / neighbors.count()) - if(math.vec2.length(centroid)>GOTO_MAXVEL) - centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid)) - goto_abs(centroid.x, centroid.y, 0.0, 0.0) + cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS)) + cmdbin = LimitSpeed(cmdbin, 1.0/2.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # follow one another rotang = 0.0 function pursuit() { BVMSTATE="PURSUIT" - insight = 0 - leader = math.vec2.newp(0.0, 0.0) - var cmdbin = math.vec2.newp(0.0, 0.0) - neighbors.foreach(function(rid, data) { - if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) { - insight = 1 - leader = math.vec2.newp(data.distance, data.azimuth) - } - }) - if(insight == 1) { - log("Leader in sight !") - #cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader)) - cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader)) - } else { - rotang = rotang + math.pi/60 - cmdbin = math.vec2.newp(2.0, rotang) - } - goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang) + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + math.vec2.scale(centroid, 1.0 / neighbors.count()) + rotang = rotang + math.pi/50.0 + cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang)) + cmdbin = LimitSpeed(cmdbin, 1.0/5.0) + goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 0.000001 +EPSILON = 3.0 function lj_magnitude(dist, target, epsilon) { - return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + # log(dist, (target / dist), (epsilon / dist), lj) + return lj } # Neighbor data to LJ interaction vector @@ -157,15 +150,15 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction +old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator - var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) - math.vec2.scale(accum, 1.0 / neighbors.count()) - if(math.vec2.length(accum)>GOTO_MAXVEL*10) - accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum)) - goto_abs(accum.x, accum.y, 0.0, 0.0) + math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) + goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } function rc_cmd_listen() { diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 826ee01..61e77fd 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -78,14 +78,7 @@ m_fTargetDistanceTolerance=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 4000 #13.5 the LJ parameter for other robots - -# Lennard-Jones interaction magnitude - -function FlockInteraction(dist,target,epsilon){ - var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) - return mag -} +EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots # #return the number of value in table @@ -264,7 +257,7 @@ function LJ_vec(i){ var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),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) diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 144cabf..50a58bc 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -7,6 +7,12 @@ function LimitAngle(angle){ return angle } +function LimitSpeed(vel_vec, factor){ + if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) + vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) + return vel_vec +} + # TODO: add other conversions.... function convert_path(P) { var pathR={} diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 7191155..1927d66 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "TASK_ALLOCATE" +AUTO_LAUNCH_STATE = "AGGREGATE" ##### # Vehicule type: From 2f25df02f0ba38f3648c2e4d7ae38b30248b8018 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 5 Mar 2018 00:10:18 -0500 Subject: [PATCH 31/98] minor fixes and fixed pursuit (vector field guidance) --- buzz_scripts/include/act/states.bzz | 30 ++++++++++++++++------------- buzz_scripts/main.bzz | 2 +- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 3e27726..573a92f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -108,26 +108,31 @@ function aggregate() { return centroid }, {.x=0, .y=0}) if(neighbors.count() > 0) - math.vec2.scale(centroid, 1.0 / neighbors.count()) - cmdbin = math.vec2.sub(centroid, math.vec2.newp(15.0, id * 2 * math.pi / ROBOTS)) + 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) } # follow one another -rotang = 0.0 function pursuit() { BVMSTATE="PURSUIT" - centroid = neighbors.reduce(function(rid, data, centroid) { - centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) - return centroid + rd = 15.0 + pc = 3.2 + vd = 15.0 + r_vec = neighbors.reduce(function(rid, data, r_vec) { + r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth)) + return r_vec }, {.x=0, .y=0}) if(neighbors.count() > 0) - math.vec2.scale(centroid, 1.0 / neighbors.count()) - rotang = rotang + math.pi/50.0 - cmdbin = math.vec2.sub(centroid, math.vec2.newp(80.0, id * 2 * math.pi / ROBOTS + rotang)) - cmdbin = LimitSpeed(cmdbin, 1.0/5.0) - goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) + r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) + r = math.vec2.length(r_vec) + gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + dr = -gamma * (r-rd) + 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) } # Lennard-Jones interaction magnitude @@ -135,7 +140,6 @@ TARGET = 8.0 EPSILON = 3.0 function lj_magnitude(dist, target, epsilon) { lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) - # log(dist, (target / dist), (epsilon / dist), lj) return lj } @@ -156,7 +160,7 @@ function formation() { # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) - math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) } diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1927d66..e87f015 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "AGGREGATE" +AUTO_LAUNCH_STATE = "PURSUIT" ##### # Vehicule type: From e553d6a14642a518483ed93197240f6f948c9024 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 27 Apr 2018 10:31:24 -0400 Subject: [PATCH 32/98] fixes for Zooids demo view angle --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 573a92f..9668295 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -114,7 +114,7 @@ function aggregate() { goto_abs(cmdbin.x, cmdbin.y, 0.0, 0.0) } -# follow one another +# circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" rd = 15.0 From cd47fd3eb59fb8bea9d3a4857e5b23558912efc8 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 9 Mar 2018 09:37:56 -0600 Subject: [PATCH 33/98] Update readme.md --- readme.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/readme.md b/readme.md index 82d3941..d61cdb4 100644 --- a/readme.md +++ b/readme.md @@ -97,7 +97,7 @@ The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its st References ------ -* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 +* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 * Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. @@ -108,4 +108,4 @@ To activate highlights of the code in Visual Studio Code or Roboware add the fol "files.associations": { "*.bzz":"python" } -``` \ No newline at end of file +``` From 44212c8fcdfa6c9ceed760b48d436221535d8e52 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 27 Apr 2018 11:42:59 -0400 Subject: [PATCH 34/98] rosbuzz dev enabled in sim, fixed testaloneWP in dev --- buzz_scripts/testaloneWP.bzz | 42 ++++++++++++++++++++++++++++-------- src/buzzuav_closures.cpp | 2 +- 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index e438fb1..70531f4 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -1,27 +1,51 @@ -include "vec2.bzz" include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +#State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" function action() { + BVMSTATE = "ACTION" uav_storegoal(-1.0,-1.0,-1.0) - set_goto(picture) + goto_gps(picture) } # Executed once at init time. function init() { - statef=turnedoff - UAVSTATE = "TURNEDOFF" - TARGET_ALTITUDE = 15.0 + init_stig() + init_swarm() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" } # Executed at each time step. function step() { - uav_rccmd() + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action statef() - log("Current state: ", UAVSTATE) + log("Current state: ", BVMSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b55f9eb..364f2f8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -96,7 +96,7 @@ void setWPlist(string path) / set the absolute path for a csv list of waypoints ----------------------------------------------------------- */ { - WPlistname = path + "include/graphs/waypointlist.csv"; + WPlistname = path + "include/taskallocate/waypointlist.csv"; } float constrainAngle(float x) From 6d52a57dcd29876b67dbdf4b4e8db9549dab9cdd Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 14 Jun 2018 13:08:32 -0400 Subject: [PATCH 35/98] battery topic type changed to sensor_msgs --- include/roscontroller.h | 3 ++- src/roscontroller.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 7599c58..5259343 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -12,6 +12,7 @@ #include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" +#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" @@ -205,7 +206,7 @@ private: double gps_r_lat); /*battery status callback */ - void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); + void battery(const sensor_msgs::BatteryState::ConstPtr& msg); /*flight extended status callback*/ void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cf54154..9dc1374 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -836,12 +836,12 @@ void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); }; -void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) +void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg) /* / Update battery status into BVM from subscriber /------------------------------------------------------*/ { - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining); + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); // DEBUG // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // msg->current, msg ->remaining); From a9038601aef3393634d79149056d273bf4e56e4f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 14 Jun 2018 13:37:00 -0400 Subject: [PATCH 36/98] added update changes and neighbour pos clear on dev --- include/buzz_update.h | 248 +++---- include/buzzuav_closures.h | 4 + include/roscontroller.h | 1 + src/buzz_update.cpp | 1293 ++++++++++++++++++------------------ src/buzzuav_closures.cpp | 5 + src/roscontroller.cpp | 28 +- 6 files changed, 799 insertions(+), 780 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index 5680f85..4b43e86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,161 +1,171 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H -/*Simulation or robot check*/ -//#define SIMULATION 1 // set in CMAKELIST #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include + #define delete_p(p) \ do \ { \ free(p); \ p = NULL; \ } while (0) - -static const uint16_t CODE_REQUEST_PADDING = 250; -static const uint16_t MIN_UPDATE_PACKET = 251; -static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; -static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; -/*********************/ -/* Updater states */ -/********************/ - -typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update -} code_states_e; - -/*********************/ -/*Message types */ -/********************/ - -typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request -} code_message_e; - -/*************************/ -/*Updater message queue */ -/*************************/ - -struct updater_msgqueue_s +namespace buzz_update { - uint8_t* queue; - uint8_t* size; -}; -typedef struct updater_msgqueue_s* updater_msgqueue_t; + static const uint16_t CODE_REQUEST_PADDING = 250; + static const uint16_t MIN_UPDATE_PACKET = 251; + static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; + static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; + /*********************/ + /* Updater states */ + /********************/ -struct updater_code_s -{ - uint8_t* bcode; - uint8_t* bcode_size; -}; -typedef struct updater_code_s* updater_code_t; + typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update + } code_states_e; -/**************************/ -/*Updater data*/ -/**************************/ + /*********************/ + /*Message types */ + /********************/ -struct buzz_updater_elem_s -{ - /* robot id */ - // uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; -}; -typedef struct buzz_updater_elem_s* buzz_updater_elem_t; + typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request + } code_message_e; -/**************************************************************************/ -/*Updater routine from msg processing to file checks to be called from main*/ -/**************************************************************************/ -void update_routine(); + /*************************/ + /*Updater message queue */ + /*************************/ -/************************************************/ -/*Initalizes the updater */ -/************************************************/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + struct updater_msgqueue_s + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; -/*********************************************************/ -/*Appends buffer of given size to in msg queue of updater*/ -/*********************************************************/ + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; -void code_message_inqueue_append(uint8_t* msg, uint16_t size); + /**************************/ + /*Updater data*/ + /**************************/ -/*********************************************************/ -/*Processes messages inside the queue of the updater*/ -/*********************************************************/ + struct buzz_updater_elem_s + { + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; + }; + typedef struct buzz_updater_elem_s* buzz_updater_elem_t; -void code_message_inqueue_process(); + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); -/*****************************************************/ -/* obtains messages from out msgs queue of the updater*/ -/*******************************************************/ -uint8_t* getupdater_out_msg(); + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); -/******************************************************/ -/*obtains out msg queue size*/ -/*****************************************************/ -uint8_t* getupdate_out_msg_size(); + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ -/**************************************************/ -/*destroys the out msg queue*/ -/*************************************************/ -void destroy_out_msg_queue(); + void code_message_inqueue_append(uint8_t* msg, uint16_t size); -/***************************************************/ -/*obatins updater state*/ -/***************************************************/ -// int get_update_mode(); + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ -// buzz_updater_elem_t get_updater(); -/***************************************************/ -/*sets bzz file name*/ -/***************************************************/ -void set_bzz_file(const char* in_bzz_file); + void code_message_inqueue_process(); -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); -/****************************************************/ -/*Destroys the updater*/ -/***************************************************/ + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); -void destroy_updater(); + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); -int is_msg_present(); + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file, bool dbg); -int get_update_status(); + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ -void set_read_update_status(); + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); -int compile_bzz(std::string bzz_file); + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ -void updates_set_robots(int robots); + void destroy_updater(); -// void set_packet_id(int packet_id); + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ -// void collect_data(std::ofstream& logger); + int is_msg_present(); + + /****************************************************/ + /*Compiles a bzz script to bo and bdbg*/ + /***************************************************/ + + int compile_bzz(std::string bzz_file); + + /****************************************************/ + /*Set number of robots in the updater*/ + /***************************************************/ + + void updates_set_robots(int robots); +} #endif diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e6c3f48..7de79e1 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -118,6 +118,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) * update neighbors from in msgs */ void update_neighbors(buzzvm_t vm); +/* + *Clear neighbours struct + */ +void clear_neighbours_pos(); /* * closure to add a neighbor status */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 5259343..a366bf4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -97,6 +97,7 @@ private: float fcu_timeout; int armstate; int barrier; + int update; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 1d169f9..8624c3a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,599 +1,633 @@ -#include -#include -#include -#include -#include +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "buzz_update.h" -#include -#include -#include -#include -/*Temp for data collection*/ -// static int neigh=-1; -static struct timeval t1, t2; -static int timer_steps = 0; -// static clock_t end; +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; -/*Temp end */ -static int fd, wd = 0; -static int old_update = 0; -static buzz_updater_elem_t updater; -static int no_of_robot; -static const char* dbgf_name; -static const char* bcfname; -static const char* old_bcfname = "old_bcode.bo"; -static const char* bzz_file; -static int Robot_id = 0; -static int neigh = -1; -static int updater_msg_ready; -static uint16_t update_try_counter = 0; -static int updated = 0; -static const uint16_t MAX_UPDATE_TRY = 5; -static int packet_id_ = 0; -static size_t old_byte_code_size = 0; + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; -/*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) -{ - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 1*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; - - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); -} -/*Check for .bzz file chages*/ -int check_update() -{ - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - while (i < len) - { - struct inotify_event* event = (struct inotify_event*)&buf[i]; - /* file was modified this flag is true in nano and self delet in gedit and other editors */ - // fprintf(stdout,"inside file monitor.\n"); - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) - { - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + if(debug) ROS_INFO("Initializing file monitor..."); fd = inotify_init1(IN_NONBLOCK); - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ - if (!old_update) + if (fd < 0) { - check = 1; - old_update = 1; + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; + + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } + } + /*Check for .bzz file chages*/ + int check_update() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /*Update index to start of next event*/ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file and call bsdiff to patch*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + std::stringstream patch_exsisting; + patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 + << "tmp_patch.bo"; + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + if(debug){ + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + if(debug) { + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter); + } + + void code_message_inqueue_append(uint8_t* msg, uint16_t size) + { + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + if(debug) { + ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode)); + ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + if(debug) ROS_WARN("Received patch PASSED tests!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } } - /* update index to start of next event */ - i += sizeof(struct inotify_event) + event->len; - } - if (!check) - old_update = 0; - return check; -} - -int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) -{ - if (SIMULATION == 1) - { - return 1; - } - else - { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ - FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); - fwrite(patch, update_patch_size, 1, fp); - fclose(fp); - std::stringstream patch_exsisting; - patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 - << "tmp_patch.bo"; - fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); - if (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; - } -} - -updater_code_t obtain_patched_bo(std::string& path, std::string& name1) -{ - if (SIMULATION == 1) - { - /*Read the exsisting file to simulate the patching*/ - std::stringstream read_patched; - read_patched << path << name1 << ".bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched << path << name1 << "-patched.bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } -} - -void code_message_outqueue_append() -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; -} - -void outqueue_append_code_request(uint16_t update_no) -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; - updater_msg_ready = 1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); -} - -void code_message_inqueue_append(uint8_t* msg, uint16_t size) -{ - updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; -} -/*Used for data logging*/ -/*void set_packet_id(int packet_id) -{ - packet_id_ = packet_id; -}*/ -void code_message_inqueue_process() -{ - int size = 0; - updater_code_t out_code = NULL; - - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - - if (*(int*)(updater->mode) == CODE_RUNNING) - { - // fprintf(stdout,"[debug]Inside inmsg code running"); - if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) { + if(debug) ROS_WARN("Patch rebroadcast request received."); size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - out_code = obtain_patched_bo(path, name1); + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } - // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - // FILE *fp; - // fp=fopen("update.bo", "wb"); - // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - // fclose(fp); + void create_update_patch() + { + std::stringstream genpatch; - if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) - { - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + /*Launch bsdiff and create a patch*/ + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) + { + ROS_ERROR("Error in compiling script, resuming old script."); } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + if(debug) ROS_INFO("Broadcasting patch ..."); + code_message_outqueue_append(); + } + delete_p(BO_BUF); } } } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + + else { - size += sizeof(uint16_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) { - update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); - code_message_outqueue_append(); + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, rolling back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); } } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); -} -void create_update_patch() -{ - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-update"; - - // CALL BSDIFF CMD - genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); - - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); - if (!fp) + uint8_t* getupdater_out_msg() { - perror(patchfileName.str().c_str()); + return (uint8_t*)updater->outmsg_queue->queue; } - fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); - rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + + uint8_t* getupdate_out_msg_size() { - perror(patchfileName.str().c_str()); + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; } - fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); -} - -void update_routine() -{ - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if (*(int*)updater->mode == CODE_RUNNING) + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) { - buzzvm_function_call(VM, "updated_neigh", 0); - // Check for update - if (check_update()) + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_INFO("Update found \nUpdating script ...\n"); - - if (compile_bzz(bzz_file)) + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + if(debug) ROS_WARN("Step test passed"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; } + /*Unable to step something wrong*/ else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit - if (!fp) + if (*(int*)(updater->mode) == CODE_RUNNING) { - perror(bzzfile_in_compile.str().c_str()); + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + else { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + /*You will never reach here*/ + ROS_ERROR("Step test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - neigh = -1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); } - delete_p(BO_BUF); + return 0; } } - } - - else - { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); - if (tObj->i.value == no_of_robot) - { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, decided to roll back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - } -} - -uint8_t* getupdater_out_msg() -{ - return (uint8_t*)updater->outmsg_queue->queue; -} - -uint8_t* getupdate_out_msg_size() -{ - // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; -} - -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) -{ - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) - { - ROS_WARN("Initializtion of script test passed\n"); - if (buzz_utility::update_step_test()) - { - /*data logging*/ - old_byte_code_size = *(size_t*)updater->bcode_size; - /*data logging*/ - ROS_WARN("Step test passed\n"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + ROS_ERROR("Initialization test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -604,118 +638,87 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) return 0; } } - else - { - if (*(int*)(updater->mode) == CODE_RUNNING) - { - ROS_ERROR("Initialization test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); - } - else - { - /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } -} -void destroy_out_msg_queue() -{ - // fprintf(stdout,"out queue freed\n"); - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - updater_msg_ready = 0; -} -int get_update_status() -{ - return updated; -} -void set_read_update_status() -{ - updated = 0; -} -/*int get_update_mode() -{ - return (int)*(int*)(updater->mode); -}*/ - -int is_msg_present() -{ - return updater_msg_ready; -} -/*buzz_updater_elem_t get_updater() -{ - return updater; -}*/ -void destroy_updater() -{ - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if (updater->outmsg_queue) + void destroy_out_msg_queue() { + // fprintf(stdout,"out queue freed\n"); delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue); + updater_msg_ready = 0; } - if (updater->inmsg_queue) + + int is_msg_present() { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + return updater_msg_ready; + } + /*buzz_updater_elem_t get_updater() + { + return updater; + }*/ + void destroy_updater() + { + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); } - inotify_rm_watch(fd, wd); - close(fd); -} -void set_bzz_file(const char* in_bzz_file) -{ - bzz_file = in_bzz_file; -} + void set_bzz_file(const char* in_bzz_file, bool dbg) + { + debug=dbg; + bzz_file = in_bzz_file; + } -void updates_set_robots(int robots) -{ - no_of_robot = robots; -} + void updates_set_robots(int robots) + { + no_of_robot = robots; + } -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file) -{ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - 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/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; -}*/ + /*void collect_data(std::ofstream& logger) + { + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // + Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 364f2f8..6631ba1 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -678,6 +678,11 @@ void update_neighbors(buzzvm_t vm) } } +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} + int buzzuav_update_currentpos(buzzvm_t vm) /* / Update the BVM position table diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 9dc1374..e84025a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -27,7 +27,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) std::string fname = Compile_bzz(bzzfile_name); bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; - set_bzz_file(bzzfile_name.c_str()); + buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); // Initialize variables SetMode("LOITER", 0); @@ -107,7 +107,7 @@ void roscontroller::RosControllerRun() ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; // Intialize the update monitor - init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // set ROS loop rate ros::Rate loop_rate(BUZZRATE); // DEBUG @@ -120,7 +120,7 @@ void roscontroller::RosControllerRun() grid_publisher(); send_MPpayload(); // Check updater state and step code - update_routine(); + if(update) buzz_update::update_routine(); if (time_step == BUZZRATE) { time_step = 0; @@ -144,15 +144,10 @@ void roscontroller::RosControllerRun() prepare_msg_and_publish(); // Call the flight controler service flight_controller_service_call(); - // Set multi message available after update - if (get_update_status()) - { - set_read_update_status(); - } // Set ROBOTS variable (swarm size) get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); - updates_set_robots(no_of_robots); + if(update) buzz_update::updates_set_robots(no_of_robots); // get_xbee_status(); // commented out because it may slow down the node too much, to be tested ros::spinOnce(); @@ -561,11 +556,11 @@ with size......... | / payload_pub.publish(payload_out); delete[] out; delete[] payload_out_ptr; - // Check for updater message if present send - if (is_msg_present()) + /// Check for updater message if present send + if (update && buzz_update::is_msg_present()) { uint8_t* buff_send = 0; - uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size()); + uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; @@ -578,10 +573,10 @@ with size......... | / *(uint16_t*)(buff_send + tot) = updater_msgSize; tot += sizeof(uint16_t); // Append updater msgs - memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); tot += updater_msgSize; // Destroy the updater out msg queue - destroy_out_msg_queue(); + buzz_update::destroy_out_msg_queue(); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint64_t* payload_64 = new uint64_t[total_size]; memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); @@ -750,6 +745,7 @@ void roscontroller::maintain_pos(int tim_step) if (timer_step >= BUZZRATE) { neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! timer_step = 0; @@ -1031,8 +1027,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); - code_message_inqueue_process(); + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); } free(pl); } From ece31509cb21c1085f450be7127be44a6962922f Mon Sep 17 00:00:00 2001 From: vivek Date: Thu, 14 Jun 2018 13:56:23 -0400 Subject: [PATCH 37/98] added solo config --- launch/launch_config/solo.yaml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 launch/launch_config/solo.yaml diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml new file mode 100644 index 0000000..b8a465d --- /dev/null +++ b/launch/launch_config/solo.yaml @@ -0,0 +1,22 @@ +topics: + gps : mavros/global_position/global + battery : mavros/battery + status : mavros/state + fcclient: mavros/cmd/command + setpoint: mavros/setpoint_position/local + armclient: mavros/cmd/arming + modeclient: mavros/set_mode + localpos: /mavros/local_position/pose + stream: mavros/set_stream_rate + altitude: mavros/global_position/rel_alt +type: + gps : sensor_msgs/NavSatFix + # for SITL Solo + # battery : mavros_msgs/BatteryState + # for solo + battery : mavros_msgs/BatteryStatus + status : mavros_msgs/State + altitude: std_msgs/Float64 +environment : + environment : solo-simulator + From 4845ff262d5a16f869ae7e23c4ec59fc6810ed49 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 15 Jun 2018 15:15:25 -0400 Subject: [PATCH 38/98] added launch_config arg and made BVM state optional on dev --- include/roscontroller.h | 1 + launch/rosbuzz.launch | 3 +- launch/rosbuzzd.launch | 3 +- src/roscontroller.cpp | 165 +++++++++++++++++++++------------------- 4 files changed, 93 insertions(+), 79 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index a366bf4..029eca5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -98,6 +98,7 @@ private: int armstate; int barrier; int update; + int statepub_active; int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index c2531f4..f632f68 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -6,9 +6,10 @@ + - + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index de7857d..a89b1af 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -6,9 +6,10 @@ + - + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e84025a..413ccf9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -110,13 +110,24 @@ void roscontroller::RosControllerRun() update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); // 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."); + } // DEBUG // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); while (ros::ok() && !buzz_utility::buzz_script_done()) { // Publish topics neighbours_pos_publisher(); - uavstate_publisher(); + if(statepub_active) uavstate_publisher(); grid_publisher(); send_MPpayload(); // Check updater state and step code @@ -652,88 +663,88 @@ script armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - cmd_srv.request.param5 = home.latitude; - cmd_srv.request.param6 = home.longitude; - cmd_srv.request.param7 = home.altitude; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + // cmd_srv.request.param5 = home.latitude; + // cmd_srv.request.param6 = home.longitude; + // cmd_srv.request.param7 = home.altitude; + // cmd_srv.request.command = buzzuav_closures::getcmd(); + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! + // goto_pos = buzzuav_closures::getgoto(); + // cmd_srv.request.param5 = goto_pos[0]; + // cmd_srv.request.param6 = goto_pos[1]; + // cmd_srv.request.param7 = goto_pos[2]; + // cmd_srv.request.command = buzzuav_closures::getcmd(); + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_ARM: - if (!armstate) - { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - } - break; + // case buzzuav_closures::COMMAND_ARM: + // if (!armstate) + // { + // SetMode("LOITER", 0); + // armstate = 1; + // Arm(); + // } + // break; - case buzzuav_closures::COMMAND_DISARM: - if (armstate) - { - armstate = 0; - SetMode("LOITER", 0); - Arm(); - } - break; + // case buzzuav_closures::COMMAND_DISARM: + // if (armstate) + // { + // armstate = 0; + // SetMode("LOITER", 0); + // Arm(); + // } + // break; - case buzzuav_closures::COMMAND_MOVETO: - goto_pos = buzzuav_closures::getgoto(); - roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); - break; + // case buzzuav_closures::COMMAND_MOVETO: + // goto_pos = buzzuav_closures::getgoto(); + // roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + // break; - case buzzuav_closures::COMMAND_GIMBAL: - gimbal = buzzuav_closures::getgimbal(); - cmd_srv.request.param1 = gimbal[0]; - cmd_srv.request.param2 = gimbal[1]; - cmd_srv.request.param3 = gimbal[2]; - cmd_srv.request.param4 = gimbal[3]; - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; + // case buzzuav_closures::COMMAND_GIMBAL: + // gimbal = buzzuav_closures::getgimbal(); + // cmd_srv.request.param1 = gimbal[0]; + // cmd_srv.request.param2 = gimbal[1]; + // cmd_srv.request.param3 = gimbal[2]; + // cmd_srv.request.param4 = gimbal[3]; + // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + // if (mav_client.call(cmd_srv)) + // { + // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + // } + // else + // ROS_ERROR("Failed to call service from flight controller"); + // break; - case buzzuav_closures::COMMAND_PICTURE: - ROS_INFO("TAKING A PICTURE HERE!! --------------"); - mavros_msgs::CommandBool capture_command; - if (capture_srv.call(capture_command)) - { - ROS_INFO("Reply: %ld", (long int)capture_command.response.success); - } - else - ROS_ERROR("Failed to call service from camera streamer"); - break; + // case buzzuav_closures::COMMAND_PICTURE: + // ROS_INFO("TAKING A PICTURE HERE!! --------------"); + // mavros_msgs::CommandBool capture_command; + // if (capture_srv.call(capture_command)) + // { + // ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + // } + // else + // ROS_ERROR("Failed to call service from camera streamer"); + // break; } } From 392531e1315f2351ebfcdc421687b09895ea7f81 Mon Sep 17 00:00:00 2001 From: Marius Klimavicius Date: Wed, 27 Jun 2018 16:52:58 -0400 Subject: [PATCH 39/98] solo sim parameters updated --- buzz_scripts/include/act/states.bzz | 3 ++- launch/launch_config/solo.yaml | 14 ++------------ launch/rosbuzz.launch | 2 +- src/roscontroller.cpp | 3 +-- 4 files changed, 6 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9668295..0b80cea 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -127,7 +127,8 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index b8a465d..c18075b 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -6,17 +6,7 @@ topics: setpoint: mavros/setpoint_position/local armclient: mavros/cmd/arming modeclient: mavros/set_mode - localpos: /mavros/local_position/pose - stream: mavros/set_stream_rate + localpos: mavros/local_position/pose + stream: mavros/set_stream_rate altitude: mavros/global_position/rel_alt -type: - gps : sensor_msgs/NavSatFix - # for SITL Solo - # battery : mavros_msgs/BatteryState - # for solo - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/State - altitude: std_msgs/Float64 -environment : - environment : solo-simulator diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index f632f68..7f3c63b 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -3,7 +3,7 @@ - + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 413ccf9..1f25370 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1001,8 +1001,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(); } - // DEBUG - // ROS_INFO("Set stream rate call successful"); + ROS_WARN("Set stream rate call successful"); } void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) From 1cb390f5954a489eee22dc44f04b8c5de1550ce2 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:10:57 -0400 Subject: [PATCH 40/98] Hub: made changes to allow arbitrary robot id's and Rosbuzz: changed bzzfiles barrier, states and main to fit arbitrary id's. --- buzz_scripts/include/act/barrier.bzz | 5 ++--- buzz_scripts/include/act/states.bzz | 20 +++++++++++++------- buzz_scripts/main.bzz | 9 +++++++-- 3 files changed, 22 insertions(+), 12 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 23e87d7..a87bed7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -53,12 +53,11 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier.put(id, bc) barrier.get(id) var allgood = 0 - log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { - var bi = 0 + var bi = LOWEST_ROBOT_ID allgood = 1 - while (bi= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) barrier_ready(22) } else { log("Altitude: ", pose.position.altitude) @@ -40,7 +46,7 @@ function launch() { uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH", 22) + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) barrier_ready(22) } } @@ -183,10 +189,10 @@ function rc_cmd_listen() { neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { flight.rc_cmd=0 - AUTO_LAUNCH_STATE = "STOP" + AUTO_LAUNCH_STATE = "TURNEDOFF" barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) barrier_ready(21) - BVMSTATE = "GOHOME" + BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==20) { flight.rc_cmd=0 @@ -246,8 +252,8 @@ function nei_cmd_listen() { AUTO_LAUNCH_STATE = "IDLE" BVMSTATE = "GOHOME" } else if(value==21) { - AUTO_LAUNCH_STATE = "STOP" - BVMSTATE = "GOHOME" + AUTO_LAUNCH_STATE = "TURNEDOFF" + BVMSTATE = "STOP" } else if(value==400 and BVMSTATE=="TURNEDOFF") { arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e4a33b3..d52fd95 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,9 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "FORMATION" +AUTO_LAUNCH_STATE = "CUSFUN" +#Lowest robot id in the network +LOWEST_ROBOT_ID = 97 TARGET = 9.0 EPSILON = 30.0 @@ -23,12 +25,13 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } + # Executed once at init time. function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 10 + id*2.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 # start the swarm command listener @@ -51,6 +54,8 @@ function step() { # if(BVMSTATE=="TURNEDOFF") statef=turnedoff + else if(BVMSTATE=="CUSFUN") + statef=cusfun else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE From 1bfeada346c627833264314a884ef51feb5a59d8 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:17:42 -0400 Subject: [PATCH 41/98] adapted bzz files from sim. --- buzz_scripts/include/act/barrier.bzz | 36 ++++--- buzz_scripts/include/act/states.bzz | 148 +++++++++++++++++--------- buzz_scripts/include/plan/rrtstar.bzz | 2 +- buzz_scripts/include/update.bzz | 3 +- buzz_scripts/main.bzz | 13 ++- buzz_scripts/minimal.bzz | 1 - buzz_scripts/stand_by.bzz | 45 ++++---- buzz_scripts/testLJ.bzz | 72 +++++++++++++ 8 files changed, 229 insertions(+), 91 deletions(-) create mode 100755 buzz_scripts/testLJ.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..a87bed7 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -7,10 +7,11 @@ # # Constants # -BARRIER_TIMEOUT = 1200 # in steps +BARRIER_TIMEOUT = 200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil +bc = 0; # # Sets a barrier @@ -22,15 +23,15 @@ function barrier_create() { #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) if(barrier!=nil) { barrier=nil - BARRIER_VSTIG = BARRIER_VSTIG +1 + # BARRIER_VSTIG = BARRIER_VSTIG +1 } #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef) { +function barrier_set(threshold, transf, resumef, bc) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bc); } BVMSTATE = "BARRIERWAIT" barrier_create() @@ -39,30 +40,41 @@ function barrier_set(threshold, transf, resumef) { # # Make yourself ready # -function barrier_ready() { +function barrier_ready(bc) { #log("BARRIER READY -------") - barrier.put(id, 1) + barrier.put(id, bc) barrier.put("d", 0) } # # Executes the barrier # -function barrier_wait(threshold, transf, resumef) { - barrier.put(id, 1) - +function barrier_wait(threshold, transf, resumef, bc) { + barrier.put(id, bc) barrier.get(id) - #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + var allgood = 0 + log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + var bi = LOWEST_ROBOT_ID + allgood = 1 + while (bi= BARRIER_TIMEOUT) { log("------> Barrier Timeout !!!!") - barrier=nil + barrier = nil timeW = 0 BVMSTATE = resumef - } + } else if(timeW % 10 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) timeW = timeW+1 } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 0b80cea..cda775b 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -27,44 +27,60 @@ function idle() { BVMSTATE = "IDLE" } +# Custom function for the user. +function cusfun(){ + BVMSTATE="CUSFUN" + log("cusfun: yay!!!") +} + function launch() { BVMSTATE = "LAUNCH" - if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) } else { log("Altitude: ", pose.position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } } else { - barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") - barrier_ready() + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) } } +gohomeT=100 +function goinghome() { + BVMSTATE = "GOHOME" + if(gohomeT > 0) { # TODO: Make a real check if home is reached + gohome() + gohomeT = gohomeT - 1 + } else + BVMSTATE = AUTO_LAUNCH_STATE +} + function stop() { - BVMSTATE = "STOP" + BVMSTATE = "STOP" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) } } else { - barrier_set(ROBOTS,"TURNEDOFF","STOP") - barrier_ready() - } + barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + barrier_ready(21) + } } function take_picture() { BVMSTATE="PICTURE" - uav_setgimbal(0.0, 0.0, -90.0, 20.0) + setgimbal(0.0, 0.0, -90.0, 20.0) if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize - uav_takepicture() + takepicture() } else if(pic_time>=PICTURE_WAIT) { # wait for the picture BVMSTATE="IDLE" pic_time=0 @@ -117,7 +133,7 @@ function aggregate() { # circle all together around centroid function pursuit() { BVMSTATE="PURSUIT" - rd = 15.0 + rd = 20.0 pc = 3.2 vd = 15.0 r_vec = neighbors.reduce(function(rid, data, r_vec) { @@ -127,8 +143,7 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - sqr = (r-rd)*(r-rd)+pc*pc*r*r - gamma = vd / math.sqrt(sqr) + gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) @@ -138,9 +153,9 @@ function pursuit() { # Lennard-Jones interaction magnitude TARGET = 8.0 -EPSILON = 3.0 +EPSILON = 30.0 function lj_magnitude(dist, target, epsilon) { - lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2) + lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) return lj } @@ -155,92 +170,119 @@ function lj_sum(rid, data, accum) { } # Calculates and actuates the flocking interaction -old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes function formation() { BVMSTATE="FORMATION" # Calculate accumulator accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) if(neighbors.count() > 0) accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) - old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0) - goto_abs(old_lj.x, old_lj.y, 0.0, 0.0) + accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# listens to commands from the remote control (web, commandline, etc) function rc_cmd_listen() { - if(flight.rc_cmd==22) { + if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 BVMSTATE = "LAUNCH" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { - log("cmd 21") flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "TURNEDOFF" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + barrier_ready(21) BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 BVMSTATE = "PATHPLAN" } else if(flight.rc_cmd==400) { flight.rc_cmd=0 - uav_arm() + arm() neighbors.broadcast("cmd", 400) } else if (flight.rc_cmd==401){ flight.rc_cmd=0 - uav_disarm() + disarm() neighbors.broadcast("cmd", 401) } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() } else if (flight.rc_cmd==900){ flight.rc_cmd=0 - BVMSTATE = "TASK_ALLOCATE" + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "PURSUIT" + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "AGGREGATE" + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 destroyGraph() - BVMSTATE = "FORMATION" + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + barrier_ready(903) neighbors.broadcast("cmd", 903) } } +# listens to other fleet members broadcasting commands function nei_cmd_listen() { neighbors.listen("cmd", function(vid, value, rid) { print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") - if(value==22 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "LAUNCH" - } else if(value==21 and BVMSTATE!="BARRIERWAIT") { - BVMSTATE = "STOP" - } else if(value==400 and BVMSTATE=="TURNEDOFF") { - uav_arm() - } else if(value==401 and BVMSTATE=="TURNEDOFF"){ - uav_disarm() - } else if(value==900){ # Shapes - BVMSTATE = "TASK_ALLOCATE" - } else if(value==901){ # Pursuit - destroyGraph() - BVMSTATE = "PURSUIT" - } else if(value==902){ # Agreggate - destroyGraph() - BVMSTATE = "AGGREGATE" - } else if(value==903){ # Formation - destroyGraph() - BVMSTATE = "FORMATION" - } else if(value==16 and BVMSTATE=="IDLE"){ - # neighbors.listen("gt",function(vid, value, rid) { - # print("Got (", vid, ",", value, ") from robot #", rid) - # # if(gt.id == id) statef=goto - # }) + if(BVMSTATE!="BARRIERWAIT") { + if(value==22) { + BVMSTATE = "LAUNCH" + }else if(value==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + } else if(value==21) { + AUTO_LAUNCH_STATE = "TURNEDOFF" + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + disarm() + } else if(value==900){ # Shapes + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + #barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if(value==901){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + #barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if(value==902){ # Agreggate + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + #barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if(value==903){ # Formation + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + #barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if(value==16 and BVMSTATE=="IDLE"){ + # 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/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 54c69ab..a55aff1 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -20,7 +20,7 @@ mapgoal = {} function navigate() { BVMSTATE="NAVIGATE" if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz - uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) } diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index f49fd7c..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -4,6 +4,7 @@ #################################################################################################### updated="update_ack" update_no=0 -function updated_neigh(){ +updates_active = 1 +function updated_no_bct(){ neighbors.broadcast(updated, update_no) } \ No newline at end of file diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e87f015..d52fd95 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -7,7 +7,11 @@ include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "PURSUIT" +AUTO_LAUNCH_STATE = "CUSFUN" +#Lowest robot id in the network +LOWEST_ROBOT_ID = 97 +TARGET = 9.0 +EPSILON = 30.0 ##### # Vehicule type: @@ -21,12 +25,13 @@ goal_list = { .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} } + # Executed once at init time. function init() { init_stig() init_swarm() - TARGET_ALTITUDE = 10 + id*2.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 # start the swarm command listener @@ -49,10 +54,14 @@ function step() { # if(BVMSTATE=="TURNEDOFF") statef=turnedoff + else if(BVMSTATE=="CUSFUN") + statef=cusfun else if(BVMSTATE=="STOP") # ends on turnedoff statef=stop else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE statef=launch + else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE + statef=goinghome else if(BVMSTATE=="IDLE") statef=idle else if(BVMSTATE=="AGGREGATE") diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz index 8f0494b..a49329b 100644 --- a/buzz_scripts/minimal.bzz +++ b/buzz_scripts/minimal.bzz @@ -51,7 +51,6 @@ function step() { statef=action statef() - log("Current state: ", BVMSTATE) } diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1349089..c605089 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -1,35 +1,38 @@ +include "act/states.bzz" +include "vstigenv.bzz" + updated="update_ack" update_no=0 -function init(){ -barrier = stigmergy.create(101) -barrier.put(id,1) -barrier_val=0 -barrier.onconflict(function (k,l,r) { -if(r. data < l. data or (r. data == l. data )) return l -else return r -}) +BVMSTATE = "UPDATESTANDBY" -s = swarm.create(1) -s.join() +# Executed once at init time. +function init(){ + barrier = stigmergy.create(101) + barrier.put(id,1) + barrier_val=0 + barrier.onconflict(function (k,l,r) { + if(r. data < l. data or (r. data == l. data )) return l + else return r + }) + init_swarm() + # start the swarm command listener + nei_cmd_listen() } function stand_by(){ + barrier.get(id) + barrier_val = barrier.size() -barrier.get(id) -barrier_val = barrier.size() - -neighbors.listen(updated, - function(vid, value, rid) { - print(" got from",vid," ", " value = ",value," ","rid"," " ) - if(value==update_no) barrier.put(rid,1) - } + neighbors.listen(updated, + function(vid, value, rid) { + if(value==update_no) barrier.put(rid,1) + } ) } - +# Executed at each time step. function step() { - -stand_by() + stand_by() } diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100755 index 0000000..5240618 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -0,0 +1,72 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +V_TYPE = 0 + +#State launched after takeoff +AUTO_LAUNCH_STATE = "FORMATION" + + +TARGET = 8.0 +EPSILON = 3.0 +GOTO_MAXVEL = 2.5 # m/steps + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" + TAKEOFF_COUNTER = 20 +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="FORMATION") + statef=formation + + statef() + + log("Current state: ", BVMSTATE) + + # Auto-takeoff (delayed for simulator boot) + if(id == 0) { + if(TAKEOFF_COUNTER>0) + TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1 + else if(TAKEOFF_COUNTER == 0) { + BVMSTATE="LAUNCH" + TAKEOFF_COUNTER = -1 + } + } +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} From de648255619c703e02aa92e891cdcba994608db6 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 4 Jul 2018 21:45:35 -0400 Subject: [PATCH 42/98] added data logging --- src/roscontroller.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1f25370..5e97929 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -60,6 +60,18 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } + // Create log dir and open log file + std::string path = + bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; + std::string folder_check="mkdir -p "+path; + system(folder_check.c_str()); + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; + log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } roscontroller::~roscontroller() @@ -69,6 +81,8 @@ roscontroller::~roscontroller() { // Destroy the BVM buzz_utility::buzz_script_destroy(); + // Close the data logging file + log.close(); } void roscontroller::GetRobotId() @@ -149,6 +163,13 @@ void roscontroller::RosControllerRun() } if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // log data + log< Date: Wed, 4 Jul 2018 22:22:42 -0400 Subject: [PATCH 43/98] moved BVM state publisher to buzz_utility --- include/buzz_utility.h | 2 ++ include/buzzuav_closures.h | 5 +---- src/buzz_utility.cpp | 15 +++++++++++++++ src/buzzuav_closures.cpp | 13 ------------- src/roscontroller.cpp | 2 +- 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 12cf011..600b9e2 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -77,4 +77,6 @@ buzzvm_t get_vm(); void set_robot_var(int ROBOTS); int get_inmsg_size(); + +std::string getuavstate(); } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 7de79e1..dbd3110 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -98,10 +98,7 @@ double* getgoto(); * returns the current grid */ std::map> getgrid(); -/* - * returns the current Buzz state - */ -std::string getuavstate(); + /* * returns the gimbal commands */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 507e70c..0862533 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,4 +564,19 @@ int get_inmsg_size() { return IN_MSG.size(); } +string getuavstate() +/* +/ return current BVM state +---------------------------------------*/ +{ + std::string uav_state = "Unknown"; + if(VM && VM->strings){ + 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); + buzzvm_pop(VM); + } + return uav_state; +} } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 6631ba1..d0a9ea6 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -479,19 +479,6 @@ float* getgimbal() return rc_gimbal; } -string getuavstate() -/* -/ return current BVM state ----------------------------------------*/ -{ - static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); - buzzvm_gload(VM); - buzzobj_t uav_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - return uav_state->s.value.str; -} - int getcmd() /* / return current mavros command to the FC diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5e97929..5f71a82 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -475,7 +475,7 @@ void roscontroller::uavstate_publisher() /----------------------------------------------------*/ { std_msgs::String uavstate_msg; - uavstate_msg.data = buzzuav_closures::getuavstate(); + uavstate_msg.data = buzz_utility::getuavstate(); uavstate_pub.publish(uavstate_msg); } From ecb2aa67fe7b359bea0c561f056947ce7c8bdaa4 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 5 Jul 2018 02:46:51 -0400 Subject: [PATCH 44/98] states.bzz syntax fix --- buzz_scripts/include/act/states.bzz | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index cda775b..f08b0e8 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -143,7 +143,8 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + var sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) From 87083c5658e062a32ef590637dd09921ea5ab5e1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 10 Jul 2018 14:00:01 -0400 Subject: [PATCH 45/98] enhanced barrier with foreach --- buzz_scripts/include/act/barrier.bzz | 24 +++++++++++++++++------- src/roscontroller.cpp | 22 +++++++++++++++++++++- 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index a87bed7..d7e141c 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -55,13 +55,7 @@ function barrier_wait(threshold, transf, resumef, bc) { var allgood = 0 log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { - var bi = LOWEST_ROBOT_ID - allgood = 1 - while (bi0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; + log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } roscontroller::~roscontroller() @@ -68,6 +80,8 @@ roscontroller::~roscontroller() { // Destroy the BVM buzz_utility::buzz_script_destroy(); + // Close the data logging file + log.close(); } void roscontroller::GetRobotId() @@ -137,6 +151,13 @@ void roscontroller::RosControllerRun() } if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // log data + log< Date: Tue, 10 Jul 2018 14:03:26 -0400 Subject: [PATCH 46/98] barrier enhance fix on branch dev --- buzz_scripts/include/act/barrier.bzz | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index a87bed7..f7d5910 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -55,13 +55,7 @@ function barrier_wait(threshold, transf, resumef, bc) { var allgood = 0 log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { - var bi = LOWEST_ROBOT_ID - allgood = 1 - while (bi Date: Wed, 11 Jul 2018 18:41:50 -0400 Subject: [PATCH 47/98] Time Sync algorithm and rc cmd implementation to sync time on request --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 15 ++++- buzz_scripts/include/timesync.bzz | 86 +++++++++++++++++++++++++++ buzz_scripts/include/utils/string.bzz | 12 ++-- buzz_scripts/main.bzz | 7 +++ include/buzz_utility.h | 2 + include/roscontroller.h | 1 + misc/cmdlinectr.sh | 3 + src/buzz_utility.cpp | 17 ++++++ src/roscontroller.cpp | 10 +++- 10 files changed, 145 insertions(+), 10 deletions(-) create mode 100644 buzz_scripts/include/timesync.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index f7d5910..d7e141c 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -87,4 +87,4 @@ function barrier_allgood(barrier, bc) { } ) return barriergood -} +} \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f08b0e8..6f94586 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -215,7 +215,13 @@ function rc_cmd_listen() { } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() - } else if (flight.rc_cmd==900){ + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ flight.rc_cmd=0 barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) barrier_ready(900) @@ -259,7 +265,12 @@ function nei_cmd_listen() { arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() - } else if(value==900){ # Shapes + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) neighbors.broadcast("cmd", 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..c441bf5 --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,86 @@ +include "utils/string.bzz" + +# Time to be sync +logical_time = 0 +# sync algo. constants +TIME_JUMP_THR = 5 +TIME_TO_FORGET = 20 +TIME_TO_SYNC = 100 +# table to store neighbor time data +time_nei_table = {} +# Algo. global parameters +diffMaxLogical = 0 +jumped = 0 +syncError = 99999 +sync_timer = 0 + +# Function to intialize sync algorithm +function init_time_sync(){ + neighbors.listen("time_sync", + function(vid, value, rid) { + log(" TIME SYNC Got (", vid, ",", value, ") #", rid) + var msg = string.split(value,",") + var msg_time = string.toint(msg[0]) + var msg_max = string.toint(msg[1]) + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } + ) +} + +# Function to sync. algo +function step_time_sync(){ + logical_time = logical_time + 1 + sync_timer = sync_timer + 1 + if(sync_timer < TIME_TO_SYNC){ + log(" SYNC ALGO ACTIVE time:", sync_timer) + cnt = 0 + avg_offset = 0 + foreach(time_nei_table, function(key, value) { + if(value != nil){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) + time_nei_table[key] = nil + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } + } + + jumped = 0 + syncError=0 + var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + neighbors.broadcast("time_sync",mstr) + } + log("Logical time now ", logical_time) +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz index 4140a1b..91ceb63 100644 --- a/buzz_scripts/include/utils/string.bzz +++ b/buzz_scripts/include/utils/string.bzz @@ -17,10 +17,10 @@ string.charat = function(s, n) { # RETURN: The position of the first match, or nil # string.indexoffirst = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) var i = 0 - while(i < ls-lm+1) { + while(i < las-lm+1) { if(string.sub(s, i, i+lm) == m) return i i = i + 1 } @@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) { # RETURN: The position of the last match, or nil # string.indexoflast = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) - var i = ls - lm + 1 + var i = las - lm + 1 while(i >= 0) { if(string.sub(s, i, i+lm) == m) return i i = i - 1 @@ -56,10 +56,10 @@ string.split = function(s, d) { var i2 = 0 # index to move along s (token end) var c = 0 # token count var t = {} # token list - var ls = string.length(s) + var las = string.length(s) var ld = string.length(d) # Go through string s - while(i2 < ls) { + while(i2 < las) { # Try every delimiter var j = 0 # index to move along d var f = nil # whether the delimiter was found or not diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d52fd95..0a3bc32 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -5,6 +5,7 @@ include "act/states.bzz" include "plan/rrtstar.bzz" include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" +include "timesync.bzz" #State launched after takeoff AUTO_LAUNCH_STATE = "CUSFUN" @@ -31,6 +32,8 @@ function init() { init_stig() init_swarm() + init_time_sync() + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -43,6 +46,10 @@ function init() { # Executed at each time step. function step() { + + # step time sync algorithm + step_time_sync() + # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 600b9e2..1c3c506 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); std::string getuavstate(); + +int getlogicaltime(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 029eca5..566b1a0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -43,6 +43,7 @@ #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 +#define CMD_SYNC_CLOCK 777 using namespace std; diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 00df45f..68b4296 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -11,6 +11,9 @@ function arm { function disarm { rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 } +function timesync { + rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0 +} function testWP { rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 0862533..c3986d0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,6 +564,7 @@ int get_inmsg_size() { return IN_MSG.size(); } + string getuavstate() /* / return current BVM state @@ -579,4 +580,20 @@ string getuavstate() } return uav_state; } + +int getlogicaltime() +/* +/ return current logical time +--------------------------------------*/ +{ + int logical_time = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + buzzvm_pop(VM); + } + return logical_time; +} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5f71a82..cb00b3e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -165,6 +165,8 @@ void roscontroller::RosControllerRun() ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data log<"); + res.success = true; + break; default: buzzuav_closures::rc_call(req.command); - ROS_INFO("----> Received unregistered command: ", req.command); + ROS_ERROR("----> Received unregistered command: ", req.command); res.success = true; break; } From 9f75bd989e7b01cb77d2efd698f62e4c62df1cb3 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 18:43:20 -0400 Subject: [PATCH 48/98] Time sync algo and rc cmd implemeantation on sim --- buzz_scripts/include/act/states.bzz | 18 +++++- buzz_scripts/include/timesync.bzz | 86 +++++++++++++++++++++++++++ buzz_scripts/include/utils/string.bzz | 12 ++-- buzz_scripts/main.bzz | 7 +++ include/buzz_utility.h | 2 + include/roscontroller.h | 1 + misc/cmdlinectr.sh | 3 + src/buzz_utility.cpp | 15 +++++ src/roscontroller.cpp | 10 +++- 9 files changed, 144 insertions(+), 10 deletions(-) create mode 100644 buzz_scripts/include/timesync.bzz diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index cda775b..6f94586 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -143,7 +143,8 @@ function pursuit() { if(neighbors.count() > 0) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r = math.vec2.length(r_vec) - gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r) + var sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) dr = -gamma * (r-rd) dT = gamma * pc vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) @@ -214,7 +215,13 @@ function rc_cmd_listen() { } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() - } else if (flight.rc_cmd==900){ + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ flight.rc_cmd=0 barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) barrier_ready(900) @@ -258,7 +265,12 @@ function nei_cmd_listen() { arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() - } else if(value==900){ # Shapes + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) neighbors.broadcast("cmd", 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..c441bf5 --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,86 @@ +include "utils/string.bzz" + +# Time to be sync +logical_time = 0 +# sync algo. constants +TIME_JUMP_THR = 5 +TIME_TO_FORGET = 20 +TIME_TO_SYNC = 100 +# table to store neighbor time data +time_nei_table = {} +# Algo. global parameters +diffMaxLogical = 0 +jumped = 0 +syncError = 99999 +sync_timer = 0 + +# Function to intialize sync algorithm +function init_time_sync(){ + neighbors.listen("time_sync", + function(vid, value, rid) { + log(" TIME SYNC Got (", vid, ",", value, ") #", rid) + var msg = string.split(value,",") + var msg_time = string.toint(msg[0]) + var msg_max = string.toint(msg[1]) + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } + ) +} + +# Function to sync. algo +function step_time_sync(){ + logical_time = logical_time + 1 + sync_timer = sync_timer + 1 + if(sync_timer < TIME_TO_SYNC){ + log(" SYNC ALGO ACTIVE time:", sync_timer) + cnt = 0 + avg_offset = 0 + foreach(time_nei_table, function(key, value) { + if(value != nil){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) + time_nei_table[key] = nil + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } + } + + jumped = 0 + syncError=0 + var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + neighbors.broadcast("time_sync",mstr) + } + log("Logical time now ", logical_time) +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz index 4140a1b..91ceb63 100644 --- a/buzz_scripts/include/utils/string.bzz +++ b/buzz_scripts/include/utils/string.bzz @@ -17,10 +17,10 @@ string.charat = function(s, n) { # RETURN: The position of the first match, or nil # string.indexoffirst = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) var i = 0 - while(i < ls-lm+1) { + while(i < las-lm+1) { if(string.sub(s, i, i+lm) == m) return i i = i + 1 } @@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) { # RETURN: The position of the last match, or nil # string.indexoflast = function(s, m) { - var ls = string.length(s) + var las = string.length(s) var lm = string.length(m) - var i = ls - lm + 1 + var i = las - lm + 1 while(i >= 0) { if(string.sub(s, i, i+lm) == m) return i i = i - 1 @@ -56,10 +56,10 @@ string.split = function(s, d) { var i2 = 0 # index to move along s (token end) var c = 0 # token count var t = {} # token list - var ls = string.length(s) + var las = string.length(s) var ld = string.length(d) # Go through string s - while(i2 < ls) { + while(i2 < las) { # Try every delimiter var j = 0 # index to move along d var f = nil # whether the delimiter was found or not diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index d52fd95..0a3bc32 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -5,6 +5,7 @@ include "act/states.bzz" include "plan/rrtstar.bzz" include "taskallocate/graphformGPS.bzz" include "vstigenv.bzz" +include "timesync.bzz" #State launched after takeoff AUTO_LAUNCH_STATE = "CUSFUN" @@ -31,6 +32,8 @@ function init() { init_stig() init_swarm() + init_time_sync() + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -43,6 +46,10 @@ function init() { # Executed at each time step. function step() { + + # step time sync algorithm + step_time_sync() + # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 600b9e2..1c3c506 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); std::string getuavstate(); + +int getlogicaltime(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 29c97e6..72bd44b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -42,6 +42,7 @@ #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 +#define CMD_SYNC_CLOCK 777 using namespace std; diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 00df45f..68b4296 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -11,6 +11,9 @@ function arm { function disarm { rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 } +function timesync { + rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0 +} function testWP { rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 8a03316..d247f4a 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -581,4 +581,19 @@ string getuavstate() return uav_state; } +int getlogicaltime() +/* +/ return current logical time +--------------------------------------*/ +{ + int logical_time = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + buzzvm_pop(VM); + } + return logical_time; +} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3591daa..5663871 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -153,6 +153,8 @@ void roscontroller::RosControllerRun() ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data log<"); + res.success = true; + break; default: buzzuav_closures::rc_call(req.command); - ROS_INFO("----> Received unregistered command: ", req.command); + ROS_ERROR("----> Received unregistered command: ", req.command); res.success = true; break; } From d3f0299b7e86e27442adf42dcf6a760250b6dd71 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 23:18:35 -0400 Subject: [PATCH 49/98] adapted syc algo to robots --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/timesync.bzz | 85 +++++++++++++++------------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index d7e141c..1a08ac4 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 10 == 0 and bc > 0) + } else if(timeW % 50 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index c441bf5..f190457 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -5,7 +5,8 @@ logical_time = 0 # sync algo. constants TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 -TIME_TO_SYNC = 100 +TIME_TO_SYNC = 200 +COM_DELAY = 3 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -18,20 +19,23 @@ sync_timer = 0 function init_time_sync(){ neighbors.listen("time_sync", function(vid, value, rid) { - log(" TIME SYNC Got (", vid, ",", value, ") #", rid) - var msg = string.split(value,",") - var msg_time = string.toint(msg[0]) - var msg_max = string.toint(msg[1]) - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) - var time_offset = msg_time - logical_time - if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset - if(time_offset > TIME_JUMP_THR){ - logical_time = logical_time + time_offset - diffMaxLogical = math.max(diffMaxLogical-time_offset,0) - jumped = 1 + if(value != nil){ + log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) + var msg_time = value.time + var msg_max = value.max + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + if(msg_time != nil and msg_max != nil){ + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } } - time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} } ) } @@ -44,36 +48,39 @@ function step_time_sync(){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 avg_offset = 0 - foreach(time_nei_table, function(key, value) { - if(value != nil){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age - if(local_offset > 0){ - avg_offset = avg_offset + 1 * local_offset - cnt = cnt + 1 - } - else{ - if(math.abs(local_offset) 0){ + foreach(time_nei_table, function(key, value) { + if(value.time != 0){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) - time_nei_table[key] = nil - value.age = value.age + 1 + if(value.age > TIME_TO_FORGET) + value.time = 0 + + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } } - }) - if(cnt > 0 and jumped != 1){ - var correction = math.ceil(avg_offset / (cnt + 1) ) - if(math.abs(correction) < TIME_JUMP_THR){ - logical_time = logical_time + correction - } } - jumped = 0 syncError=0 - var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } + #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } log("Logical time now ", logical_time) From 7ab97b4fcea0529d556bd990d02655d2076773f2 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 23:19:51 -0400 Subject: [PATCH 50/98] adapted sync algo to robots --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/timesync.bzz | 85 +++++++++++++++------------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index d7e141c..1a08ac4 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 10 == 0 and bc > 0) + } else if(timeW % 50 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index c441bf5..f190457 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -5,7 +5,8 @@ logical_time = 0 # sync algo. constants TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 -TIME_TO_SYNC = 100 +TIME_TO_SYNC = 200 +COM_DELAY = 3 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -18,20 +19,23 @@ sync_timer = 0 function init_time_sync(){ neighbors.listen("time_sync", function(vid, value, rid) { - log(" TIME SYNC Got (", vid, ",", value, ") #", rid) - var msg = string.split(value,",") - var msg_time = string.toint(msg[0]) - var msg_max = string.toint(msg[1]) - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) - var time_offset = msg_time - logical_time - if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset - if(time_offset > TIME_JUMP_THR){ - logical_time = logical_time + time_offset - diffMaxLogical = math.max(diffMaxLogical-time_offset,0) - jumped = 1 + if(value != nil){ + log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) + var msg_time = value.time + var msg_max = value.max + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + if(msg_time != nil and msg_max != nil){ + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } } - time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} } ) } @@ -44,36 +48,39 @@ function step_time_sync(){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 avg_offset = 0 - foreach(time_nei_table, function(key, value) { - if(value != nil){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age - if(local_offset > 0){ - avg_offset = avg_offset + 1 * local_offset - cnt = cnt + 1 - } - else{ - if(math.abs(local_offset) 0){ + foreach(time_nei_table, function(key, value) { + if(value.time != 0){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) - time_nei_table[key] = nil - value.age = value.age + 1 + if(value.age > TIME_TO_FORGET) + value.time = 0 + + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } } - }) - if(cnt > 0 and jumped != 1){ - var correction = math.ceil(avg_offset / (cnt + 1) ) - if(math.abs(correction) < TIME_JUMP_THR){ - logical_time = logical_time + correction - } } - jumped = 0 syncError=0 - var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } + #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } log("Logical time now ", logical_time) From d0fa6d647f79bc2cac3d42becf70d1c57bfbe724 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 23:51:38 -0400 Subject: [PATCH 51/98] swithced debug on --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cb00b3e..e4f7f12 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -11,7 +11,7 @@ namespace rosbzz_node { const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; -const bool debug = false; +const bool debug = true; roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) /* From 81b263fa0ae95c842b57953e9547b4b9c591647f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 12 Jul 2018 01:01:34 -0400 Subject: [PATCH 52/98] adapted barrier rebroadcast time on robots --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 1a08ac4..27628b0 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 50 == 0 and bc > 0) + } else if(timeW % 100 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 6f94586..51624a2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -269,7 +269,7 @@ function nei_cmd_listen() { if(logical_time != nil) reinit_time_sync() barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) barrier_ready(777) - neighbors.broadcast("cmd", 777) + #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) From 0587a2ab6bac35081743ea0d1abc007635030423 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 14 Jul 2018 20:30:24 -0400 Subject: [PATCH 53/98] fixed a bug within timesync algo. --- buzz_scripts/include/timesync.bzz | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index f190457..fecf9d9 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -6,7 +6,7 @@ logical_time = 0 TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 TIME_TO_SYNC = 200 -COM_DELAY = 3 +COM_DELAY = 2 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -44,6 +44,7 @@ function init_time_sync(){ function step_time_sync(){ logical_time = logical_time + 1 sync_timer = sync_timer + 1 + log("Logical time now ", logical_time) if(sync_timer < TIME_TO_SYNC){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 @@ -51,8 +52,7 @@ function step_time_sync(){ if(size(time_nei_table) > 0){ foreach(time_nei_table, function(key, value) { if(value.time != 0){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age + var local_offset = value.time - logical_time + value.age if(local_offset > 0){ avg_offset = avg_offset + 1 * local_offset cnt = cnt + 1 @@ -80,10 +80,8 @@ function step_time_sync(){ jumped = 0 syncError=0 var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } - log("Logical time now ", logical_time) } # Function to set sync timer to zero and reinitiate sync. algo From 14ceab1047e29cf9de277a6eec04f2d3f45e1180 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 14:50:30 -0400 Subject: [PATCH 54/98] added msg size + nei id + nei pos to log --- include/buzz_utility.h | 2 ++ src/buzz_utility.cpp | 4 ++++ src/roscontroller.cpp | 22 +++++++++++++++++++++- 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 1c3c506..4eda0fb 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); +std::vector get_inmsg_vector(); + std::string getuavstate(); int getlogicaltime(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c3986d0..2cfcfd6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -565,6 +565,10 @@ int get_inmsg_size() return IN_MSG.size(); } +std::vector get_inmsg_vector(){ + return IN_MSG; +} + string getuavstate() /* / return current BVM state diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e4f7f12..6b48835 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -170,7 +170,27 @@ void roscontroller::RosControllerRun() 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 << ","; + } + std::vector in_msg = buzz_utility::get_inmsg_vector(); + log <<(int)in_msg.size()<<","; + for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ + uint8_t* first_INmsg = (uint8_t*)(*it); + size_t tot = 0; + // Size is at first 2 bytes + uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); + tot += sizeof(uint16_t); + // Decode neighbor Id + uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); + log<<(int)neigh_id<<","<<(int)msg_size<<","; + } + log < Date: Sun, 22 Jul 2018 14:51:33 -0400 Subject: [PATCH 55/98] added msg size + nei id + nei pos to log in sim --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 2 +- buzz_scripts/include/timesync.bzz | 8 +++----- include/buzz_utility.h | 2 ++ src/buzz_utility.cpp | 4 ++++ src/roscontroller.cpp | 21 ++++++++++++++++++++- 6 files changed, 31 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 1a08ac4..27628b0 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) { barrier = nil timeW = 0 BVMSTATE = resumef - } else if(timeW % 50 == 0 and bc > 0) + } else if(timeW % 100 == 0 and bc > 0) neighbors.broadcast("cmd", bc) timeW = timeW+1 diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 6f94586..51624a2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -269,7 +269,7 @@ function nei_cmd_listen() { if(logical_time != nil) reinit_time_sync() barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) barrier_ready(777) - neighbors.broadcast("cmd", 777) + #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index f190457..e8d5266 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -6,7 +6,7 @@ logical_time = 0 TIME_JUMP_THR = 5 TIME_TO_FORGET = 20 TIME_TO_SYNC = 200 -COM_DELAY = 3 +COM_DELAY = 2 # table to store neighbor time data time_nei_table = {} # Algo. global parameters @@ -44,6 +44,7 @@ function init_time_sync(){ function step_time_sync(){ logical_time = logical_time + 1 sync_timer = sync_timer + 1 + log("Logical time now ", logical_time) if(sync_timer < TIME_TO_SYNC){ log(" SYNC ALGO ACTIVE time:", sync_timer) cnt = 0 @@ -51,8 +52,7 @@ function step_time_sync(){ if(size(time_nei_table) > 0){ foreach(time_nei_table, function(key, value) { if(value.time != 0){ - #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) - var local_offset = value.time - logical_time - value.age + var local_offset = value.time - logical_time + value.age if(local_offset > 0){ avg_offset = avg_offset + 1 * local_offset cnt = cnt + 1 @@ -80,10 +80,8 @@ function step_time_sync(){ jumped = 0 syncError=0 var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) neighbors.broadcast("time_sync",mstr) } - log("Logical time now ", logical_time) } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 1c3c506..4eda0fb 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); +std::vector get_inmsg_vector(); + std::string getuavstate(); int getlogicaltime(); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index d247f4a..1b508d6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -565,6 +565,10 @@ int get_inmsg_size() return IN_MSG.size(); } +std::vector get_inmsg_vector(){ + return IN_MSG; +} + string getuavstate() /* / return current BVM state diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5663871..bb9334d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -158,7 +158,26 @@ void roscontroller::RosControllerRun() 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 << ","; + } + std::vector in_msg = buzz_utility::get_inmsg_vector(); + log <<(int)in_msg.size()<<","; + for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ + uint8_t* first_INmsg = (uint8_t*)(*it); + size_t tot = 0; + // Size is at first 2 bytes + uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); + tot += sizeof(uint16_t); + // Decode neighbor Id + uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); + log<<(int)neigh_id<<","<<(int)msg_size<<","; + } log < Date: Sun, 22 Jul 2018 15:10:13 -0400 Subject: [PATCH 56/98] fixed moveto command --- src/buzzuav_closures.cpp | 4 +- src/roscontroller.cpp | 158 +++++++++++++++++++-------------------- 2 files changed, 81 insertions(+), 81 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d0a9ea6..1d72cbf 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -228,8 +228,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 = COMMAND_MOVETO; // TODO: standard mavros? return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6b48835..c56ac72 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -706,88 +706,88 @@ script armstate = 0; break; - // case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! - // cmd_srv.request.param5 = home.latitude; - // cmd_srv.request.param6 = home.longitude; - // cmd_srv.request.param7 = home.altitude; - // cmd_srv.request.command = buzzuav_closures::getcmd(); - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + cmd_srv.request.param5 = home.latitude; + cmd_srv.request.param6 = home.longitude; + cmd_srv.request.param7 = home.altitude; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - // goto_pos = buzzuav_closures::getgoto(); - // cmd_srv.request.param5 = goto_pos[0]; - // cmd_srv.request.param6 = goto_pos[1]; - // cmd_srv.request.param7 = goto_pos[2]; - // cmd_srv.request.command = buzzuav_closures::getcmd(); - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param5 = goto_pos[0]; + cmd_srv.request.param6 = goto_pos[1]; + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_ARM: - // if (!armstate) - // { - // SetMode("LOITER", 0); - // armstate = 1; - // Arm(); - // } - // break; + case buzzuav_closures::COMMAND_ARM: + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + } + break; - // case buzzuav_closures::COMMAND_DISARM: - // if (armstate) - // { - // armstate = 0; - // SetMode("LOITER", 0); - // Arm(); - // } - // break; + case buzzuav_closures::COMMAND_DISARM: + if (armstate) + { + armstate = 0; + SetMode("LOITER", 0); + Arm(); + } + break; - // case buzzuav_closures::COMMAND_MOVETO: - // goto_pos = buzzuav_closures::getgoto(); - // roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); - // break; + case buzzuav_closures::COMMAND_MOVETO: + goto_pos = buzzuav_closures::getgoto(); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + break; - // case buzzuav_closures::COMMAND_GIMBAL: - // gimbal = buzzuav_closures::getgimbal(); - // cmd_srv.request.param1 = gimbal[0]; - // cmd_srv.request.param2 = gimbal[1]; - // cmd_srv.request.param3 = gimbal[2]; - // cmd_srv.request.param4 = gimbal[3]; - // cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - // if (mav_client.call(cmd_srv)) - // { - // ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - // } - // else - // ROS_ERROR("Failed to call service from flight controller"); - // break; + case buzzuav_closures::COMMAND_GIMBAL: + gimbal = buzzuav_closures::getgimbal(); + cmd_srv.request.param1 = gimbal[0]; + cmd_srv.request.param2 = gimbal[1]; + cmd_srv.request.param3 = gimbal[2]; + cmd_srv.request.param4 = gimbal[3]; + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - // case buzzuav_closures::COMMAND_PICTURE: - // ROS_INFO("TAKING A PICTURE HERE!! --------------"); - // mavros_msgs::CommandBool capture_command; - // if (capture_srv.call(capture_command)) - // { - // ROS_INFO("Reply: %ld", (long int)capture_command.response.success); - // } - // else - // ROS_ERROR("Failed to call service from camera streamer"); - // break; + case buzzuav_closures::COMMAND_PICTURE: + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + if (capture_srv.call(capture_command)) + { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } + else + ROS_ERROR("Failed to call service from camera streamer"); + break; } } @@ -998,9 +998,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) moveMsg.pose.orientation.w = q[3]; // To prevent drifting from stable position, uncomment - // if(fabs(x)>0.005 || fabs(y)>0.005) { - localsetpoint_nonraw_pub.publish(moveMsg); - // } + if(fabs(x)>0.005 || fabs(y)>0.005) { + localsetpoint_nonraw_pub.publish(moveMsg); + } } void roscontroller::SetMode(std::string mode, int delay_miliseconds) From 81f4aba155e05b56ef07c2dbd8260197e87d7b07 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 15:42:10 -0400 Subject: [PATCH 57/98] changed autolaunch function to formation --- buzz_scripts/main.bzz | 3 ++- misc/cmdlinectr.sh | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 0a3bc32..e187ce4 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -8,7 +8,8 @@ include "vstigenv.bzz" include "timesync.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "CUSFUN" +AUTO_LAUNCH_STATE = "FORMATION" +#AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network LOWEST_ROBOT_ID = 97 TARGET = 9.0 diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 68b4296..c2794a5 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -34,7 +34,7 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } function uavstate { From 24db1dc4fc71ad8599a08db90a1dd5121e1ce783 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 22 Jul 2018 16:01:20 -0400 Subject: [PATCH 58/98] changed updaterobot function to hold heaven launch file --- misc/cmdlinectr.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index c2794a5..bea3e11 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -34,7 +34,8 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXHeavenbuzzy.launch +# rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXxbeebuzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } function uavstate { From 2378a3881a98162044df994f0b8ffcf40be61045 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 24 Jul 2018 15:55:13 -0400 Subject: [PATCH 59/98] changed to old barrier and modified log for easy parsing --- buzz_scripts/include/act/old_barrier.bzz | 69 ++++++++++++++++++++++++ buzz_scripts/include/act/states.bzz | 6 +-- src/roscontroller.cpp | 11 ++-- 3 files changed, 78 insertions(+), 8 deletions(-) create mode 100644 buzz_scripts/include/act/old_barrier.bzz diff --git a/buzz_scripts/include/act/old_barrier.bzz b/buzz_scripts/include/act/old_barrier.bzz new file mode 100644 index 0000000..ef3c5fc --- /dev/null +++ b/buzz_scripts/include/act/old_barrier.bzz @@ -0,0 +1,69 @@ +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_TIMEOUT = 1200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil + +# +# Sets a barrier +# +function barrier_create() { + # reset + timeW = 0 + # create barrier vstig + #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) +} + +function barrier_set(threshold, transf, resumef,bc) { + statef = function() { + barrier_wait(threshold, transf, resumef,bc); + } + BVMSTATE = "BARRIERWAIT" + barrier_create() +} + +# +# Make yourself ready +# +function barrier_ready(bc) { + #log("BARRIER READY -------") + barrier.put(id, 1) + barrier.put("d", 0) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf, resumef,bc) { + barrier.put(id, 1) + + barrier.get(id) + log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + barrier.put("d", 1) + timeW = 0 + BVMSTATE = transf + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier=nil + timeW = 0 + BVMSTATE = resumef + } + + timeW = timeW+1 +} + diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 51624a2..c379bc5 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,7 +4,7 @@ # ######################################## include "utils/vec2.bzz" -include "act/barrier.bzz" +include "act/old_barrier.bzz" include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. @@ -191,8 +191,8 @@ function rc_cmd_listen() { } else if(flight.rc_cmd==21) { flight.rc_cmd=0 AUTO_LAUNCH_STATE = "TURNEDOFF" - barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) - barrier_ready(21) + #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + #barrier_ready(21) BVMSTATE = "STOP" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==20) { diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c56ac72..2c8265e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -170,16 +170,18 @@ void roscontroller::RosControllerRun() log< in_msg = buzz_utility::get_inmsg_vector(); + log <<(int)in_msg.size()<<","; + 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 << ","; } - std::vector in_msg = buzz_utility::get_inmsg_vector(); - log <<(int)in_msg.size()<<","; + for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ uint8_t* first_INmsg = (uint8_t*)(*it); size_t tot = 0; @@ -190,8 +192,7 @@ void roscontroller::RosControllerRun() uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); log<<(int)neigh_id<<","<<(int)msg_size<<","; } - - log < Date: Fri, 27 Jul 2018 00:59:39 -0400 Subject: [PATCH 60/98] Moved time sync algo to ROS side and computes in ns, as an attempt to measure msg delays. --- buzz_scripts/include/act/states.bzz | 8 +- buzz_scripts/include/timesync.bzz | 86 ++----------- buzz_scripts/main.bzz | 8 +- include/buzz_utility.h | 20 ++- include/roscontroller.h | 18 ++- src/buzz_utility.cpp | 10 +- src/roscontroller.cpp | 187 +++++++++++++++++++++++++--- 7 files changed, 219 insertions(+), 118 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c379bc5..c50e140 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -217,9 +217,7 @@ function rc_cmd_listen() { stattab_send() } else if (flight.rc_cmd==777){ flight.rc_cmd=0 - if(logical_time != nil) reinit_time_sync() - barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) - barrier_ready(777) + reinit_time_sync() neighbors.broadcast("cmd", 777) }else if (flight.rc_cmd==900){ flight.rc_cmd=0 @@ -266,9 +264,7 @@ function nei_cmd_listen() { } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() } else if(value==777 and BVMSTATE=="TURNEDOFF"){ - if(logical_time != nil) reinit_time_sync() - barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) - barrier_ready(777) + reinit_time_sync() #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index fecf9d9..41f6561 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -1,87 +1,15 @@ -include "utils/string.bzz" - -# Time to be sync -logical_time = 0 -# sync algo. constants -TIME_JUMP_THR = 5 -TIME_TO_FORGET = 20 -TIME_TO_SYNC = 200 -COM_DELAY = 2 -# table to store neighbor time data -time_nei_table = {} -# Algo. global parameters -diffMaxLogical = 0 -jumped = 0 -syncError = 99999 +TIME_TO_SYNC = 100 sync_timer = 0 - -# Function to intialize sync algorithm -function init_time_sync(){ - neighbors.listen("time_sync", - function(vid, value, rid) { - if(value != nil){ - log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) - var msg_time = value.time - var msg_max = value.max - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - if(msg_time != nil and msg_max != nil){ - diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) - var time_offset = msg_time - logical_time - if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset - if(time_offset > TIME_JUMP_THR){ - logical_time = logical_time + time_offset - diffMaxLogical = math.max(diffMaxLogical-time_offset,0) - jumped = 1 - } - time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} - } - } - } - ) -} +timesync_state = 0 # Function to sync. algo -function step_time_sync(){ - logical_time = logical_time + 1 +function check_time_sync(){ sync_timer = sync_timer + 1 - log("Logical time now ", logical_time) - if(sync_timer < TIME_TO_SYNC){ - log(" SYNC ALGO ACTIVE time:", sync_timer) - cnt = 0 - avg_offset = 0 - if(size(time_nei_table) > 0){ - foreach(time_nei_table, function(key, value) { - if(value.time != 0){ - var local_offset = value.time - logical_time + value.age - if(local_offset > 0){ - avg_offset = avg_offset + 1 * local_offset - cnt = cnt + 1 - } - else{ - if(math.abs(local_offset) TIME_TO_FORGET) - value.time = 0 - - value.age = value.age + 1 - } - }) - if(cnt > 0 and jumped != 1){ - var correction = math.ceil(avg_offset / (cnt + 1) ) - if(math.abs(correction) < TIME_JUMP_THR){ - logical_time = logical_time + correction - } - } - } - jumped = 0 - syncError=0 - var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - neighbors.broadcast("time_sync",mstr) + if(sync_timer < TIME_TO_SYNC){ + log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + timesync_state = 1 } + else timesync_state = 0 } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e187ce4..26786d2 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -11,7 +11,7 @@ include "timesync.bzz" AUTO_LAUNCH_STATE = "FORMATION" #AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network -LOWEST_ROBOT_ID = 97 +LOWEST_ROBOT_ID = 1 TARGET = 9.0 EPSILON = 30.0 @@ -33,8 +33,6 @@ function init() { init_stig() init_swarm() - init_time_sync() - TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -48,8 +46,8 @@ function init() { # Executed at each time step. function step() { - # step time sync algorithm - step_time_sync() + # check time sync algorithm state + check_time_sync() # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 4eda0fb..292f8b7 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -43,6 +43,24 @@ struct neiStatus }; typedef struct neiStatus neighbors_status; +struct neitime +{ + uint64_t nei_hardware_time; + uint64_t nei_logical_time; + uint64_t node_hardware_time; + uint64_t node_logical_time; + double nei_rate; + double relative_rate; + int age; + neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) + : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), + nei_rate(nr), relative_rate(mr) {}; + neitime() + { + } +}; +typedef struct neitime neighbor_time; + uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); @@ -82,5 +100,5 @@ std::vector get_inmsg_vector(); std::string getuavstate(); -int getlogicaltime(); +int get_timesync_state(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 566b1a0..f9c684b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,11 +35,17 @@ #include #include #include +#include #include "buzzuav_closures.h" #define UPDATER_MESSAGE_CONSTANT 987654321 -#define XBEE_MESSAGE_CONSTANT 586782343 -#define XBEE_STOP_TRANSMISSION 4355356352 +#define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 +#define BUZZ_MESSAGE_CONSTANT_TIME 523534343 +// Time sync algo. constants +#define COM_DELAY 100000000 // in nano seconds i.e 100 ms +#define TIME_SYNC_JUMP_THR 500000000 +#define MOVING_AVERAGE_ALPHA 0.1 + #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 @@ -90,8 +96,13 @@ private: uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; + std::map neighbours_time_map; int timer_step = 0; int robot_id = 0; + ros::Time logical_clock; + ros::Time previous_step_time; + double logical_time_rate; + bool time_sync_jumped; std::string robot_name = ""; int rc_cmd; @@ -268,6 +279,7 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); void get_xbee_status(); - + void time_sync_step(); + void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2cfcfd6..fdc7277 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -585,19 +585,19 @@ string getuavstate() return uav_state; } -int getlogicaltime() +int get_timesync_state() /* / return current logical time --------------------------------------*/ { - int logical_time = 0; + int timesync_state = 0; if(VM){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1)); buzzvm_gload(VM); buzzobj_t obj = buzzvm_stack_at(VM, 1); - if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value; buzzvm_pop(VM); } - return logical_time; + return timesync_state; } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2c8265e..e6b1c44 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,7 +13,8 @@ namespace rosbzz_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 ---------------*/ @@ -60,6 +61,11 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } + // time sync algo. parameter intialization + previous_step_time.fromNSec(ros::Time::now().toNSec()); + logical_clock.fromNSec(ros::Time::now().toNSec()); + logical_time_rate = 0; + time_sync_jumped = false; // Create log dir and open log file std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; @@ -164,9 +170,10 @@ void roscontroller::RosControllerRun() if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data - log<first<<","; log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } @@ -193,6 +201,9 @@ void roscontroller::RosControllerRun() log<<(int)neigh_id<<","<<(int)msg_size<<","; } log<payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) + else if (msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME || + msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_WTO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1106,8 +1134,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME) index = 5; // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1118,8 +1148,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - delete[] out; - buzz_utility::in_msg_append((message_obt + 3)); + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME){ + // update time struct for sync algo + double nr = 0; + push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } + else{ + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } } } @@ -1382,4 +1421,114 @@ void roscontroller::get_xbee_status() * TriggerAPIRssi(all_ids); */ } + +void roscontroller::time_sync_step() +/* + * Steps the time syncronization algorithm + ------------------------------------------------------------------ */ +{ + // ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + double avgRate = logical_time_rate; + double avgOffset = 0; + int offsetCount = 0; + map::iterator it; + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) + { + 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; + // ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); + if(offset > 0){ + // neighbor is ahead in time + avgOffset = avgOffset + (1 * offset); + offsetCount++; + } + else{ + if(std::abs(offset)second).age < BUZZRATE) (it->second).age++; + else neighbours_time_map.erase(it); + } + if(offsetCount>0 && !time_sync_jumped){ + int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); + if(std::abs(correction) < TIME_SYNC_JUMP_THR){ + // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); + // correct time with the diff within this step + differnce in nei + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + // ROS_INFO(" time sync loop : avgOffset %f, offsetCount : %d , correction %" PRId64 "time diff %" PRIu64 " old_time %" PRIu64 " rate %f", + // avgOffset, offsetCount, correction, time_diff, old_time, avgRate); + uint64_t final_time = old_time + time_diff + (uint64_t)correction;// + (time_diff * avgRate); + + // ROS_INFO(" Final time: %" PRIu64, final_time); + logical_clock.fromNSec(final_time); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + // ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + //ROS_INFO("inside non jump without coorection time sync : %f ", logical_clock.toSec()); + } + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + //ROS_INFO("without correction time sync : %f ", logical_clock.toSec()); + } + time_sync_jumped = false; + //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 + ------------------------------------------------------------------ */ +{ + map::iterator it = neighbours_time_map.find(nid); + double relativeRate =0; + if (it != neighbours_time_map.end()){ + uint64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + uint64_t delatNei = round((nh - (it->second).nei_hardware_time ) + + (nh - (it->second).nei_hardware_time ) * nr); + double currentRate = 0; + if(deltaLocal !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; + relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + + (1- MOVING_AVERAGE_ALPHA)*currentRate; + + // ROS_INFO("deltaLocal %" PRIu64 ", delatNei %" PRIu64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, + // (it->second).relative_rate, relativeRate); + neighbours_time_map.erase(it); + } + int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); + // ROS_INFO("neigh msg push from %i "", my clock %" PRIu64 ", nei clock %" PRIu64 ", offset: %" PRId64", relative rate %f", nid, logical_clock.toNSec(), nl, offset, relativeRate); + if(offset > TIME_SYNC_JUMP_THR){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff + offset );// + time_diff * logical_time_rate ); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + time_sync_jumped = true; + } + buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), + logical_clock.toNSec(), nr, relativeRate); + neighbours_time_map.insert(make_pair(nid, nt)); + + // ROS_INFO("neigh time sync : %f , nei logical %i", logical_clock.toSec(), nl); +} } From 145598f18c4e087da1dae623539a5307d9277c32 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 27 Jul 2018 20:30:51 -0400 Subject: [PATCH 61/98] com delay parameter tunning for time sync algo. --- include/roscontroller.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index f9c684b..7807601 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -42,7 +42,7 @@ #define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 #define BUZZ_MESSAGE_CONSTANT_TIME 523534343 // Time sync algo. constants -#define COM_DELAY 100000000 // in nano seconds i.e 100 ms +#define COM_DELAY 33000000 // in nano seconds i.e 33 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 From 3853479d639eb6e0e6d4344e6f99a1708d8307f2 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 21:03:39 -0400 Subject: [PATCH 62/98] improved sync algo. and packet delay estimation log. --- include/buzz_utility.h | 14 ++- include/roscontroller.h | 53 +++++++- src/roscontroller.cpp | 260 ++++++++++++++++++++++------------------ 3 files changed, 206 insertions(+), 121 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 292f8b7..951207e 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -34,6 +34,18 @@ struct rb_struct }; typedef struct rb_struct RB_struct; +struct neiMsg +{ + uint8_t* msg; + uint16_t msgid; + double time_received; + double time_sent; + neiMsg(uint8_t* m, uint16_t mid, double tr, double ts) + : msg(m), msgid(mid), time_received(tr), time_sent(ts) {}; + neiMsg(); +}; +typedef struct pos_struct nei_msg_struct; + struct neiStatus { uint gps_strenght = 0; @@ -54,7 +66,7 @@ struct neitime int age; neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), - nei_rate(nr), relative_rate(mr) {}; + nei_rate(nr), relative_rate(mr),age(0) {}; neitime() { } diff --git a/include/roscontroller.h b/include/roscontroller.h index 7807601..3e5739d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -38,9 +38,16 @@ #include #include "buzzuav_closures.h" -#define UPDATER_MESSAGE_CONSTANT 987654321 -#define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 -#define BUZZ_MESSAGE_CONSTANT_TIME 523534343 +/* +* ROSBuzz message types +*/ +typedef enum { + ROS_BUZZ_MSG_NIL = 0, // dummy msg + UPDATER_MESSAGE, // Update msg + BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info + BUZZ_MESSAGE_TIME, // Broadcast message with time info +} rosbuzz_msgtype; + // Time sync algo. constants #define COM_DELAY 33000000 // in nano seconds i.e 33 ms #define TIME_SYNC_JUMP_THR 500000000 @@ -55,6 +62,25 @@ using namespace std; namespace rosbzz_node { ++ +template +class circularBuffer { +private: + vector data; + unsigned int lastEntryPos; + int size; + +public: + circularBuffer(uint8_t s): data(s), lastEntryPos(0), size(s){}; + ~circularBuffer(){}; + void push(T d){ + data[lastEntryPos] = d; + if(lastEntryPos > size-1) lastEntryPos = 0; + else lastEntryPos++; + } + vector get_data(){ return data;}; +}; + class roscontroller { public: @@ -91,6 +117,20 @@ private: }; typedef struct POSE ros_pose; + struct MsgData + { + int msgid; + uint16_t nid; + uint16_t size; + double sent_time; + double received_time; + MsgData(int mi, uint16_t ni, uint16_t s, double st, double 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; @@ -103,6 +143,9 @@ private: ros::Time previous_step_time; double logical_time_rate; bool time_sync_jumped; + double com_delay; + std::vector inmsgdata; + double out_msg_time; std::string robot_name = ""; int rc_cmd; @@ -111,7 +154,7 @@ private: int barrier; int update; int statepub_active; - int message_number = 0; + int16_t msg_id = 0; uint8_t no_of_robots = 0; bool rcclient; bool xbeeplugged = false; @@ -281,5 +324,7 @@ private: void get_xbee_status(); void time_sync_step(); void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); + uint64_t get_logical_time(); + void set_logical_time_correction(uint64_t cor); }; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index e6b1c44..6d273e2 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -63,9 +63,12 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) } // time sync algo. parameter intialization previous_step_time.fromNSec(ros::Time::now().toNSec()); - logical_clock.fromNSec(ros::Time::now().toNSec()); - logical_time_rate = 0; + if(robot_id==10)logical_clock.fromSec(101.0); + else logical_clock.fromNSec(0); + logical_time_rate = 1; time_sync_jumped = false; + com_delay = 0; + out_msg_time=0; // Create log dir and open log file std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; @@ -172,38 +175,38 @@ void roscontroller::RosControllerRun() // log data // hardware time now log< in_msg = buzz_utility::get_inmsg_vector(); - log <<(int)in_msg.size()<<","; - log < 0)log<<","; map::iterator it = neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { log<< it->first<<","; log << (double)it->second.x << "," << (double)it->second.y - << "," << (double)it->second.z << ","; + << "," << (double)it->second.z; } - - for (std::vector::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){ - uint8_t* first_INmsg = (uint8_t*)(*it); - size_t tot = 0; - // Size is at first 2 bytes - uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); - tot += sizeof(uint16_t); - // Decode neighbor Id - uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); - log<<(int)neigh_id<<","<<(int)msg_size<<","; + for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ + log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<sent_time + <<","<received_time; } + inmsgdata.clear(); log<payload64[0]; + uint8_t r8header[4]; + uint8_t mtype; + uint16_t mid; + float stime; + memcpy(r8header,&rheader, 7*sizeof(uint8_t)); + memcpy(&mtype, r8header, sizeof(uint8_t)); + memcpy(&mid, r8header+1, sizeof(uint16_t)); + memcpy(&stime, r8header+3, sizeof(float)); + + ROS_INFO("Received Header : mtype %u mid %u stime %f", mtype,mid,stime); // Check for Updater message, if updater message push it into updater FIFO - if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) + if ((int)mtype == (int)UPDATER_MESSAGE) { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; @@ -1105,7 +1156,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // Copy packet into temporary buffer neglecting update constant memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t*)(pl); - fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); + if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize); if (unMsgSize > 0) { buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); @@ -1114,8 +1165,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) free(pl); } // BVM FIFO message - else if (msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME || - msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_WTO_TIME) + else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || + (int)mtype == (int)BUZZ_MESSAGE_WTO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1123,6 +1174,15 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { message_obt[i - 1] = (uint64_t)msg->payload64[i]; } + // determine buzz msg index based on msg type + int index = 3; + if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; + // Extract robot id of the neighbour + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); + get_logical_time(); + // store in msg data for data log + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + inmsgdata.push_back(inm); // Extract neighbours position from payload double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); @@ -1133,11 +1193,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; + // Compute RB in robot body ref. frame gps_rb(nei_pos, cvt_neighbours_pos_payload); - int index = 3; - if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME) index = 5; - // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1148,17 +1205,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME){ + if((int)mtype == (int)BUZZ_MESSAGE_TIME){ // update time struct for sync algo double nr = 0; push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); - delete[] out; buzz_utility::in_msg_append((message_obt + index)); } else{ - delete[] out; buzz_utility::in_msg_append((message_obt + index)); } + delete[] out; } } @@ -1427,7 +1483,7 @@ void roscontroller::time_sync_step() * Steps the time syncronization algorithm ------------------------------------------------------------------ */ { - // ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); double avgRate = logical_time_rate; double avgOffset = 0; int offsetCount = 0; @@ -1437,98 +1493,70 @@ 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; - // ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); - if(offset > 0){ - // neighbor is ahead in time - avgOffset = avgOffset + (1 * offset); - offsetCount++; - } - else{ - if(std::abs(offset)second).nei_logical_time, (int64_t)(it->second).node_logical_time); if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); } + avgRate = avgRate/(neighbours_time_map.size()+1); if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); if(std::abs(correction) < TIME_SYNC_JUMP_THR){ // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); - // correct time with the diff within this step + differnce in nei + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - // ROS_INFO(" time sync loop : avgOffset %f, offsetCount : %d , correction %" PRId64 "time diff %" PRIu64 " old_time %" PRIu64 " rate %f", - // avgOffset, offsetCount, correction, time_diff, old_time, avgRate); - uint64_t final_time = old_time + time_diff + (uint64_t)correction;// + (time_diff * avgRate); - - // ROS_INFO(" Final time: %" PRIu64, final_time); - logical_clock.fromNSec(final_time); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - - // ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); - } - else{ - // correct time with the diff within this step + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - //ROS_INFO("inside non jump without coorection time sync : %f ", logical_clock.toSec()); + set_logical_time_correction(correction); + ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); } } - else{ - // correct time with the diff within this step + rate change - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - avgRate = avgRate/(neighbours_time_map.size()+1); - logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); - previous_step_time.fromNSec(ros::Time::now().toNSec()); - - //ROS_INFO("without correction time sync : %f ", logical_clock.toSec()); - } + get_logical_time(); // just to update clock time_sync_jumped = false; //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 ------------------------------------------------------------------ */ { + ROS_INFO("Received from [%i] L %"PRIu64" , H: %"PRIu64" , nei rate %f",nid, nl,nh,nr); map::iterator it = neighbours_time_map.find(nid); - double relativeRate =0; + double relativeRate =1; if (it != neighbours_time_map.end()){ - uint64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; - uint64_t delatNei = round((nh - (it->second).nei_hardware_time ) - + (nh - (it->second).nei_hardware_time ) * nr); + int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; - if(deltaLocal !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; + ROS_INFO("current in hand for nei : me H %"PRIu64" , nei H %"PRIu64" ", (it->second).node_hardware_time, + (it->second).nei_hardware_time ); + if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + (1- MOVING_AVERAGE_ALPHA)*currentRate; - // ROS_INFO("deltaLocal %" PRIu64 ", delatNei %" PRIu64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, - // (it->second).relative_rate, relativeRate); + 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); } int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); - // ROS_INFO("neigh msg push from %i "", my clock %" PRIu64 ", nei clock %" PRIu64 ", offset: %" PRId64", relative rate %f", nid, logical_clock.toNSec(), nl, offset, relativeRate); if(offset > TIME_SYNC_JUMP_THR){ - uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); - uint64_t old_time = logical_clock.toNSec(); - logical_clock.fromNSec(old_time + time_diff + offset );// + time_diff * logical_time_rate ); - previous_step_time.fromNSec(ros::Time::now().toNSec()); + set_logical_time_correction(offset);// + time_diff * logical_time_rate ); time_sync_jumped = true; + ROS_WARN("CLock Jumped %"PRIu64, logical_clock.toNSec()); } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - // ROS_INFO("neigh time sync : %f , nei logical %i", logical_clock.toSec(), nl); } + + uint64_t roscontroller::get_logical_time(){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff);// + time_diff * logical_time_rate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + return logical_clock.toNSec(); + } + void roscontroller::set_logical_time_correction(uint64_t cor){ + uint64_t l_time_now = get_logical_time(); + logical_clock.fromNSec(l_time_now + cor); + } } From 28a31e42a1208a10e84787ace172a71b441f6e25 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 21:18:10 -0400 Subject: [PATCH 63/98] typo fix --- include/roscontroller.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 3e5739d..1ab67af 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -62,7 +62,7 @@ using namespace std; namespace rosbzz_node { -+ + template class circularBuffer { private: From ef17789c5a3145727e07d7767900a9c038ce5009 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:08:52 -0400 Subject: [PATCH 64/98] stack buffer overflow bug fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6d273e2..393eed9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -616,7 +616,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); From 3602b4c2d6667ef234447dd1bb4070b8c3843ae5 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:24:44 -0400 Subject: [PATCH 65/98] stack fix --- src/roscontroller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 393eed9..4d3e0b7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -642,7 +642,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); @@ -703,7 +703,7 @@ with size......... | / uint16_t mid = (uint16_t)msg_id; get_logical_time(); float stime = (float)logical_clock.toSec(); - uint8_t r8header[4]; + uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); @@ -1130,7 +1130,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { // decode msg header uint64_t rheader = msg->payload64[0]; - uint8_t r8header[4]; + uint8_t r8header[8]; uint8_t mtype; uint16_t mid; float stime; From 323004feceaa7db0ec3a9c87a0943dcabf8c881e Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 22:52:33 -0400 Subject: [PATCH 66/98] removed sync algo. debug prints --- src/roscontroller.cpp | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 4d3e0b7..c0e0cac 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -622,7 +622,6 @@ with size......... | / memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(float)); memcpy(&rheader, r8header, 7*sizeof(uint8_t)); - ROS_INFO("SENT Header : mtype %u mid %u stime %f", mtype,mid,stime); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -632,9 +631,6 @@ with size......... | / // add time sync algo data payload_out.payload64.push_back(ros::Time::now().toNSec()); payload_out.payload64.push_back(logical_clock.toNSec()); - // ROS_INFO("[%i] SENT_MSG NO : %i",robot_id, msg_id); - ROS_INFO("SENT L CLock : %"PRIu64" , H clock : %"PRIu64" , Rate %f", - ros::Time::now().toNSec(),logical_clock.toNSec(), logical_time_rate ); } else{ // prepare rosbuzz msg header @@ -648,7 +644,6 @@ with size......... | / memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(float)); memcpy(&rheader, r8header, 7*sizeof(uint8_t)); - ROS_INFO("SENT Header : mtype %u mid %u stime %f", mtype,mid,stime); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -1139,7 +1134,6 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) memcpy(&mid, r8header+1, sizeof(uint16_t)); memcpy(&stime, r8header+3, sizeof(float)); - ROS_INFO("Received Header : mtype %u mid %u stime %f", mtype,mid,stime); // Check for Updater message, if updater message push it into updater FIFO if ((int)mtype == (int)UPDATER_MESSAGE) { @@ -1495,7 +1489,6 @@ void roscontroller::time_sync_step() int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; avgOffset = avgOffset + offset; offsetCount++; - ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); } @@ -1503,9 +1496,7 @@ void roscontroller::time_sync_step() if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); if(std::abs(correction) < TIME_SYNC_JUMP_THR){ - // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); set_logical_time_correction(correction); - ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); } } get_logical_time(); // just to update clock @@ -1519,20 +1510,17 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou * pushes a time syncronization msg into its data slot ------------------------------------------------------------------ */ { - ROS_INFO("Received from [%i] L %"PRIu64" , H: %"PRIu64" , nei rate %f",nid, nl,nh,nr); map::iterator it = neighbours_time_map.find(nid); double relativeRate =1; if (it != neighbours_time_map.end()){ int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; int64_t delatNei = round(nh - (it->second).nei_hardware_time); double currentRate = 0; - ROS_INFO("current in hand for nei : me H %"PRIu64" , nei H %"PRIu64" ", (it->second).node_hardware_time, - (it->second).nei_hardware_time ); if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; 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); } @@ -1540,7 +1528,6 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou if(offset > TIME_SYNC_JUMP_THR){ set_logical_time_correction(offset);// + time_diff * logical_time_rate ); time_sync_jumped = true; - ROS_WARN("CLock Jumped %"PRIu64, logical_clock.toNSec()); } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); From efb43f0543868b4b08e8e2de8a2b6d26df8e6b85 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sat, 28 Jul 2018 23:55:38 -0400 Subject: [PATCH 67/98] endieness floting point possible fix --- src/roscontroller.cpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c0e0cac..3d69680 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -615,13 +615,14 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %f for %d",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -637,13 +638,14 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %f for %d",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); payload_out.payload64.push_back(position[0]); @@ -697,13 +699,13 @@ with size......... | / uint8_t mtype = (uint8_t)UPDATER_MESSAGE; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - float stime = (float)logical_clock.toSec(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); - memcpy(r8header+3, &stime, sizeof(float)); - memcpy(&rheader, r8header, 7*sizeof(uint8_t)); + memcpy(r8header+3, &stime, sizeof(uint32_t)); + memcpy(&rheader, r8header, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader); for (int i = 0; i < total_size; i++) @@ -1128,12 +1130,13 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint8_t r8header[8]; uint8_t mtype; uint16_t mid; - float stime; - memcpy(r8header,&rheader, 7*sizeof(uint8_t)); + uint32_t temptime; + memcpy(r8header,&rheader, sizeof(uint64_t)); memcpy(&mtype, r8header, sizeof(uint8_t)); memcpy(&mid, r8header+1, sizeof(uint16_t)); - memcpy(&stime, r8header+3, sizeof(float)); - + memcpy(&temptime, r8header+3, sizeof(uint32_t)); + float stime = (float)temptime/(float)100000; + ROS_INFO("received stime %f for %u",stime, mid); // Check for Updater message, if updater message push it into updater FIFO if ((int)mtype == (int)UPDATER_MESSAGE) { From c84a2110d41a4e6ec197184a736d3b3dc32dc298 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Sun, 29 Jul 2018 04:08:25 +0000 Subject: [PATCH 68/98] Debug flotting point value serialization --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3d69680..9e782b3 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -615,8 +615,8 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); + uint32_t stime = (uint32_t) (logical_clock.toSec() * 100000); + ROS_INFO("Sent stime %u for %u",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); @@ -638,8 +638,8 @@ with size......... | / uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; uint16_t mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); + uint32_t stime = (uint32_t) (logical_clock.toSec() * 100000); + ROS_INFO("Sent stime %u for %u",stime,mid); uint8_t r8header[8]; uint64_t rheader = 0; memcpy(r8header, &mtype, sizeof(uint8_t)); From 522e07f5a3d5ece04edd00b346d5ced051a55ee1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 00:21:35 -0400 Subject: [PATCH 69/98] debug serialization --- src/roscontroller.cpp | 32 +++++++++++++------------------- 1 file changed, 13 insertions(+), 19 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3d69680..6e10840 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -610,15 +610,17 @@ with size......... | / msg_id++; payload_out.sysid = (uint8_t)robot_id; payload_out.msgid = (uint32_t)msg_id; + uint8_t mtype; + uint16_t mid = (uint16_t)msg_id; + get_logical_time(); + uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; + ROS_INFO("Sent stime %u for %u",stime,mid); + uint8_t r8header[8]; + uint64_t rheader = 0; + if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)BUZZ_MESSAGE_TIME; - uint16_t mid = (uint16_t)msg_id; - get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); - uint8_t r8header[8]; - uint64_t rheader = 0; + mtype = (uint8_t)BUZZ_MESSAGE_TIME; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); @@ -635,13 +637,7 @@ with size......... | / } else{ // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; - uint16_t mid = (uint16_t)msg_id; - get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - ROS_INFO("Sent stime %f for %d",stime,mid); - uint8_t r8header[8]; - uint64_t rheader = 0; + mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); @@ -696,12 +692,10 @@ with size......... | / memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); free(buff_send); // prepare rosbuzz msg header - uint8_t mtype = (uint8_t)UPDATER_MESSAGE; - uint16_t mid = (uint16_t)msg_id; + mtype = (uint8_t)UPDATER_MESSAGE; + mid = (uint16_t)msg_id; get_logical_time(); - uint32_t stime = (uint32_t)logical_clock.toSec() * 100000; - uint8_t r8header[8]; - uint64_t rheader = 0; + stime = (uint32_t)logical_clock.toSec() * 100000; memcpy(r8header, &mtype, sizeof(uint8_t)); memcpy(r8header+1, &mid, sizeof(uint16_t)); memcpy(r8header+3, &stime, sizeof(uint32_t)); From f86da0d2e197da259a468a78e18d0e3fce2dc529 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 18:28:08 -0400 Subject: [PATCH 70/98] fixed the issues with serialization and time sync algo. --- buzz_scripts/include/timesync.bzz | 13 ++- include/buzz_utility.h | 16 +--- include/roscontroller.h | 29 +----- src/buzz_utility.cpp | 18 +++- src/roscontroller.cpp | 151 ++++++++++++++---------------- 5 files changed, 107 insertions(+), 120 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index 41f6561..8843795 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -1,5 +1,7 @@ TIME_TO_SYNC = 100 sync_timer = 0 +timesync_old_state = 0 +timesync_itr = 0 timesync_state = 0 # Function to sync. algo @@ -9,11 +11,18 @@ function check_time_sync(){ log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) timesync_state = 1 } - else timesync_state = 0 + else{ + timesync_state = 0 + } + if(timesync_old_state == 0 and timesync_state == 1){ + timesync_itr = timesync_itr + 1 + timesync_old_state = 0 + } + timesync_old_state = timesync_state } # Function to set sync timer to zero and reinitiate sync. algo function reinit_time_sync(){ sync_timer = 0 -} \ No newline at end of file +} diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 951207e..047b575 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -34,18 +34,6 @@ struct rb_struct }; typedef struct rb_struct RB_struct; -struct neiMsg -{ - uint8_t* msg; - uint16_t msgid; - double time_received; - double time_sent; - neiMsg(uint8_t* m, uint16_t mid, double tr, double ts) - : msg(m), msgid(mid), time_received(tr), time_sent(ts) {}; - neiMsg(); -}; -typedef struct pos_struct nei_msg_struct; - struct neiStatus { uint gps_strenght = 0; @@ -66,7 +54,7 @@ struct neitime int age; neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), - nei_rate(nr), relative_rate(mr),age(0) {}; + nei_rate(nr), relative_rate(mr) {}; neitime() { } @@ -113,4 +101,6 @@ std::vector get_inmsg_vector(); std::string getuavstate(); int get_timesync_state(); + +int get_timesync_itr(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 1ab67af..9713a9d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -49,7 +49,7 @@ typedef enum { } rosbuzz_msgtype; // Time sync algo. constants -#define COM_DELAY 33000000 // in nano seconds i.e 33 ms +#define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 @@ -62,25 +62,6 @@ using namespace std; namespace rosbzz_node { - -template -class circularBuffer { -private: - vector data; - unsigned int lastEntryPos; - int size; - -public: - circularBuffer(uint8_t s): data(s), lastEntryPos(0), size(s){}; - ~circularBuffer(){}; - void push(T d){ - data[lastEntryPos] = d; - if(lastEntryPos > size-1) lastEntryPos = 0; - else lastEntryPos++; - } - vector get_data(){ return data;}; -}; - class roscontroller { public: @@ -130,7 +111,6 @@ private: }; typedef struct MsgData msg_data; - ros_pose target, home, cur_pos; uint64_t payload; @@ -141,11 +121,10 @@ private: int robot_id = 0; ros::Time logical_clock; ros::Time previous_step_time; + std::vector inmsgdata; + double out_msg_time; double logical_time_rate; bool time_sync_jumped; - double com_delay; - std::vector inmsgdata; - double out_msg_time; std::string robot_name = ""; int rc_cmd; @@ -154,7 +133,7 @@ private: int barrier; int update; int statepub_active; - int16_t msg_id = 0; + int message_number = 0; uint8_t no_of_robots = 0; bool rcclient; bool xbeeplugged = false; diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index fdc7277..ebedb52 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -587,7 +587,7 @@ string getuavstate() int get_timesync_state() /* -/ return current logical time +/ return time sync state from bzz script --------------------------------------*/ { int timesync_state = 0; @@ -600,4 +600,20 @@ int get_timesync_state() } return timesync_state; } +int get_timesync_itr() +/* +/ return time sync iteration from bzz script +--------------------------------------*/ +{ + int timesync_itr = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value; + buzzvm_pop(VM); + } + return timesync_itr; +} + } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6e10840..1e42ebe 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -63,11 +63,9 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) } // time sync algo. parameter intialization previous_step_time.fromNSec(ros::Time::now().toNSec()); - if(robot_id==10)logical_clock.fromSec(101.0); - else logical_clock.fromNSec(0); - logical_time_rate = 1; + logical_clock.fromSec(0); + logical_time_rate = 0; time_sync_jumped = false; - com_delay = 0; out_msg_time=0; // Create log dir and open log file std::string path = @@ -172,12 +170,14 @@ void roscontroller::RosControllerRun() } if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // log data // hardware time now log< 0)log<<","; @@ -203,10 +203,10 @@ void roscontroller::RosControllerRun() } inmsgdata.clear(); log<payload64[0]; - uint8_t r8header[8]; - uint8_t mtype; - uint16_t mid; - uint32_t temptime; - memcpy(r8header,&rheader, sizeof(uint64_t)); - memcpy(&mtype, r8header, sizeof(uint8_t)); - memcpy(&mid, r8header+1, sizeof(uint16_t)); - memcpy(&temptime, r8header+3, sizeof(uint32_t)); + uint64_t rhead = msg->payload64[0]; + uint16_t r16head[4]; + memcpy(r16head,&rhead, sizeof(uint64_t)); + uint16_t mtype = r16head[0]; + uint16_t mid = r16head[1]; + uint32_t temptime=0; + memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; ROS_INFO("received stime %f for %u",stime, mid); + ROS_INFO("Received header : type %u mid %u time %u",r16head[0],r16head[1],r16head[2]); // Check for Updater message, if updater message push it into updater FIFO - if ((int)mtype == (int)UPDATER_MESSAGE) + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; @@ -1147,7 +1140,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // Copy packet into temporary buffer neglecting update constant memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); uint16_t unMsgSize = *(uint16_t*)(pl); - if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize); + fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); if (unMsgSize > 0) { buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); @@ -1165,15 +1158,6 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) { message_obt[i - 1] = (uint64_t)msg->payload64[i]; } - // determine buzz msg index based on msg type - int index = 3; - if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; - // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); - get_logical_time(); - // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); - inmsgdata.push_back(inm); // Extract neighbours position from payload double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); @@ -1184,8 +1168,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - // Compute RB in robot body ref. frame gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; + // Extract robot id of the neighbour + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); + get_logical_time(); + // store in msg data for data log + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + inmsgdata.push_back(inm); + if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1196,16 +1188,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - if((int)mtype == (int)BUZZ_MESSAGE_TIME){ + if((int)mtype == (int)BUZZ_MESSAGE_TIME){ // update time struct for sync algo double nr = 0; push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; buzz_utility::in_msg_append((message_obt + index)); } else{ + delete[] out; buzz_utility::in_msg_append((message_obt + index)); } - delete[] out; } } @@ -1529,7 +1522,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - + ROS_INFO("SYNC MSG RECEIVED deltaLocal from %i",nid); } uint64_t roscontroller::get_logical_time(){ From 92cf7c4412917271af88c8eeb34bc71ca7482e8e Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Sun, 29 Jul 2018 19:05:08 -0400 Subject: [PATCH 71/98] Adapted graph script for tests. --- .../include/taskallocate/graphformGPS.bzz | 31 +++++++++---------- buzz_scripts/main.bzz | 3 ++ 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 61e77fd..cb82810 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -10,7 +10,7 @@ include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER -ROOT_ID = 2 +ROOT_ID = 5 # # Global variables @@ -389,7 +389,7 @@ function TransitionToJoined(){ #write statues #v_tag.put(m_nLabel, m_lockstig) barrier_create() - barrier_ready() + barrier_ready(940) m_navigation.x=0.0 m_navigation.y=0.0 @@ -568,7 +568,7 @@ function set_rc_goto() { goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) print("Saving GPS goal: ",goal.latitude, goal.longitude) - uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) + storegoal(goal.latitude, goal.longitude, pose.position.altitude) m_gotjoinedparent = 1 } } @@ -655,8 +655,7 @@ function DoJoined(){ return } } - - barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED") + barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941) BroadcastGraph() } # @@ -681,17 +680,17 @@ function DoLock() { goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() - if(loop) { - if(timeout_graph==0) { - if(graphid < 3) - graphid = graphid + 1 - else - graphid = 0 - timeout_graph = 40 - BVMSTATE="TASK_ALLOCATE" - } - timeout_graph = timeout_graph - 1 - } +# if(loop) { +# if(timeout_graph==0) { +# if(graphid < 3) +# graphid = graphid + 1 +# else +# graphid = 0 +# timeout_graph = 40 +# BVMSTATE="TASK_ALLOCATE" +# } +# timeout_graph = timeout_graph - 1 +# } } # diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 26786d2..97ecc9b 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -32,6 +32,7 @@ goal_list = { function init() { init_stig() init_swarm() + initGraph() TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -108,8 +109,10 @@ function step() { # Executed once when the robot (or the simulator) is reset. function reset() { + resetGraph() } # Executed once at the end of experiment. function destroy() { + destroyGraph() } From 0dd435e80eba3e8235d23921813afcf3ad97d2e8 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Sun, 29 Jul 2018 23:44:43 +0000 Subject: [PATCH 72/98] state change to task allocation --- buzz_scripts/main.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 97ecc9b..1249829 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -8,7 +8,7 @@ include "vstigenv.bzz" include "timesync.bzz" #State launched after takeoff -AUTO_LAUNCH_STATE = "FORMATION" +AUTO_LAUNCH_STATE = "TASK_ALLOCATE" #AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network LOWEST_ROBOT_ID = 1 From 5665ac6ef2bb418a4725d2e75f6df2a1ff01b2cc Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 29 Jul 2018 19:58:19 -0400 Subject: [PATCH 73/98] fixed hook naming issue for storegoal --- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index cb82810..7eccc4f 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -568,7 +568,7 @@ function set_rc_goto() { goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) print("Saving GPS goal: ",goal.latitude, goal.longitude) - storegoal(goal.latitude, goal.longitude, pose.position.altitude) + uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude) m_gotjoinedparent = 1 } } From 5d06389f5ecca779feb3ce9e34699b5c389e3203 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Mon, 30 Jul 2018 03:10:54 +0000 Subject: [PATCH 74/98] changes to pos tolarence value in navigation --- buzz_scripts/include/act/states.bzz | 9 +++++---- src/roscontroller.cpp | 7 +------ 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c50e140..065614f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,10 +10,10 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2.5 # m/steps +GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.5 # m. -GOTOANG_TOL = 0.1 # rad. +GOTODIST_TOL = 0.8 # m. +GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 pic_time = 0 @@ -96,7 +96,8 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - LimitSpeed(m_navigation, 1.0) + #LimitSpeed(m_navigation, 1.0) + log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1e42ebe..0d94cf4 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -614,7 +614,6 @@ with size......... | / tmphead[1] = (uint16_t)message_number; get_logical_time(); uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000); - ROS_INFO("Sent stime %u for %u",stime,message_number); memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); uint64_t rheader[1]; payload_out.sysid = (uint8_t)robot_id; @@ -623,7 +622,6 @@ with size......... | / if(buzz_utility::get_timesync_state()){ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME; - ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]); memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -641,7 +639,6 @@ with size......... | / else{ // prepare rosbuzz msg header tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME; - ROS_INFO("sent header : type %u mid %u time %u",tmphead[0],tmphead[1],tmphead[2]); memcpy(rheader, tmphead, sizeof(uint64_t)); // push header into the buffer payload_out.payload64.push_back(rheader[0]); @@ -1122,8 +1119,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint32_t temptime=0; memcpy(&temptime, r16head+2, sizeof(uint32_t)); float stime = (float)temptime/(float)100000; - ROS_INFO("received stime %f for %u",stime, mid); - ROS_INFO("Received header : type %u mid %u time %u",r16head[0],r16head[1],r16head[2]); + 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 if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) { @@ -1522,7 +1518,6 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); neighbours_time_map.insert(make_pair(nid, nt)); - ROS_INFO("SYNC MSG RECEIVED deltaLocal from %i",nid); } uint64_t roscontroller::get_logical_time(){ From c5f0a3ddd40f70d2fed94919561bf3f85730684e Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Mon, 30 Jul 2018 03:39:49 +0000 Subject: [PATCH 75/98] time sync out of bound fix --- src/roscontroller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0d94cf4..ae70aee 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1477,11 +1477,12 @@ void roscontroller::time_sync_step() offsetCount++; if((it->second).age < BUZZRATE) (it->second).age++; else neighbours_time_map.erase(it); + ROS_INFO("Size of nei time %i",neighbours_time_map.size()); } avgRate = avgRate/(neighbours_time_map.size()+1); if(offsetCount>0 && !time_sync_jumped){ int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); - if(std::abs(correction) < TIME_SYNC_JUMP_THR){ + if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){ set_logical_time_correction(correction); } } @@ -1506,7 +1507,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou 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); } @@ -1517,6 +1518,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou } buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), logical_clock.toNSec(), nr, relativeRate); + nt.age=0; neighbours_time_map.insert(make_pair(nid, nt)); } From 00fdc56642dab32fee34a98d579c9c5e26b28f35 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:09:29 -0400 Subject: [PATCH 76/98] iterator of neighbours time map bug fix --- src/roscontroller.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ae70aee..8a19bfa 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1468,15 +1468,20 @@ void roscontroller::time_sync_step() double avgOffset = 0; int offsetCount = 0; map::iterator it; - for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ) { 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; avgOffset = avgOffset + offset; offsetCount++; - if((it->second).age < BUZZRATE) (it->second).age++; - else neighbours_time_map.erase(it); + if((it->second).age > BUZZRATE){ + neighbours_time_map.erase(it); + } + else{ + (it->second).age++; + ++it; + } ROS_INFO("Size of nei time %i",neighbours_time_map.size()); } avgRate = avgRate/(neighbours_time_map.size()+1); From 5195fbf3293d3f274e118e20750db40ae3faae87 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:24:24 -0400 Subject: [PATCH 77/98] iterator post increment fail fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 8a19bfa..ba440e6 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1476,7 +1476,7 @@ void roscontroller::time_sync_step() avgOffset = avgOffset + offset; offsetCount++; if((it->second).age > BUZZRATE){ - neighbours_time_map.erase(it); + neighbours_time_map.erase(it++); } else{ (it->second).age++; From 79fea108a2f6e4069cb41d9472bc029c9511a246 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 00:54:56 -0400 Subject: [PATCH 78/98] nav parameter adaptation --- buzz_scripts/include/act/states.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 065614f..9fa5df6 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,7 +12,7 @@ BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 0.8 # m. +GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x/2.0, m_navigation.y/2.0, rc_goto.altitude - pose.position.altitude, 0.0) } } From fbc5bd8e72a70c500e493a66b4f66b829faa4889 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:07:50 -0400 Subject: [PATCH 79/98] disabled movement at lock state --- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 7eccc4f..5f387e4 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -677,7 +677,7 @@ function DoLock() { m_navigation=motion_vector() } # #move - goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) +# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) BroadcastGraph() # if(loop) { From 7048c28142c28ca7610a0bfc09f9d9434a916c39 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:23:05 -0400 Subject: [PATCH 80/98] nav param removed scaling --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9fa5df6..5e1db50 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x/2.0, m_navigation.y/2.0, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } From a2a19d558363620943d3682bc17605e6bb8df821 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:32:05 -0400 Subject: [PATCH 81/98] nav param tunning to avoid ossilations --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 5e1db50..f486291 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x/5, m_navigation.y/5, rc_goto.altitude - pose.position.altitude, 0.0) } } From 861305bfcd70790afad51153deb5692a3a0f97be Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 30 Jul 2018 01:40:13 -0400 Subject: [PATCH 82/98] nav parameter --- buzz_scripts/include/act/states.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f486291..9e8fd78 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -12,7 +12,7 @@ BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 1.0 # m. +GOTODIST_TOL = 2.0 # m. GOTOANG_TOL = 0.2 # rad. path_it = 0 graphid = 0 @@ -98,7 +98,7 @@ function goto_gps(transf) { else { #LimitSpeed(m_navigation, 1.0) log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) - goto_abs(m_navigation.x/5, m_navigation.y/5, rc_goto.altitude - pose.position.altitude, 0.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } From 25968cfe6d6f6c2f3cbf78d24d119d80d81e1290 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 31 Jul 2018 09:25:02 +0000 Subject: [PATCH 83/98] Changes on speed limit, navigation and local pos control. --- buzz_scripts/include/act/states.bzz | 8 +++++--- buzz_scripts/include/taskallocate/graphformGPS.bzz | 2 +- buzz_scripts/include/utils/conversions.bzz | 8 ++++---- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9e8fd78..2ec6c4f 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 0.5 # m/steps +GOTO_MAXVEL = 5.0 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 2.0 # m. GOTOANG_TOL = 0.2 # rad. @@ -89,6 +89,7 @@ function take_picture() { } function goto_gps(transf) { + log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude) m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) @@ -96,8 +97,9 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - #LimitSpeed(m_navigation, 1.0) - log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) + log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y ) + m_navigation = LimitSpeed(m_navigation, 1.0) + log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y ) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 5f387e4..11d2984 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -537,7 +537,7 @@ function DoJoining(){ if(m_gotjoinedparent!=1) set_rc_goto() - else + else goto_gps(TransitionToJoined) #pack the communication package m_selfMessage.State=s2i(BVMSTATE) diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 50a58bc..92dc4e1 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) { } ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); - return math.vec2.new(ned_x,ned_y) + m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing) } function gps_from_vec(vec) { @@ -66,4 +66,4 @@ function gps_from_vec(vec) { #goal.longitude = d_lon + pose.position.longitude; return Lgoal -} \ No newline at end of file +} From 79857a3f5582e5854ef3a48abf70772086f010a3 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 31 Jul 2018 18:32:58 +0000 Subject: [PATCH 84/98] lowered the velocity and target threshold. increased altitude --- buzz_scripts/include/act/states.bzz | 6 +++--- buzz_scripts/main.bzz | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 2ec6c4f..a4746af 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,10 +10,10 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 5.0 # m/steps +GOTO_MAXVEL = 2.0 # m/steps GOTO_MAXDIST = 150 # m. -GOTODIST_TOL = 2.0 # m. -GOTOANG_TOL = 0.2 # rad. +GOTODIST_TOL = 1.0 # m. +GOTOANG_TOL = 0.1 # rad. path_it = 0 graphid = 0 pic_time = 0 diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 1249829..e0340d9 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -34,7 +34,7 @@ function init() { init_swarm() initGraph() - TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m loop = 1 # start the swarm command listener From bf4f23bc48b8fe0e7affcfba6d4f8746ecf9b4cd Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Mon, 6 Aug 2018 14:26:17 -0400 Subject: [PATCH 85/98] data logging fixes. --- buzz_scripts/include/act/states.bzz | 2 +- include/roscontroller.h | 6 +++--- src/roscontroller.cpp | 14 +++++++------- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index a4746af..c708df9 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 2.0 # m/steps +GOTO_MAXVEL = 0.2 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. diff --git a/include/roscontroller.h b/include/roscontroller.h index 9713a9d..f1604f0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -104,8 +104,8 @@ private: uint16_t nid; uint16_t size; double sent_time; - double received_time; - MsgData(int mi, uint16_t ni, uint16_t s, double st, double rt): + uint64_t received_time; + 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(){}; }; @@ -122,7 +122,7 @@ private: ros::Time logical_clock; ros::Time previous_step_time; std::vector inmsgdata; - double out_msg_time; + uint64_t out_msg_time; double logical_time_rate; bool time_sync_jumped; std::string robot_name = ""; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ba440e6..2441720 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -176,25 +176,25 @@ void roscontroller::RosControllerRun() log< 0)log<<","; + // if(neighbours_pos_map.size() > 0)log<<","; map::iterator it = neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { log<< it->first<<","; - log << (double)it->second.x << "," << (double)it->second.y + log <<","<< (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ @@ -654,7 +654,7 @@ with size......... | / } //Store out msg time get_logical_time(); - out_msg_time = logical_clock.toSec(); + out_msg_time = logical_clock.toNSec(); // publish prepared messages in respective topic payload_pub.publish(payload_out); delete[] out; @@ -1171,7 +1171,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); get_logical_time(); // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec()); + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec()); inmsgdata.push_back(inm); if (debug) From 670d70601b1a59ef67ecf54a98032370136f5531 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 6 Aug 2018 15:00:38 -0400 Subject: [PATCH 86/98] data logging fix --- src/roscontroller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2441720..6e85238 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -193,8 +193,8 @@ void roscontroller::RosControllerRun() neighbours_pos_map.begin(); for (; it != neighbours_pos_map.end(); ++it) { - log<< it->first<<","; - log <<","<< (double)it->second.x << "," << (double)it->second.y + log<<","<< it->first<<","; + log<< (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z; } for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ From d05dd25bf64aab628adb7a95d056dcc9b06867b6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 6 Aug 2018 15:16:47 -0400 Subject: [PATCH 87/98] data logging fixes --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6e85238..3f704de 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -173,7 +173,7 @@ void roscontroller::RosControllerRun() // log data // hardware time now - log< Date: Wed, 8 Aug 2018 01:09:13 +0000 Subject: [PATCH 88/98] velocity change --- buzz_scripts/include/act/states.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c708df9..7fe503d 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -10,7 +10,7 @@ include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps -GOTO_MAXVEL = 0.2 # m/steps +GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. From f58373e2b5c721d524852f090e606b5a6917bc0a Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Wed, 8 Aug 2018 01:54:48 +0000 Subject: [PATCH 89/98] diabled barrier for stop state --- buzz_scripts/include/act/states.bzz | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 7fe503d..f0d2af2 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -67,12 +67,14 @@ function stop() { neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - barrier_ready(21) + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) } } else { - barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) - barrier_ready(21) + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) } } From f2c662dfa9b0f7fa218fd9d510b695330574d0de Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Tue, 14 Aug 2018 16:27:21 -0400 Subject: [PATCH 90/98] changed logging time to rostime, to test NTP. --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3f704de..f809a6d 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -613,7 +613,7 @@ with size......... | / uint16_t tmphead[4]; tmphead[1] = (uint16_t)message_number; get_logical_time(); - uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000); + uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); uint64_t rheader[1]; payload_out.sysid = (uint8_t)robot_id; @@ -654,7 +654,7 @@ with size......... | / } //Store out msg time get_logical_time(); - out_msg_time = logical_clock.toNSec(); + out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec(); // publish prepared messages in respective topic payload_pub.publish(payload_out); delete[] out; @@ -1169,9 +1169,9 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; // Extract robot id of the neighbour uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); - get_logical_time(); + //get_logical_time(); // store in msg data for data log - msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toNSec()); + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec()); inmsgdata.push_back(inm); if (debug) From b129d26a029580b4c59d0e8cdb7121e51fc9e40f Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 16 Aug 2018 15:28:52 -0400 Subject: [PATCH 91/98] made NED pos available in buzz --- include/buzzuav_closures.h | 2 ++ src/buzzuav_closures.cpp | 18 ++++++++++++++++++ src/roscontroller.cpp | 1 + 3 files changed, 21 insertions(+) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index dbd3110..f43da11 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -89,6 +89,8 @@ void set_filtered_packet_loss(float value); /* * sets current position */ +void set_currentNEDpos(double x, double y); + void set_currentpos(double latitude, double longitude, float altitude, float yaw); /* * returns the current go to position diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1d72cbf..e011c61 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,6 +19,7 @@ static float rc_gimbal[4]; static float batt[3]; static float obst[5] = { 0, 0, 0, 0, 0 }; static double cur_pos[4]; +static double cur_NEDpos[2]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd = 0; @@ -629,6 +630,15 @@ int buzzuav_update_xbee_status(buzzvm_t vm) return vm->state; } +void set_currentNEDpos(double x, double y) +/* +/ update interface position array +-----------------------------------*/ +{ + cur_NEDpos[0] = x; + cur_NEDpos[1] = y; +} + void set_currentpos(double latitude, double longitude, float altitude, float yaw) /* / update interface position array @@ -697,6 +707,14 @@ int buzzuav_update_currentpos(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0)); buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0)); + buzzvm_pushf(vm, cur_NEDpos[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0)); + buzzvm_pushf(vm, cur_NEDpos[1]); + buzzvm_tput(vm); // Store read table in the proximity table buzzvm_push(vm, tPoseTable); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index f809a6d..0d87452 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -987,6 +987,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; + 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( msg->pose.orientation.x, From 2d6d5d0d285213007a2a47ed0bb1d43375f649d4 Mon Sep 17 00:00:00 2001 From: Gwaihir Date: Thu, 16 Aug 2018 16:24:04 -0400 Subject: [PATCH 92/98] namespace fix --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0d87452..d2da99a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -987,7 +987,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; - set_currentNEDpos(msg->pose.position.y,msg->pose.position.x); + 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( msg->pose.orientation.x, From ffbe4569cc4045a30c15fb9638e8c45dbcb8f1a1 Mon Sep 17 00:00:00 2001 From: mistTX1 Date: Tue, 21 Aug 2018 01:40:49 +0000 Subject: [PATCH 93/98] mavros version selection as cmake flag --- CMakeLists.txt | 4 ++-- src/buzzuav_closures.cpp | 8 ++++++++ src/roscontroller.cpp | 21 +++++++++++++++++++++ 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 672d68d..2b48f35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,9 +6,9 @@ if(UNIX) endif() if(SIM) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1") else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1") endif() ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index e011c61..1e368c5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -401,7 +401,11 @@ int buzzuav_arm(buzzvm_t vm) / Buzz closure to arm /---------------------------------------*/ { +#ifdef MAVROSKINETIC + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; +#else cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; +#endif printf(" Buzz requested Arm \n"); buzz_cmd = COMMAND_ARM; return buzzvm_ret0(vm); @@ -412,7 +416,11 @@ int buzzuav_disarm(buzzvm_t vm) / Buzz closure to disarm /---------------------------------------*/ { +#ifdef MAVROSKINETIC + cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; +#else cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; +#endif printf(" Buzz requested Disarm \n"); buzz_cmd = COMMAND_DISARM; return buzzvm_ret0(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d2da99a..5b4df3a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -780,7 +780,11 @@ script } else ROS_ERROR("Failed to call service from flight controller"); +#ifdef MAVROSKINETIC + cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; +#else cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; +#endif if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -818,7 +822,11 @@ script cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; +#ifdef MAVROSKINETIC + cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +#else cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +#endif if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -1220,8 +1228,13 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; +#ifdef MAVROSKINETIC + case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; +#else case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; +#endif armstate = req.param1; if (armstate) { @@ -1249,10 +1262,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m buzzuav_closures::rc_call(rc_cmd); res.success = true; break; +#ifdef MAVROSKINETIC + case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: +#else case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: +#endif ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); +#ifdef MAVROSKINETIC + rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +#else rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +#endif buzzuav_closures::rc_call(rc_cmd); res.success = true; break; From fff1c3398d0a051a6413ba6e4f18458cf281b1bc Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 28 Aug 2018 16:18:29 -0400 Subject: [PATCH 94/98] disabled onboard time sync, hoping NTP would sync. --- buzz_scripts/include/timesync.bzz | 26 +++++++++++++------------- src/roscontroller.cpp | 9 +++++---- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index 8843795..8b933ee 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -6,19 +6,19 @@ timesync_state = 0 # Function to sync. algo function check_time_sync(){ - sync_timer = sync_timer + 1 - if(sync_timer < TIME_TO_SYNC){ - log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) - timesync_state = 1 - } - else{ - timesync_state = 0 - } - if(timesync_old_state == 0 and timesync_state == 1){ - timesync_itr = timesync_itr + 1 - timesync_old_state = 0 - } - timesync_old_state = timesync_state + # sync_timer = sync_timer + 1 + # if(sync_timer < TIME_TO_SYNC){ + # log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + # timesync_state = 1 + # } + # else{ + # timesync_state = 0 + # } + # if(timesync_old_state == 0 and timesync_state == 1){ + # timesync_itr = timesync_itr + 1 + # timesync_old_state = 0 + # } + # timesync_old_state = timesync_state } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5b4df3a..220522b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -204,9 +204,9 @@ void roscontroller::RosControllerRun() inmsgdata.clear(); log<payload64[0] == (uint64_t)UPDATER_MESSAGE) { From c41bb81763584223c188d47bd56b31887263aaf1 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 30 Aug 2018 11:22:37 -0400 Subject: [PATCH 95/98] Fixed the limited buffer size bug in inmsgs and changed to y formation --- buzz_scripts/include/act/states.bzz | 2 +- include/roscontroller.h | 1 + src/roscontroller.cpp | 4 ++-- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f0d2af2..c00c78b 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -15,7 +15,7 @@ GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. GOTOANG_TOL = 0.1 # rad. path_it = 0 -graphid = 0 +graphid = 3 pic_time = 0 g_it = 0 diff --git a/include/roscontroller.h b/include/roscontroller.h index f1604f0..0ed4756 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -52,6 +52,7 @@ typedef enum { #define COM_DELAY 100000000 // in nano seconds i.e 100 ms #define TIME_SYNC_JUMP_THR 500000000 #define MOVING_AVERAGE_ALPHA 0.1 +#define MAX_NUMBER_OF_ROBOTS 10 #define TIMEOUT 60 #define BUZZRATE 10 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 220522b..c03a33e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -392,12 +392,12 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) Subscribe(n_c); - payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + payload_sub = n_c.subscribe(in_payload, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this); // publishers payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); - neigh_pos_pub = n_c.advertise("neighbours_pos", 5); + neigh_pos_pub = n_c.advertise("neighbours_pos", MAX_NUMBER_OF_ROBOTS); uavstate_pub = n_c.advertise("uavstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); From 064760108611591426d86c679c7789b1a95cebee Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 31 Aug 2018 19:59:14 -0400 Subject: [PATCH 96/98] Custom function to test waypoints. --- buzz_scripts/include/act/states.bzz | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c00c78b..09b9eb7 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -31,6 +31,15 @@ function idle() { function cusfun(){ BVMSTATE="CUSFUN" log("cusfun: yay!!!") + LOCAL_TARGET = math.vec2.new(5 ,0 ) + local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location + if(math.vec2.length(local_set_point) > GOTODIST_TOL){ + local_set_point = LimitSpeed(local_set_point, 1.0) + goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) + } + else{ + log("TARGET REACHED") + } } function launch() { From 8cdd5eb10546bdb0d1b5eb22b332b929c4742489 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 7 Sep 2018 00:17:30 -0400 Subject: [PATCH 97/98] force push from ros_drones_ws --- CMakeLists.txt | 12 +- buzz_scripts/include/act/states.bzz | 25 +-- .../include/taskallocate/graphformGPS.bzz | 31 ++-- buzz_scripts/include/utils/conversions.bzz | 6 +- buzz_scripts/main.bzz | 7 +- include/buzz_utility.h | 4 +- include/buzzuav_closures.h | 16 +- include/rosbuzz/mavrosCC.h | 24 +++ include/roscontroller.h | 27 ++- src/buzz_utility.cpp | 55 +++--- src/buzzuav_closures.cpp | 50 +++-- src/rosbuzz.cpp | 2 +- src/roscontroller.cpp | 171 +++++++----------- 13 files changed, 200 insertions(+), 230 deletions(-) create mode 100644 include/rosbuzz/mavrosCC.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b48f35..4d868f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,11 +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 -DMAVROSKINETIC=1") -else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1") -endif() +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}") + ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp @@ -70,6 +67,11 @@ install(TARGETS rosbuzz_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) find_package(catkin REQUIRED COMPONENTS roslaunch) roslaunch_add_file_check(launch) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 09b9eb7..d657797 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -4,12 +4,14 @@ # ######################################## include "utils/vec2.bzz" -include "act/old_barrier.bzz" +include "act/barrier.bzz" +#include "act/old_barrier.bzz" include "utils/conversions.bzz" TARGET_ALTITUDE = 15.0 # m. BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps + GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 1.0 # m. @@ -27,6 +29,7 @@ function idle() { BVMSTATE = "IDLE" } + # Custom function for the user. function cusfun(){ BVMSTATE="CUSFUN" @@ -94,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 } @@ -108,9 +111,7 @@ function goto_gps(transf) { else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination transf() else { - log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y ) - m_navigation = LimitSpeed(m_navigation, 1.0) - log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y ) + m_navigation = LimitSpeed(m_navigation, 1.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) } } @@ -141,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 @@ -163,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 @@ -173,17 +174,17 @@ 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 function formation() { BVMSTATE="FORMATION" @@ -200,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 @@ -303,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 11d2984..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/buzzuav_closures.h b/include/buzzuav_closures.h index f43da11..71b99f1 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -6,6 +6,7 @@ #include "mavros_msgs/Mavlink.h" #include "ros/ros.h" #include "buzz_utility.h" +#include "rosbuzz/mavrosCC.h" #define EARTH_RADIUS (double)6371000.0 #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) @@ -13,19 +14,6 @@ namespace buzzuav_closures { -typedef enum { - COMMAND_NIL = 0, // Dummy command - COMMAND_TAKEOFF, // Take off - COMMAND_LAND, - COMMAND_GOHOME, - COMMAND_ARM, - COMMAND_DISARM, - COMMAND_GOTO, - COMMAND_MOVETO, - COMMAND_PICTURE, - COMMAND_GIMBAL, -} Custom_CommandCode; - /* * prextern int() function in Buzz * This function is used to print data from buzz @@ -89,6 +77,7 @@ void set_filtered_packet_loss(float value); /* * sets current position */ + void set_currentNEDpos(double x, double y); void set_currentpos(double latitude, double longitude, float altitude, float yaw); @@ -101,6 +90,7 @@ double* getgoto(); */ std::map> getgrid(); + /* * returns the gimbal commands */ diff --git a/include/rosbuzz/mavrosCC.h b/include/rosbuzz/mavrosCC.h new file mode 100644 index 0000000..7917bac --- /dev/null +++ b/include/rosbuzz/mavrosCC.h @@ -0,0 +1,24 @@ +#pragma once + +#if MAVROSKINETIC + +const short MISSION_START = mavros_msgs::CommandCode::MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; + +#else + +const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM; + +#endif + +const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF; +const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND; +const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; +const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT; +const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT; +const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE; +const short CMD_REQUEST_UPDATE = 666; +const short CMD_SYNC_CLOCK = 777; diff --git a/include/roscontroller.h b/include/roscontroller.h index 0ed4756..d7bbbc5 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,8 +35,8 @@ #include #include #include -#include #include "buzzuav_closures.h" +#include "rosbuzz/mavrosCC.h" /* * ROSBuzz message types @@ -44,24 +44,22 @@ 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 #define TIMEOUT 60 #define BUZZRATE 10 -#define CMD_REQUEST_UPDATE 666 -#define CMD_SYNC_CLOCK 777 using namespace std; -namespace rosbzz_node +namespace rosbuzz_node { class roscontroller { @@ -99,6 +97,8 @@ private: }; typedef struct POSE ros_pose; + ros_pose target, home, cur_pos; + struct MsgData { int msgid; @@ -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(); @@ -302,6 +300,7 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); void get_xbee_status(); + void time_sync_step(); void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); uint64_t get_logical_time(); 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 1e368c5..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,9 +229,9 @@ 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]); - buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? + // 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); } @@ -332,7 +332,7 @@ int buzzuav_takepicture(buzzvm_t vm) / Buzz closure to take a picture here. /----------------------------------------*/ { - buzz_cmd = COMMAND_PICTURE; + buzz_cmd = IMAGE_START_CAPTURE; return buzzvm_ret0(vm); } @@ -356,7 +356,7 @@ int buzzuav_setgimbal(buzzvm_t vm) rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); - buzz_cmd = COMMAND_GIMBAL; + buzz_cmd = DO_MOUNT_CONTROL; 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); } @@ -401,13 +401,9 @@ int buzzuav_arm(buzzvm_t vm) / Buzz closure to arm /---------------------------------------*/ { -#ifdef MAVROSKINETIC - cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; -#endif + cur_cmd = COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); - buzz_cmd = COMMAND_ARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -416,13 +412,9 @@ int buzzuav_disarm(buzzvm_t vm) / Buzz closure to disarm /---------------------------------------*/ { -#ifdef MAVROSKINETIC - cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; -#endif + cur_cmd = COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); - buzz_cmd = COMMAND_DISARM; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -436,9 +428,9 @@ int buzzuav_takeoff(buzzvm_t vm) buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value; height = goto_pos[2]; - cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; + cur_cmd = NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); - buzz_cmd = COMMAND_TAKEOFF; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -447,9 +439,9 @@ int buzzuav_land(buzzvm_t vm) / Buzz closure to land /-------------------------------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::NAV_LAND; + cur_cmd = NAV_LAND; printf(" Buzz requested Land !!! \n"); - buzz_cmd = COMMAND_LAND; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -458,9 +450,9 @@ int buzzuav_gohome(buzzvm_t vm) / Buzz closure to return Home /-------------------------------------------------------------*/ { - cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + cur_cmd = NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); - buzz_cmd = COMMAND_GOHOME; + buzz_cmd = cur_cmd; return buzzvm_ret0(vm); } @@ -683,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 c03a33e..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]); @@ -714,7 +711,7 @@ script float* gimbal; switch (buzzuav_closures::bzz_cmd()) { - case buzzuav_closures::COMMAND_TAKEOFF: + case NAV_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); @@ -737,7 +734,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_LAND: + case NAV_LAND: cmd_srv.request.command = buzzuav_closures::getcmd(); if (current_mode != "LAND") { @@ -756,7 +753,7 @@ script armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + case NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! cmd_srv.request.param5 = home.latitude; cmd_srv.request.param6 = home.longitude; cmd_srv.request.param7 = home.altitude; @@ -769,32 +766,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! - goto_pos = buzzuav_closures::getgoto(); - cmd_srv.request.param5 = goto_pos[0]; - cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = goto_pos[2]; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); -#ifdef MAVROSKINETIC - cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START; -#else - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; -#endif - if (mav_client.call(cmd_srv)) - { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } - else - ROS_ERROR("Failed to call service from flight controller"); - break; - - case buzzuav_closures::COMMAND_ARM: + case COMPONENT_ARM_DISARM: if (!armstate) { SetMode("LOITER", 0); @@ -803,7 +775,7 @@ script } break; - case buzzuav_closures::COMMAND_DISARM: + case COMPONENT_ARM_DISARM+1: if (armstate) { armstate = 0; @@ -812,22 +784,18 @@ script } break; - case buzzuav_closures::COMMAND_MOVETO: + case NAV_SPLINE_WAYPOINT: goto_pos = buzzuav_closures::getgoto(); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); break; - case buzzuav_closures::COMMAND_GIMBAL: + case DO_MOUNT_CONTROL: gimbal = buzzuav_closures::getgimbal(); cmd_srv.request.param1 = gimbal[0]; cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; -#ifdef MAVROSKINETIC - cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + cmd_srv.request.command = DO_MOUNT_CONTROL; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -836,7 +804,7 @@ script ROS_ERROR("Failed to call service from flight controller"); break; - case buzzuav_closures::COMMAND_PICTURE: + case IMAGE_START_CAPTURE: ROS_INFO("TAKING A PICTURE HERE!! --------------"); mavros_msgs::CommandBool capture_command; if (capture_srv.call(capture_command)) @@ -902,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[]) @@ -917,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; } @@ -996,6 +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( @@ -1127,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 @@ -1156,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 @@ -1217,25 +1183,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m int rc_cmd; switch (req.command) { - case mavros_msgs::CommandCode::NAV_TAKEOFF: + case NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; + rc_cmd = NAV_TAKEOFF; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::NAV_LAND: + case NAV_LAND: ROS_INFO("RC_Call: LAND!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_LAND; + rc_cmd = NAV_LAND; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; -#ifdef MAVROSKINETIC - case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; -#else - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; -#endif + case COMPONENT_ARM_DISARM: + rc_cmd = COMPONENT_ARM_DISARM; armstate = req.param1; if (armstate) { @@ -1250,31 +1211,23 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m res.success = true; } break; - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: + case NAV_RETURN_TO_LAUNCH: ROS_INFO("RC_Call: GO HOME!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + rc_cmd = NAV_RETURN_TO_LAUNCH; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case mavros_msgs::CommandCode::NAV_WAYPOINT: + case NAV_WAYPOINT: ROS_INFO("RC_Call: GO TO!!!! "); buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); - rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; + rc_cmd = NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; -#ifdef MAVROSKINETIC - case mavros_msgs::CommandCode::DO_MOUNT_CONTROL: -#else - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: -#endif + case DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); -#ifdef MAVROSKINETIC - rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + rc_cmd = DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; @@ -1303,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; @@ -1483,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()); @@ -1495,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; } @@ -1519,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); @@ -1532,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); } From 5fc3276ac2bff7490bc829ffbbf25a39977c0621 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 7 Sep 2018 00:56:17 -0400 Subject: [PATCH 98/98] minor fix from sitl --- CMakeLists.txt | 77 + buzz_scripts/Update.log | 0 buzz_scripts/include/act/barrier.bzz | 90 + buzz_scripts/include/act/old_barrier.bzz | 69 + buzz_scripts/include/act/states.bzz | 311 ++++ buzz_scripts/include/plan/mapmatrix.bzz | 145 ++ buzz_scripts/include/plan/rrtstar.bzz | 525 ++++++ .../include/taskallocate/graphformGPS.bzz | 798 +++++++++ .../graphs/images/Graph_droneL.graph | 6 + .../graphs/images/Graph_droneO.graph | 6 + .../graphs/images/Graph_droneP.graph | 6 + .../graphs/images/Graph_droneY.graph | 6 + .../graphs/images/frame_01186.png | Bin 0 -> 68471 bytes .../graphs/images/frame_01207.png | Bin 0 -> 78891 bytes .../graphs/images/frame_01394.png | Bin 0 -> 76528 bytes .../graphs/images/frame_01424.png | Bin 0 -> 98463 bytes .../include/taskallocate/graphs/shapes_L.bzz | 59 + .../include/taskallocate/graphs/shapes_O.bzz | 59 + .../include/taskallocate/graphs/shapes_P.bzz | 59 + .../include/taskallocate/graphs/shapes_Y.bzz | 59 + .../taskallocate/graphs/shapes_square.bzz | 60 + .../include/taskallocate/waypointlist.csv | 89 + buzz_scripts/include/timesync.bzz | 28 + buzz_scripts/include/update.bzz | 10 + buzz_scripts/include/utils/conversions.bzz | 69 + buzz_scripts/include/utils/string.bzz | 92 + buzz_scripts/include/utils/vec2.bzz | 107 ++ buzz_scripts/include/vstigenv.bzz | 131 ++ buzz_scripts/main.bzz | 117 ++ buzz_scripts/mainRRT.bzz | 80 + buzz_scripts/minimal.bzz | 63 + buzz_scripts/stand_by.bzz | 38 + buzz_scripts/testLJ.bzz | 72 + buzz_scripts/testaloneWP.bzz | 57 + include/buzz_update.h | 171 ++ include/buzz_utility.h | 108 ++ include/buzzuav_closures.h | 183 ++ include/rosbuzz/mavrosCC.h | 24 + include/roscontroller.h | 309 ++++ launch/launch_config/solo.yaml | 12 + launch/launch_config/topics.yaml | 13 + launch/rosbuzz.launch | 25 + launch/rosbuzzd.launch | 25 + misc/cmdlinectr.sh | 44 + msg/neigh_pos.msg | 2 + package.xml | 61 + readme.md | 111 ++ src/buzz_update.cpp | 724 ++++++++ src/buzz_utility.cpp | 626 +++++++ src/buzzuav_closures.cpp | 859 ++++++++++ src/rosbuzz.cpp | 24 + src/roscontroller.cpp | 1517 +++++++++++++++++ 52 files changed, 8126 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 buzz_scripts/Update.log create mode 100644 buzz_scripts/include/act/barrier.bzz create mode 100644 buzz_scripts/include/act/old_barrier.bzz create mode 100644 buzz_scripts/include/act/states.bzz create mode 100644 buzz_scripts/include/plan/mapmatrix.bzz create mode 100644 buzz_scripts/include/plan/rrtstar.bzz create mode 100644 buzz_scripts/include/taskallocate/graphformGPS.bzz create mode 100644 buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph create mode 100644 buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph create mode 100644 buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph create mode 100644 buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph create mode 100644 buzz_scripts/include/taskallocate/graphs/images/frame_01186.png create mode 100644 buzz_scripts/include/taskallocate/graphs/images/frame_01207.png create mode 100644 buzz_scripts/include/taskallocate/graphs/images/frame_01394.png create mode 100644 buzz_scripts/include/taskallocate/graphs/images/frame_01424.png create mode 100644 buzz_scripts/include/taskallocate/graphs/shapes_L.bzz create mode 100644 buzz_scripts/include/taskallocate/graphs/shapes_O.bzz create mode 100644 buzz_scripts/include/taskallocate/graphs/shapes_P.bzz create mode 100644 buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz create mode 100644 buzz_scripts/include/taskallocate/graphs/shapes_square.bzz create mode 100644 buzz_scripts/include/taskallocate/waypointlist.csv create mode 100644 buzz_scripts/include/timesync.bzz create mode 100644 buzz_scripts/include/update.bzz create mode 100644 buzz_scripts/include/utils/conversions.bzz create mode 100644 buzz_scripts/include/utils/string.bzz create mode 100644 buzz_scripts/include/utils/vec2.bzz create mode 100644 buzz_scripts/include/vstigenv.bzz create mode 100644 buzz_scripts/main.bzz create mode 100644 buzz_scripts/mainRRT.bzz create mode 100644 buzz_scripts/minimal.bzz create mode 100644 buzz_scripts/stand_by.bzz create mode 100755 buzz_scripts/testLJ.bzz create mode 100644 buzz_scripts/testaloneWP.bzz create mode 100644 include/buzz_update.h create mode 100644 include/buzz_utility.h create mode 100644 include/buzzuav_closures.h create mode 100644 include/rosbuzz/mavrosCC.h create mode 100644 include/roscontroller.h create mode 100644 launch/launch_config/solo.yaml create mode 100644 launch/launch_config/topics.yaml create mode 100644 launch/rosbuzz.launch create mode 100644 launch/rosbuzzd.launch create mode 100644 misc/cmdlinectr.sh create mode 100644 msg/neigh_pos.msg create mode 100644 package.xml create mode 100644 readme.md create mode 100644 src/buzz_update.cpp create mode 100644 src/buzz_utility.cpp create mode 100644 src/buzzuav_closures.cpp create mode 100644 src/rosbuzz.cpp create mode 100644 src/roscontroller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..4d868f8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosbuzz) + +if(UNIX) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + mavros_msgs + sensor_msgs + nav_msgs + message_generation +) + +############################## +############################## + +add_message_files( + FILES + neigh_pos.msg +) + +generate_messages( +DEPENDENCIES +sensor_msgs +) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES xbee_ros_node + CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +include_directories( + include ${rosbuzz_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) +# set the path to the library folder +link_directories(/usr/local/lib) + +# Executables +add_executable(rosbuzz_node src/rosbuzz.cpp + src/roscontroller.cpp + src/buzz_utility.cpp + src/buzzuav_closures.cpp + src/buzz_update.cpp) +target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread) +add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp) + +# Executables and libraries for installation to do +install(TARGETS rosbuzz_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) + +find_package(catkin REQUIRED COMPONENTS roslaunch) +roslaunch_add_file_check(launch) diff --git a/buzz_scripts/Update.log b/buzz_scripts/Update.log new file mode 100644 index 0000000..e69de29 diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz new file mode 100644 index 0000000..27628b0 --- /dev/null +++ b/buzz_scripts/include/act/barrier.bzz @@ -0,0 +1,90 @@ +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_TIMEOUT = 200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil +bc = 0; + +# +# Sets a barrier +# +function barrier_create() { + # reset + timeW = 0 + # create barrier vstig + #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + # BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) +} + +function barrier_set(threshold, transf, resumef, bc) { + statef = function() { + barrier_wait(threshold, transf, resumef, bc); + } + BVMSTATE = "BARRIERWAIT" + barrier_create() +} + +# +# Make yourself ready +# +function barrier_ready(bc) { + #log("BARRIER READY -------") + barrier.put(id, bc) + barrier.put("d", 0) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf, resumef, bc) { + barrier.put(id, bc) + barrier.get(id) + var allgood = 0 + log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW) + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + allgood = barrier_allgood(barrier,bc) + } + + if(allgood) { + barrier.put("d", 1) + timeW = 0 + BVMSTATE = transf + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier = nil + timeW = 0 + BVMSTATE = resumef + } else if(timeW % 100 == 0 and bc > 0) + neighbors.broadcast("cmd", bc) + + timeW = timeW+1 +} + +barriergood = 1 + +# Barrer check all entries +function barrier_allgood(barrier, bc) { + barriergood = 1 + barrier.foreach( + function(key, value, robot){ + #log("VS entry : ", key, " ", value, " ", robot) + if(value != bc and key != "d"){ + barriergood = 0 + } + } + ) + return barriergood +} \ No newline at end of file diff --git a/buzz_scripts/include/act/old_barrier.bzz b/buzz_scripts/include/act/old_barrier.bzz new file mode 100644 index 0000000..ef3c5fc --- /dev/null +++ b/buzz_scripts/include/act/old_barrier.bzz @@ -0,0 +1,69 @@ +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_TIMEOUT = 1200 # in steps +BARRIER_VSTIG = 80 +timeW = 0 +barrier = nil + +# +# Sets a barrier +# +function barrier_create() { + # reset + timeW = 0 + # create barrier vstig + #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) +} + +function barrier_set(threshold, transf, resumef,bc) { + statef = function() { + barrier_wait(threshold, transf, resumef,bc); + } + BVMSTATE = "BARRIERWAIT" + barrier_create() +} + +# +# Make yourself ready +# +function barrier_ready(bc) { + #log("BARRIER READY -------") + barrier.put(id, 1) + barrier.put("d", 0) +} + +# +# Executes the barrier +# +function barrier_wait(threshold, transf, resumef,bc) { + barrier.put(id, 1) + + barrier.get(id) + log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { + barrier.put("d", 1) + timeW = 0 + BVMSTATE = transf + } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") + barrier=nil + timeW = 0 + BVMSTATE = resumef + } + + timeW = timeW+1 +} + diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz new file mode 100644 index 0000000..d657797 --- /dev/null +++ b/buzz_scripts/include/act/states.bzz @@ -0,0 +1,311 @@ +######################################## +# +# FLIGHT-RELATED FUNCTIONS +# +######################################## +include "utils/vec2.bzz" +include "act/barrier.bzz" +#include "act/old_barrier.bzz" +include "utils/conversions.bzz" + +TARGET_ALTITUDE = 15.0 # m. +BVMSTATE = "TURNEDOFF" +PICTURE_WAIT = 20 # steps + +GOTO_MAXVEL = 0.5 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 1.0 # m. +GOTOANG_TOL = 0.1 # rad. +path_it = 0 +graphid = 3 +pic_time = 0 +g_it = 0 + +function turnedoff() { + BVMSTATE = "TURNEDOFF" +} + +function idle() { + BVMSTATE = "IDLE" +} + + +# Custom function for the user. +function cusfun(){ + BVMSTATE="CUSFUN" + log("cusfun: yay!!!") + LOCAL_TARGET = math.vec2.new(5 ,0 ) + local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location + if(math.vec2.length(local_set_point) > GOTODIST_TOL){ + local_set_point = LimitSpeed(local_set_point, 1.0) + goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) + } + else{ + log("TARGET REACHED") + } +} + +function launch() { + BVMSTATE = "LAUNCH" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF + homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} + if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) + } else { + log("Altitude: ", pose.position.altitude) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } + } else { + barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) + barrier_ready(22) + } +} + +gohomeT=100 +function goinghome() { + BVMSTATE = "GOHOME" + if(gohomeT > 0) { # TODO: Make a real check if home is reached + gohome() + gohomeT = gohomeT - 1 + } else + BVMSTATE = AUTO_LAUNCH_STATE +} + +function stop() { + BVMSTATE = "STOP" + if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND + neighbors.broadcast("cmd", 21) + uav_land() + if(flight.status != 2 and flight.status != 3) { + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) + } + } else { + BVMSTATE = "TURNEDOFF" + #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) + #barrier_ready(21) + } +} + +function take_picture() { + BVMSTATE="PICTURE" + setgimbal(0.0, 0.0, -90.0, 20.0) + if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize + takepicture() + } else if(pic_time>=PICTURE_WAIT) { # wait for the picture + BVMSTATE="IDLE" + pic_time=0 + } + pic_time=pic_time+1 +} + +function goto_gps(transf) { + log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude) + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else { + m_navigation = LimitSpeed(m_navigation, 1.0) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) + } +} + +function follow() { + if(size(targets)>0) { + BVMSTATE = "FOLLOW" + attractor=math.vec2.newp(0,0) + foreach(targets, function(id, tab) { + force=(0.05)*(tab.range)^4 + attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) + }) + goto_abs(attractor.x, attractor.y, 0.0, 0.0) + } else { + log("No target in local table!") + BVMSTATE = "IDLE" + } +} + +# converge to centroid +function aggregate() { + BVMSTATE="AGGREGATE" + centroid = neighbors.reduce(function(rid, data, centroid) { + centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth)) + return centroid + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + 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) +} + +# circle all together around centroid +function pursuit() { + BVMSTATE="PURSUIT" + rd = 20.0 + pc = 3.2 + vd = 15.0 + r_vec = neighbors.reduce(function(rid, data, r_vec) { + r_vec = math.vec2.add(r_vec, math.vec2.newp(data.distance, data.azimuth)) + return r_vec + }, {.x=0, .y=0}) + if(neighbors.count() > 0) + r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) + r = math.vec2.length(r_vec) + var sqr = (r-rd)*(r-rd)+pc*pc*r*r + gamma = vd / math.sqrt(sqr) + dr = -gamma * (r-rd) + 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) +} + +# Lennard-Jones interaction magnitude +TARGET = 8.0 +EPSILON = 30.0 +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 +function formation() { + BVMSTATE="FORMATION" + # Calculate accumulator + accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) + accum_lj = LimitSpeed(accum_lj, 1.0/3.0) + goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) +} + +# listens to commands from the remote control (web, commandline, etc) +function rc_cmd_listen() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + BVMSTATE = "LAUNCH" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "TURNEDOFF" + #barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21) + #barrier_ready(21) + BVMSTATE = "STOP" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==20) { + flight.rc_cmd=0 + AUTO_LAUNCH_STATE = "IDLE" + barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20) + barrier_ready(20) + neighbors.broadcast("cmd", 20) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + BVMSTATE = "PATHPLAN" + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + disarm() + neighbors.broadcast("cmd", 401) + } else if (flight.rc_cmd==666){ + flight.rc_cmd=0 + stattab_send() + } else if (flight.rc_cmd==777){ + flight.rc_cmd=0 + reinit_time_sync() + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + barrier_ready(903) + neighbors.broadcast("cmd", 903) + } +} + +# listens to other fleet members broadcasting commands +function nei_cmd_listen() { + neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(BVMSTATE!="BARRIERWAIT") { + if(value==22) { + BVMSTATE = "LAUNCH" + }else if(value==20) { + AUTO_LAUNCH_STATE = "IDLE" + BVMSTATE = "GOHOME" + } else if(value==21) { + AUTO_LAUNCH_STATE = "TURNEDOFF" + BVMSTATE = "STOP" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { + arm() + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ + disarm() + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + reinit_time_sync() + #neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes + barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) + #barrier_ready(900) + neighbors.broadcast("cmd", 900) + } else if(value==901){ # Pursuit + destroyGraph() + barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901) + #barrier_ready(901) + neighbors.broadcast("cmd", 901) + } else if(value==902){ # Agreggate + destroyGraph() + barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902) + #barrier_ready(902) + neighbors.broadcast("cmd", 902) + } else if(value==903){ # Formation + destroyGraph() + barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903) + #barrier_ready(903) + neighbors.broadcast("cmd", 903) + } else if(value==16 and BVMSTATE=="IDLE"){ + # 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/plan/mapmatrix.bzz b/buzz_scripts/include/plan/mapmatrix.bzz new file mode 100644 index 0000000..c3d3393 --- /dev/null +++ b/buzz_scripts/include/plan/mapmatrix.bzz @@ -0,0 +1,145 @@ + +# Write to matrix +robot_marker = "X" + +# copy a full matrix row +function mat_copyrow(out,ro,in,ri){ + out[ro] = {} + var icr = 1 + while(icr <= size(in[ri])) { + out[ro][icr]=in[ri][icr] + icr = icr + 1 + } +} + +function getvec(t,row){ + return math.vec2.new(t[row][1],t[row][2]) +} + +function init_test_map(len){ + map = {} + var indexR = 1 + while(indexR <= len) { + map[indexR] = {} + var indexC = 1 + while(indexC <= len) { + map[indexR][indexC] = 1.0 + indexC = indexC + 1 + } + indexR = indexR + 1 + } + # DEBUG\TEST: puts an obstacle right in the middle + map[5][5] = 0.0 + map[6][5] = 0.0 + map[4][5] = 0.0 + + log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.") +} + +function init_map(len){ + map = {} + var indexR = 1 + while(indexR <= len) { + map[indexR] = {} + var indexC = 1 + while(indexC <= len) { + map[indexR][indexC] = 1.0 + indexC = indexC + 1 + } + indexR = indexR + 1 + } + log("Occupancy grid initialized (",size(map),"x",size(map[1]),").") +} + +function add_obstacle(pos, off, inc_trust) { + var xi = math.round(pos.x) + var yi = math.round(pos.y) + + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { + # log("Add obstacle in cell: ", xi, " ", yi) + var old=map[xi][yi] + if(old-inc_trust > 0.0) + map[xi][yi] = old-inc_trust + else + map[xi][yi] = 0.0 + } +} + +function remove_obstacle(pos, off, dec_trust) { + var xi = math.round(pos.x) + var yi = math.round(pos.y) + + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { + # log("Remove obstacle in cell: ", xi, " ", yi) + var old=map[xi][yi] + if(old + dec_trust < 1.0) #x,y + map[xi][yi] = old+dec_trust + else + map[xi][yi] = 1.0 + } +} + + +function get_occupied_cells(cur_cell){ + var i = 1 + var occupied_cells = {} + occupied_cells[0] = cur_cell + occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y) + occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y) + occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1) + occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1) + return occupied_cells +} + +function is_in(dictionary, x, y){ + var i = 0 + while(i < size(dictionary)){ + if(dictionary[i].x == x and dictionary[i].y == y){ + return 1 + } + i = i + 1 + } + return 0 +} + + +function table_print(t) { + foreach(t, function(key, value) { + log(key, " -> ", value) + }) +} + +function table_copy(t) { + var t2 = {} + foreach(t, function(key, value) { + t2[key] = value + }) + return t2 +} + +function print_pos(t) { + var ir=1 + while(ir <= size(t)) { + log(ir, ": ", t[ir][1], " ", t[ir][2]) + ir = ir + 1 + } +} + +function print_map(t) { + var ir=size(t) + log("Printing a ", size(t), " by ", size(t[1]), " map") + while(ir > 0) { + logst=string.concat("\t", string.tostring(ir), "\t:") + ic = size(t[ir]) + while(ic > 0) { + if(ir==cur_cell.x and ic==cur_cell.y) + logst = string.concat(logst, " XXXXXXXX") + else + logst = string.concat(logst, " ", string.tostring(t[ir][ic])) + ic = ic - 1 + } + log(logst) + ir = ir - 1 + } + export_map(map) +} diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz new file mode 100644 index 0000000..a55aff1 --- /dev/null +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -0,0 +1,525 @@ +##### +# RRT* Path Planing +# +# map table-based matrix +##### +include "plan/mapmatrix.bzz" + +RRT_TIMEOUT = 500 +RRT_RUNSTEP = 3 +PROX_SENSOR_THRESHOLD_M = 8.0 +GSCALE = {.x=1, .y=1} +map = nil +cur_cell = {} +nei_cell = {} +mapgoal = {} + + +# +# Work in progress.... +function navigate() { + BVMSTATE="NAVIGATE" + if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz + storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) + } + + print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y) + if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) { + log("Sorry this is too far.") + return + } + + # create the map + if(map==nil) { + dyn_init_map(cur_goal) + if(V_TYPE == 0) { + homegps.lat = pose.position.latitude + homegps.long = pose.position.longitude + } + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else if(path_it <= size(Path)) { + var cur_path_pt = convert_pt(getvec(Path, path_it)) + print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y) + if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, path_it, m_pos, 0)) { + # stop + goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0) + Path = nil + if(V_TYPE == 0) + cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + path_it = 1 + pathPlanner(cur_goal, m_pos) + } else { + cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt)) + goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0) + } + } else + path_it = path_it + 1 + } else { + Path = nil + BVMSTATE="IDLE" + } +} + +# create a map with its length to fit the goal (rel. pos.) and the current position as the center +# TODO: test with khepera/argos +function dyn_init_map(m_navigation) { + # create a map big enough for the goal + init_map(math.round(2*math.vec2.length(m_navigation))+10) + # center the robot on the grid + cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) +} + +# start the RRT* to reach the goal (rel. pos.) +# TODO: test with khepera/argos +function pathPlanner(m_goal, m_pos, kh4) { + # place the robot on the grid + cur_cell = getcell(m_pos) + # create the goal in the map grid + mapgoal = getcell(math.vec2.add(m_goal, m_pos)) + + #print_map(map) + export_map(map) + + # Initialize RRTStar var + log("--------Initialize RRTStar--------------") + HEIGHT = size(map) + WIDTH = size(map[1]) + RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive + goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0} + #table_print(goalBoundary) + numberOfPoints = 1 + arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance + Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=0,.4=1,.5=0} + goalReached = 0 + timeout = 0 + + # search for a path + old_statef = statef + rrtstar() +} + +# get the grid cell position (x,y) equivalent to the input position +# input should be relative to home position (planing start point) +function getcell(m_curpos) { + var cell = {} + # relative to center (start position) + cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0)) + var offset = math.vec2.new(m_curpos.x*GSCALE.x, m_curpos.y*GSCALE.y) + cell = math.vec2.add(cell, offset) + cell = math.vec2.new(math.round(cell.x), math.round(cell.y)) + + if(cell.x > size(map)) + cell.x = size(map) + if(cell.y > size(map[1])) + cell.y = size(map[1]) + if(cell.x < 1) + cell.x = 1 + if(cell.y < 1) + cell.y = 1 + + return cell +} + +function populateGrid(m_pos) { + # getproxobs(m_pos) + getneiobs (m_pos) + export_map(map) +} + +# TODO: populate the map with neighors as blobs instead ? +function getneiobs (m_curpos) { + cur_cell = getcell(m_curpos) + # add all neighbors as obstacles in the grid + neighbors.foreach(function(rid, data) { + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) + }) +} + +# old function tested with the khepera for reference +# DS 20/11 +# function getneiobs (m_curpos) { +# var foundobstacle = 0 +# cur_cell = getcell(m_curpos) +# var old_nei = table_copy(nei_cell) + +# neighbors.foreach(function(rid, data) { +# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) +# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) +# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) +# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} +# if(old_nei[rid]!=nil) { +# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# } else { +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# }) +# return foundobstacle +# } + +# populate a line in front of the sensor using plateform independant proximity sensing +# TODO: adapt to M100 stereo +function getproxobs (m_curpos) { + var updated_obstacle = 0 + cur_cell = getcell(m_curpos) + + reduce(proximity, + function(key, value, acc) { + if (value.angle != -1) { # down sensor of M100 + if(value.value < PROX_SENSOR_THRESHOLD_M) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(value.value, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl2 = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + # obs2 = getcell(math.vec2.newp(math.vec2.length(obs) + 1.0, math.vec2.angle(obs))) + # obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) + # obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) + if(map[math.round(obs.x)][math.round(obs.y)]!=0) { + add_obstacle(obs, 0, 0.25) + add_obstacle(obsr, 0, 0.25) + add_obstacle(obsl, 0, 0.25) + add_obstacle(obsr2, 0, 0.25) + add_obstacle(obsl2, 0, 0.25) + # add_obstacle(obs2, 0, 0.25) + # add_obstacle(obsr2, 0, 0.25) + # add_obstacle(obsl2, 0, 0.25) + updated_obstacle = 1 + } + } else { + var line_length = PROX_SENSOR_THRESHOLD_M; + while(line_length > 0) { + obs = getcell(math.vec2.add(m_curpos, math.vec2.newp(line_length, value.angle + pose.orientation.yaw))) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsr = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsl = math.vec2.add(math.vec2.newp(2.25, math.vec2.angle(per) - math.pi/2.0), obs) + remove_obstacle(obs, 0, 0.05) + remove_obstacle(obsr, 0, 0.05) + remove_obstacle(obsl, 0, 0.05) + line_length = line_length - 1; + } + } + } + return acc + }, math.vec2.new(0, 0)) + + return updated_obstacle +} + +# check if any obstacle blocks the way +# TODO: remove the kh4 bool for retro-compatilibty +function check_path(m_path, goal_l, m_curpos, kh4) { + var pathisblocked = 0 + var nb = goal_l + cur_cell = getcell(m_curpos) + var Cvec = cur_cell + while(nb <= size(m_path) and nb <= goal_l+1) { + var Nvec = getvec(m_path, nb) + if(kh4 == 0) { + Nvec = vec_from_gps(Nvec.x,Nvec.y,1) + Nvec = getcell(Nvec) + } + if(doesItIntersect(Cvec, Nvec)) { + log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + pathisblocked = 1 + } + Cvec = Nvec + nb = nb + 1 + } + + return pathisblocked +} + +## +# main search loop as a state +## +function rrtstar() { + # update state machine variables + BVMSTATE = "PATHPLAN" + + + step_timeout = 0 + while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { + + # Point generation only as grid cell centers + pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) + #log("Point generated: ", pt.x, " ", pt.y) + + var pointList = findPointsInRadius(pt,Q,RADIUS); + + # Find connection that provides the least cost to come + nbCount = 0; + minCounted = 999; + minCounter = 0; + + if(size(pointList) != 0) { + #log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3]) + ipt=1 + while(ipt <= size(pointList)) { + pointNumber = {} + mat_copyrow(pointNumber,1,pointList,ipt) + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(pointNumber,1)); + #log("intersects1: ", intersects) + + # If there is no intersection we need consider its connection + nbCount = nbCount + 1; + if(intersects != 1) { + #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") + var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][4]][5] + + if(distance < minCounted) { + minCounted = distance; + minCounter = nbCount; + } + } + ipt = ipt + 1 + } + if(minCounter > 0) { + numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} + arrayOfPoints[numberOfPoints][1]=pt.x + arrayOfPoints[numberOfPoints][2]=pt.y + + Q[numberOfPoints] = {} + Q[numberOfPoints][1] = pt.x + Q[numberOfPoints][2] = pt.y + Q[numberOfPoints][3] = pointList[minCounter][4] + Q[numberOfPoints][4] = numberOfPoints + Q[numberOfPoints][5] = minCounted + + #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y) + + # Now check to see if any of the other points can be redirected + nbCount = 0; + ipt = 1 + while(ipt <= size(pointList)) { + pointNumber = {} + mat_copyrow(pointNumber,1,pointList,ipt) + #log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4]) + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(pointNumber,1)); + #log("intersects2: ", intersects) + + # If there is no intersection we need consider its connection + nbCount = nbCount + 1; + if(intersects != 1) { + # If the alternative path is shorter than change it + tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt)) + #log("Q: ", size(Q), "x", size(Q[1])) + if(tmpdistance < Q[pointNumber[1][4]][5]) { + Q[pointNumber[1][4]][3] = numberOfPoints + Q[pointNumber[1][4]][5] = tmpdistance + } + } + ipt = ipt + 1 + } + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } else { + # Associate with the closest point + pointNum = findClosestPoint(pt,arrayOfPoints); + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum)); + #log("intersects3 (", pointNum, "): ", intersects) + + # If there is no intersection we need to add to the tree + if(intersects != 1) { + numberOfPoints = numberOfPoints + 1; + arrayOfPoints[numberOfPoints] = {} + arrayOfPoints[numberOfPoints][1]=pt.x + arrayOfPoints[numberOfPoints][2]=pt.y + + Q[numberOfPoints] = {} + Q[numberOfPoints][1] = pt.x + Q[numberOfPoints][2] = pt.y + Q[numberOfPoints][3] = pointNum + Q[numberOfPoints][4] = numberOfPoints + Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt)) + + #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y) + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } + if(numberOfPoints % 100 == 0) { + log(numberOfPoints, " points processed. Still looking for goal."); + } + timeout = timeout + 1 + step_timeout = step_timeout + 1 + } + + if(goalReached){ + log("Goal found(",numberOfPoints,")!") + Path = convert_path(getPath(Q,numberOfPoints)) + # done. resume the state machine + BVMSTATE = "NAVIGATE" + } else if(timeout >= RRT_TIMEOUT) { + log("FAILED TO FIND A PATH!!!") + Path = nil + # done. resume the state machine + BVMSTATE = "NAVIGATE" + } +} + +# Go through each points and find the distances between them and the +# target point +function findClosestPoint(point,aPt) { + var distance = 999 + var pointNb = -1 + var ifcp=1 + while(ifcp <= size(aPt)) { + var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + + if(range < distance) { + distance = range; + pointNb = ifcp; + } + ifcp = ifcp + 1 + } + return pointNb +} + +# Find the closest point in the tree +function findPointsInRadius(point,q,r) { + var ptList = {} + var counted = 0; + var iir = 1 + while(iir <= size(q)) { + var tmpvv = getvec(q,iir) + #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) + var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + + if(distance < r) { + counted = counted+1; + mat_copyrow(ptList,counted,q,iir) + } + + iir = iir + 1 + } + return ptList +} + +# check if the line between 2 point intersect an obstacle +function doesItIntersect(point,vector) { + #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) + var dif = math.vec2.sub(point,vector) + var distance = math.vec2.length(dif) + if(distance == 0.0){ + # Find what block we're in right now + var xi = math.round(point.x) #+1 + var yi = math.round(point.y) #+1 + if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) { + if(map[xi][yi] > 0.5) + return 1 + else + return 0 + } else + return 0 + } + var vec = math.vec2.scale(dif,1.0/distance) + var pathstep = size(map) - 2 + + idii = 1.0 + while(idii <= pathstep) { + var range = distance*idii/pathstep + var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + + # Find what block we're in right now + var xi = math.round(pos_chk.x) #+1 + var yi = math.round(pos_chk.y) #+1 + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")") + + # TODO: this check if the pos_chk is under the robot, but we need to check the whole line for a cross + if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){ + #if(xi!=cur_cell.x and yi!=cur_cell.y) { + if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { + if(map[xi][yi] < 0.5) { # because obstacle use trust values + #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") + return 1; + } + } else { + #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y) + return 1; + } + } + idii = idii + 1.0 + } + #log("No intersect!") + return 0 +} + +# create a table with only the path's points in order +function getPath(Q,nb){ + var pathL={} + var npt=0 + # get the path from the point list + while(nb > 0) { + npt=npt+1 + pathL[npt] = {} + pathL[npt][1]=Q[nb][1] + pathL[npt][2]=Q[nb][2] + if( nb > 1) { + if(nb == Q[Q[nb][3]][3]) {# TODO: why is this happening? + log("ERROR - Recursive path !!!") + return nil + } + } + nb = Q[nb][3]; + } + + # Re-Order the list. + var pathR={} + var totpt = npt + 1 + while(npt > 0){ + pathR[totpt-npt] = {} + var tmpgoal = getvec(pathL,npt) + pathR[totpt-npt][1]=tmpgoal.x + pathR[totpt-npt][2]=tmpgoal.y + npt = npt - 1 + } + #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) + return pathR +} diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz new file mode 100644 index 0000000..dc6527e --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -0,0 +1,798 @@ +# +# Include files +# + +include "taskallocate/graphs/shapes_P.bzz" +include "taskallocate/graphs/shapes_O.bzz" +include "taskallocate/graphs/shapes_L.bzz" +include "taskallocate/graphs/shapes_Y.bzz" + +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 + +# +# Global variables +# + +# +# Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLabel={}#store received neighbour message +m_MessageReqLabel={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCount=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + +# +#Save the message to send +#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + +#navigation vector +m_navigation={.x=0,.y=0} + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 +m_messageID={} +repeat_assign=0 +assign_label=-1 +assign_id=-1 +m_gotjoinedparent = 0 +#neighbor distance to lock the current pattern +lock_neighbor_id={} +lock_neighbor_dis={} + +#Label request id +m_unRequestId=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +# virtual stigmergy for the LOCK barrier. +m_lockstig = 1 + +# Lennard-Jones parameters, may need change +EPSILON_GRAPH = 4000 #13.5 the LJ parameter for other robots + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i=0){ + pon=0 + } + else{ + pon=1 + } + var b=math.abs(send_table.Bearing) + send_value=r_id*1000+pon*100+b + return send_value +} +# +#unpack message +# +function unpackmessage(recv_value){ + var wan=(recv_value-recv_value%100000)/100000 + recv_value=recv_value-wan*100000 + var qian=(recv_value-recv_value%10000)/10000 + recv_value=recv_value-qian*10000 + var bai=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-bai*1000 + var shi=(recv_value-recv_value%10)/10 + recv_value=recv_value-shi*10 + var ge=recv_value + var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0} + return_table.State=wan + return_table.Label=qian + return_table.ReqLabel=bai + return_table.ReqID=shi + return_table.Response=ge + return return_table +} +# +#unpack guide message +# +function unpack_guide_msg(recv_value){ +#log(id,"I pass value=",recv_value) + var qian=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-qian*1000 + var bai=(recv_value-recv_value%100)/100 + recv_value=recv_value-bai*100 + var b=recv_value + var return_table={.Label=0.0,.Bearing=0.0} + return_table.Label=qian + if(bai==1){ + b=b*-1.0 + } + return_table.Bearing=b + return return_table +} +# +#get the target distance to neighbr nei_id +# +function target4label(nei_id){ + var return_val="miss" + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} +# +#Transistion to state free +# +function TransitionToFree(){ + BVMSTATE="GRAPH_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=s2i(BVMSTATE) +} +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + BVMSTATE="GRAPH_ASKING" + m_nLabel=un_label + m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.ReqLabel=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} +# +#Transistion to state joining +# +function TransitionToJoining(){ + BVMSTATE="GRAPH_JOINING" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod +} +# +#Transistion to state joined +# +function TransitionToJoined(){ + BVMSTATE="GRAPH_JOINED" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + + #write statues + #v_tag.put(m_nLabel, m_lockstig) + barrier_create() + barrier_ready(900+50) + + m_navigation.x=0.0 + m_navigation.y=0.0 + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) +} +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + BVMSTATE="GRAPH_LOCK" + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + #record neighbor distance + lock_neighbor_id={} + lock_neighbor_dis={} + var i=0 + while(i0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLabels={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLabel) + return + } + + #set message + m_selfMessage.State=s2i(BVMSTATE) + + BroadcastGraph() +} +# +#Do asking +# +function DoAsking(){ + UpdateGraph() + #look for response from predecessor + var i=0 + var psResponse=-1 + while(im_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + if(repeat_assign==0){ + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + assign_label=ReqLabel + assign_id=ReqID + repeat_assign=1 + } + m_selfMessage.ReqLabel=assign_label + m_selfMessage.ReqID=assign_id + m_selfMessage.Response=r2i("REQ_GRANTED") + #m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",assign_label) + log("ID=",assign_id) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + } + + #lost pred, wait for some time and transit to free + if(seenPred==0){ + m_unWaitCount=m_unWaitCount-1 + if(m_unWaitCount==0){ + TransitionToFree() + return + } + } + barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941) + BroadcastGraph() +} +# +#Do Lock +# +timeout_graph = 40 +function DoLock() { + UpdateGraph() + m_selfMessage.State=s2i(BVMSTATE) + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 + m_navigation.y=0.0 + #calculate motion vection + if(m_nLabel==0){ + m_navigation.x=0.0 #change value so that robot 0 will move + m_navigation.y=0.0 + } + if(m_nLabel!=0){ + m_navigation=motion_vector() + } +# #move + +# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0) + BroadcastGraph() + +# if(loop) { +# if(timeout_graph==0) { +# if(graphid < 3) +# graphid = graphid + 1 +# else +# graphid = 0 +# timeout_graph = 40 +# BVMSTATE="TASK_ALLOCATE" +# } +# timeout_graph = timeout_graph - 1 +# } +} + +# +# Executed at init +# +function initGraph() { + # + # Global parameters for graph formation + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=100 + m_fTargetAngleTolerance=0.1 + m_unJoiningLostPeriod=100 +} +# +# Executed every step (main loop) +# +function UpdateGraph() { + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +} + +function BroadcastGraph() { + #navigation + #broadcast message + neighbors.broadcast("m",packmessage(m_selfMessage)) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLabel={}#store received neighbour message + m_MessageReqLabel={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCount=0 + +} +# +# Executed when reset +# +function resetGraph(){ + BVMSTATE="GRAPH_FREE" + m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + m_navigation={.x=0,.y=0} + m_nLabel=-1 + m_messageID={} + lock_neighbor_id={} + lock_neighbor_dis={} + m_unRequestId=0 + m_bias=0 + m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + m_unWaitCount=0 + repeat_assign=0 + assign_label=-1 + assign_id=-1 + m_gotjoinedparent = 0 + + if(graphid==0){ + log("Loading P graph") + Read_GraphP() + } else if(graphid==1) { + log("Loading O graph") + Read_GraphO() + } else if(graphid==2) { + log("Loading L graph") + Read_GraphL() + } else if(graphid==3) { + log("Loading Y graph") + Read_GraphY() + } + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==ROOT_ID){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} +# +# Executed upon destroy +# +function destroyGraph() { + #clear neighbour message + m_navigation.x=0.0 + m_navigation.y=0.0 + goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("m") +} diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph new file mode 100644 index 0000000..6747976 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneL.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 1 200.0 0.0 9000.0 +4 2 200.0 1.57 11000.0 +5 4 200.0 1.57 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph new file mode 100644 index 0000000..3724ba7 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneO.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 1 200.0 1.57 9000.0 +4 1 141.0 3.93 11000.0 +5 2 141.0 0.785 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph new file mode 100644 index 0000000..52a8d6a --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneP.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 0.0 5000.0 +2 0 200.0 1.57 7000.0 +3 0 200.0 4.71 9000.0 +4 1 141.0 0.79 11000.0 +5 2 200.0 0.0 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph new file mode 100644 index 0000000..0aff949 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/images/Graph_droneY.graph @@ -0,0 +1,6 @@ +0 -1 -1 -1 3000.0 +1 0 200.0 4.71 5000.0 +2 0 141.0 2.36 7000.0 +3 0 141.0 0.79 9000.0 +4 2 141.0 2.36 11000.0 +5 3 141.0 0.79 11000.0 \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01186.png new file mode 100644 index 0000000000000000000000000000000000000000..3e01a88f58e6be5e0b014d1f5dab1107e5d387d8 GIT binary patch literal 68471 zcmXtgc|4Tw_x{)lBl{Mzj5UT3+1EjKl5K1uLMddtX~>o>k}V@LWZ$>3#7M~!5{4nW z?0X_)N&IeozP~@DW?ntdJmz?PP#y2lfb5KJdkV|^HT4oRkX%F~er6dKf zh^j@agI{F6ntJAxl$4WmCbQt5RNlHsUkHS@wGJ2(;c2~VD7>kN_ns#bQ|PgeR>>Z4lfJC%oN+I2 zQ1Z?4iMb-yQlve=^A-|gQDSaLTvTPoxL%^6@#dk1wB~d#Js$VOP+!kah2W==Q%}bwetqS=Z7)kcHMN| zp%g1VOZq8F@!O|J8r>$!*j^1^Bt3C#1CBa$yXZx{@`TNRSCo_X^B4A3)_Futs+xs4%~AmC(VeJ>($2>*x+MhsGXiYYU-jBCQ&uj z(O@(N%kNxM5fgB$%7|D#a^Lz}n^TP-X~y%*tDKQDBZ-dVpR)LmYh*uq?{c&9Kiusr zllyYmJg>%VoKh*#+tPgV8C8&uU;?b+I@mYIw|N6Z4)Liquv}?UZ;UD{M?4>WC|jKN ztWDx8w$R;us+ye9aed1Uy9bQpp5JigL8>NoB#l}U)1pHP*595BcoIPbzluZDiyQe8 zBg%UABSc?0u$jO&v{&SeZjbsI5#hZs)aA~1avUPd?6i>y1w;?wvsZCcr7%UfugXs<2{%DR&o+eTJ?m6>aDt$bZfxY`71!%QDXmlj6q9 zDW~}ot6xveSYxpL3ALmU>2yO-yGUi8UtIgW_52M>I6hF6flgzZ4_)tl!Aa~+%EGC} zJLEcz8@PUVp1_lGnTtPne1jvj#nltH=mqxhh0h(dV>np)XkuUn+NkKpJ4%=1dgk%s zhy>Sm5+k#e)~^T?_Ln3G%@^#O>3Jp)(+Bl7pm?VB#^wgB(MDQA6J`ALUtMb?^R_W; zlcZT3`?I~q9wzi~h{RiW)eoY+Mhh|$6>ald=Wm#hO=*de07^vuSRgCKA|l|kZGv4F zL7jN-F-aqNFXL^go3jzTZ=OD}$ey4Iz3C6#>1C5iu1+m4FWjPEj$2hHUiL1JYWuF3 z;^6imcuoL3WwK27=%Dp=nRuO5(``SoebKh1Z?*OPN$hiq%&2urMHNL+zIS_Y28=~b z0(iQ^hSq}S;D>e$K7xi#|H+v>^NWt{gjs6IJ7_dP0-^~U*4CD+1xl}am%=gSHlMjt zavWvWR{iuNwzAZ)-z916M@d4z#;&R}O(^nRtbctM>E>l_R77+^y!FT!VzA8@5n>uZ z++f~}xO#0}R<}&)l_SzURdMYShlXoOccA64Xi!JYgEDztYtcnr|?hA(3y; zjU`Oq&DlNMeNvYPk&kmjS~5si%QMF>$2D)qu~XfiO@elt3hXK7_{&lnV;urdg4|_P zTW)D~E^&%?T=l~>QJn4hn0MX(G9=FzA@4UkVM73zqGNC*750Zfig<_6oOxiO+`4u<%&3q zK1TGa&}6283^T#-&FVOpXa99uQX^;U6IMm$@ZVchq$HZzfk(6yPCbN4c3Tj*p`#yT zu+x$hYYJD!R`g-^IpGHhK-e+=0jR0HEeFxkQur4=-+y!nFDdwO?%IkD`<_e z=7n>{^k*9v`J}mnM6KU7xWs@mO?kjVB2|)7ooSj9>mcgs*06cEDT~gGL|(Ug({Uie zVWqxn;6K$~Znc7@qqxAx`73!_5)O+?YXtWH+rKyb# zMdn*BCHV%}`TU)w+)<6)iMeHHvbvqgZPG7hR7TxBGb;3$L@Eg_>!10y$NW?wifRWD zCkzcBu{*+-q1mdKA2Q}l8q+zeq-hPW7bKbaK+yD*yDs$}mr<a9 z*{JP6!J{O?#tBKlTUCrWTMY4vz9mDQQx|UU^omY>sG-oaR!uvoa(@YNnRVhy?=fiG zYlTTGiS)W{VvZm+0p(Q-$!=2HLSVl>v9_Lz!;J_~MO?sRcRkhSI= z)U3v-kmC`!a+n!$BNR3?pY2Y=po^itRMObWVy$jwSm@bUU0y4(3&N7LAOjiAP zUpqv5LZpRGhaZHz)q61`)1l%Bq6w~Jjz<)>duIP4&A9O&x2lrgV@;hDj^3eXU#)S! za&{NoYUG*Qx40iZ+Y^s^_nNB>X6$-xy?%W73c$tze^-0CYK{f0)+1;OM->b#_tDc9 z{xJnEBht$e4j0WTU^1*7IHo|aSF}CCv&IJ9@T`JXoXlGC6?ah@RB(_xgMjcCIq3KX zD}4e+pxM^ZK|Av)ocIj=ka%T&WSP9gOjs(XOgCuN4}PBn<2rW+Lxf8pwbQw&T3$8; zRKir@AR_Dgiv8qu^!n=#B#1vki{2oa$%CoJ{m|xR77=WFrq!>?PmEBS%aN|pq_#9r z2;D>%?0G1BC?IhAhlhsU?V^viiK2JZ>8JVrtA|YdI(iRB@Lg>&*78rlI&_iB3`*X0 zsYPnv0*}iqFjwdDle>nmTXR#65{k{P#!(8(!3d}eT)PpDz(6KY3?TO+vDO;xUNKtoVai0M`u-~=4{Rb}g zMiuUY!NPvj=M7!gpt{wL7Yc6(&l!N(y&oKY&@TBN6-~NdllhGbk~v*#hX@-rCdvev z?~5WM6AKtK6c^6mnudyS18+X`jf=KgNKxs&d@(Z6p1k}MrqE&fmV3vVLOP{QsR$41 zg;l;%&ef4Xa&=f}_2i9KO$Dr3<^2xcqw_}Yk8n*5IbTB_y+*y`mO0pZUTWJa z2~a^|fo6(ttX9vT$Eb)emZ7kgkM1 zOmH1vJ1H%-!9+gp84MGWT6=vzsFpx*f(BW4T~a{7C?~pbm-`*pDtS8I($IH{#|&P9 zy}NVXJXC_mHeb1m@aBv*J+ZVH*;k05(320$i^y5hABc6v5jJA#c`*Cekb^bj-`F4o znF@!hveGYUC@P$w%dFye#osnu*^%kAVz>txjxr z!2S9NYoD2hI?dmUw3}ARK=oMToL?2dsvRQeGv8Wh4YdU3)_BGwrhJ?{td@Uwy$j1Dv~Gr?U>mNr zqq?k&Dd}d4(!wKpM8jDrLob>^P0uF^l%f$TI^a?aAxF0w9`R$VW^;%C?6b6=yTF{x z+9Q=&{i-~pDO0Vrt)o31AGv5J_R`WJsQ%s>-`+%+dDWBovFY6oR_hyvm%d$zg&7FG zWaooI-YFy~tnd9QH%-1=6xJ|5nBFq9pBMDTU$?@p&`?JpH(q^Lp#B8sYFKf zl??3`bo30x)@_{g_XT0*P~kF4ABjgI-G=vm#!uWIZCG=Xy+iWlsbwflr^~f}Z5;93 z74(^oXO|w#``C|e>?@wW2`o+X`I!|1?_JwJbXvANGh`pLCq^%1nn-D7|XL-EO)HN!-{usyfi zEQ!K`e2q~tm3?kWA(zL`(XlmH*P^B`eU-NJ>{vmZ3*vRvz!k~5o;?(#C_&^0mmP`7 zW&QA|sT@}MZ&)lF`T7ZuuyZMin*9bQnwQA#8!9yI>N~gHT2BSVK8G$SGuQb^V=P#r zHBvY~+57A2KqTz7BLw}T5({8tlOrP(cF^RZCFpAC7WW@+wc#sY?$sWuYrecm^V+-? zcI;y#Ox*BfJ-)D0w7XFE>DgXoD?vTw#*_5>*k`Uj=;BUi1}^a2tzB1#6AV9FXwAe@ z?tIn8#G-T*=|d+C*SE5KKWM)yXfwVXTkczB4w7~r>5lyxBBDt+>Jd}dyNgYGZrsiF zlzN{lJ2r+5d&kcq=a9Vbk)AerypTQa`Yu+tO?HBod{$iL91X-7W96CH4J^eE4*BvW zpS)}3%Ndn@DAF7#9+-zRcX1v==e>^Hp4ho$fsdehc#t9d9|iS6`a~r}IifR&#mB6Q zGhU~_Os~K!QZwwg;oTT-ocTk#)^Q_NxPcdmEKS~^`4pWw?$;T04uXUVbLG5d&0o$& zc;*LBdW5Z>Ty?5~P*8!=^cgqR9H`z$eG=o>DIYQdcO z9wB4If7E^X{R1AT%^Izfc0K~q_)%|qy7m(#1{AtogW|13yRw+!Jw>#Na|$Ifp>i14tH`8QPn}aO2PSs3i1G$&J4Gwv&1$Uf>hPL&3*ePT?z5~&HN`gA zsXJ5d#XqEJMKnf^tw1Na)$B+8?ENpjj$)YGGts8e)~wZ}g97aId`Ff88GR zKm=R#mIH4JLoYRcf7L#WU4frqZZmjPQMmZ%)%M9cYhpRSd zC2KIEGa}xvrm&gsPnit#KYC)zs30dPg)7fsq@R#FJKace3)gk52!G(Gk%nyN>?pso#({MxV9 zZ#R=})3f@bu^ZeU@7Kq^P0?Mq6z z-mcJ|CUw4CmtgB{tKGI=!yLUuf2x!-su4K2k*Q8}NhxTv?*PPc^OmV*6(X)(blwIh z-Ga0X=mFeIi8<^}hv003UN_gjVs}x$dlMC0P!jUxmoY}21OZRKZ#V{XL2(N)hu&S` zLl-^Aa4H9Uo;d>3TybSJSAq(m$T=&B`3TK`eXV9IjHwu_~Doflor2px+WFO=PE6- zNK%q{>t0PMfWeBJqep4ECfUcs3gIa7PVwrCIy=_10F`eTLCa~oIT5W@W?OCb|cjKOFn~~LYP7Kamvlk*| zr~Tfra*O*3t%nT9KKe>gCcsTpnk#8id^gFx#VZdahc?FkSYN%Lz_U)fwEM*ELkzbs zO;vV6PjhT_<|eg_{R2k*+h;Ow`j&T}eE1*mX&)X3pDIUyh(`<--OOC`Q+=dy1I}5~ zyrK>EX_-xh^VllVDcIGdH&p5VPlq@j1(!X%$W!=y%66R*NeAU~h`f!`;0EB|JmVw);wOSA}VoO zs)=ScLFkcqN;pLRs>*nlunp`v24^n+h1Yf+a0eZ)?m;ujf6~d^_QaV3<{&4+;ilo9 zs3s~JWEr_$<8K{3C>I#)W29R8zxz%)j)P(&Q#B(3nN{rbFTpT>i|*GH|8_w_L+O05 zCsV=}MsJzA*M~M-oYlZ~g@~-K%~dHaQp@BR`9*_G#GwJ{9u!KY0@Y1i3^r%zUdc9x z1hlGl3+Zs~Ond9U+0qshDY*X9sJjEk%4S`oP5AraH1LGL*`VM$cqN(H?{fug{$m&2 zooiY81=8s>)1(QiDvSD9Ldl-u=uC0tMyh^ip;Ep)O+xJe{bi>XyP?C5>jx|JncS}> z#sc$NB~?}_QVqV=bxQJGN7L?%7yHGjKZPqYyxlStN|~!vdQ}c{_tC`yg3R2)sK5*) znio_VAbIpMXnyYg2@aw`=jhf=BJDOiIxmcLO9YKg=&WvDIWIoYeUl{aPp4n9FyB5? zDHLn>Xjc3bVXS*t<_4y&7Z$7DfCix_^Ts5z95tq( zkzSo>iPQXb$?W6Zv1wmf+(>AZ?I38+ip*#41pujMNF0{AWMC5T_oQSL!BqG0k`jc@ zDI5SqNG4DT%3I%sPN1~fNQ@>TNWIs-Kmb{>sH5{)5X|*U&u_0gOW%1n%s%@FWVGJQ4`QG?F>&sqSVa86b{1C@@-uBJRC!m4b9-g+!b z^SO|LAWCHMi>Bsiq4%^3%bP0#s&A6Ki7yX!wCEHYN{C(Jx~bPfNkXGRaXFJ^c%@3t}(C>@_hQy|_$66FQbVN9$f!NLw$f z^vW{F2v_fFzetj#_|T|<$K(?%v|#W!vbckfM*bqX@KixFe@b#T(17BF!PcR?&v%_X zp_a_3Gu~D3Wh|6Y$VIQFe8cpR%%~7oyZTn)n0Z}cnT5e>T{c(Cy@c}@^okm>E4i%)B z1;kZd>(00K?xt?Do;HS>CJL(BL{qrIPx=NG6h1h%pSyo8%n#aun1c2&3cA$&ZpEFp z+?vQ#PdRxWjCRUqFOQ2yiT4B)8uP5zujME)+ha=Y0k;|#;y<9*QfXNRab&;nBfY?7 z$qU@-HP8`Nz=X%!{5aE1^n9$W8~k598do5rzr$ROud{4O(JpE}v6yxxOSjp^ucf zS~ZlM$La5r>!@a11wa>$FLHrGBY(Ti`Cj<9|L+CZ;?8pmsfRert2}P9%Nqoe zdPH8|gGVu|TN-Yg(|2X0#YitvDD_gO=$x~3^i&G!)1-@b+VaN{pQ>g)k-J6xxH}$v zjnDo+UdTHnrz5gTC0JvG6zDsNS|zu2;7T3SE&rpmCe!1 zcetp@Wu5~tV1E6}$}UK2O#v56Pe2mB95pZe*AD%nxA&`W)C!XR_m$oDWei{xAF?on zxl+v!`(f)^fo36`?9HJf7NBhqXj~1a?AHW(hB4mN^sOG}liy3mc*APA)ZnyA+q|%D z_$`e74nd9L(|~c9epJ{7D^|VA*D~8M79DR8lPjX-A7izzrwExz-&3?&_sbjfUZ-us zl!qH9@IkzLMB98mj2A|K1g#kp&{(+byO$&=HJOb>`-^wq_DESHGr7i`)m9cRY8Cp)^H@-K>|0N`v2p+vM+Y3Di!GVwI^8&=IgX}UmD!!Q6ey6SSm z7Wa5B5$Dn=Vvu_O1F88GZ&z%|w;^%ek?Cj4ri9)2mh*Xq&HTj+6HV@5PliV6rd!@mm7t}c_1M5xqCI+0i{jf*{*$z*${?8gumS=i(%W( zuoqXi7(urf_s?wV}w(YJ6G1qu7h>SplXQSlbB#84Vmfx+a6=szD9^}D4$;XX+_65fuFkV>+-)gs%f z$%65SA$v6+*HG&vC@5fNbsaVaZAAoE=92fn_ zNX!ekr=UR7@OCmi=Z`gzf%Lo|v!q1-J+7GqAQp6*zW2Wmkw(*eTP7n?hFI~6zUp!& z_l&PN^Fj|yuPeiG(L<9e=;9OhJG;y;`xBhf^aW?X3_ePT_}Y1R7s{c>OWLY56(?Ch z&Ybd=iOWEFQ)pil#?T_6-Qp9?;4WJ6`w$oi2;=|bRCD>%1>SLkQnJLY7B1O%5iSAk zjBx0Bl*{xuZ;*FW_jhtClRN3iut5v+cL7S*<} zFPcc$xX>KQZe3?K)7wJH&XGBWWE->5qg356%Aulk#Op5@Tm=A|*^Clx>9dNx@@;2WFq z98tiVwZ4xxx)}kwHBSY~72;!kgLYb_-lpQ%D*M$ZocS7xW0!(bM5R;G@A!2iX3?9( zTR7SARe`ioEiiu7A{p?AcG^}lBxe;2uAcIe^}+DCXM;8Tm$O~&{~sQG2@zj4SA-uz ze{r=LoSCmyq&Xrf$EI&fwE#L>^?9iBw$JxIsm+q>#Z=fYyf4@_wn83PyY}e&uznrM{TsHqqxCE7UjZ166(~6<8vv|HEKKesL)9#O7MNc zJ<77pD!Gs<9giUJDfe11Fi0`y`{sCLeB4TW(DP;GVzOFKs$xOjDAoEFNocMJ<*cW; zcNIuBZZ(6~L$2q98@}1?+5QVbvCcRB2_2O0{|n;bUjk@L0ps_SImf{AKA~(915u9M z4WNw_+s6;OyJsvHC^6r{l*R)fGyo?429@A*N1d$IG~ZdWg~2&mdF*%4?Mr!&rVZl= z(o8M3`%R(}SMQLyo%iNO%~Si=!VNAgIVxRFEKX?!n$tOkLi}-Crs1v+`w`?*V74D; z(%qod&iEcLKO_?-T!AK%m9QAE>`{@ZF7B{mLj>Jttn}hHZ0TnhN=IxL^#X+OU%@f zBD!rJn$fw${GArKo#rFGm9Ca0O8C%Pok@=pw0+ z=Pt7CrOP$sRqnu(h;>@3NaR0n5;i1O!XWZV)=x8W{&KJ0E$(L0$cH|F0g2S)`|&6I zWugj03<-p8-EL}sVQKuof?NLGEZ2y-uwuAHG{jqHK+`RfcbczV($ke{lpXxBJ#xjl zBql;~&5~iQ8a_=*XM_8dR)EW>g2>(_PCxL_bfeHsjm&)K04iO}2Lka~mi=ijO2CuP z;6vybJ-_>|6_nsNU82;XjVTpBhp%ca{dz6m{95cJCQ;FVvsYzBywo+|42LuCS^#Go zzH-6nO0mn*&ujy<$|rQy&-qy@MTR5 zH3T;fZ)gp>U4vISXWUKx#Wq<2@v1xs4h&YxT@P7yk_vEegHhlXVr03*5Bp+P@1se3 zU(<qz@AtvnI~w}eYd!Ia7T&@oh9sa-mDN540@3V9br#7N6Kdb~I{@O$ zocrQqg~SLHaM{GvRKWnPux(;*Zy$91Gd&FjAhvxct+cFHPqb78u*NC#a%pRyUMC{H zi6>h`{5IU6joi9BPUBXMLXhZbN8rmI`i6p{@UiRK;FVmXnJw-X12NN{*h+gMQ|tW? zIT?ZDROECNcix#lAW9?t~_}c04QKgP3f5p=J_B6{lv;UC)$?e;Fjl5yGk3G zQm{@A>34qYgv3pf%7tGiHL?N_M3tW>qjOqH1?yVbop56OY(D zcTgbA47tw*5&Ci^^Rf|0t0Ya;3jKd?J^si8MgXE-y2TwY0Opbem?|e3P+ucZ3U<>i zfT6jOcW$iF%_K+Un_acAi0g!TKMXUt)OuuNXvy$fCAhTIIU!|+qTjVHk~T8m+A?#6 zfJaoe+#0^p#_->Iwr0d-*p<*{{p~mhR4`FyV;}FtPA*fo881CSk2(m7h4Qh9uf;IP zK!oLd!7>`PG-R~>IV`a@a=iwsMBPz2Ox(b-j!Af$G2`cxd8Ka9`u~Hrzl#k>Ew{my z0}N2nIMC^0*!?5kl?*W43Iae=JiVsezi~@z)%0)*J;xIhEwxIolYtfY1J1J=PhsL7 z{y%-}oS_673&ejXpJG7*`37aZwXh5+}%ePvGUA~R`b&$}R^E$?@y7`=@X7`yd-(HKyW6Ix$=XlXO z3=U=8eUoT!G!#o|gkKaiD3H0+$xG_fCMme6>j#bL5}hCEdfYNp^66S&#{xr@yAp}F z129=cxR($^60%*9Kp?vN`2s)8n9c_QJTrs5SS%HXD8Apr(+gF?!T~tf3vb14Xy>f|@KgPV6di=vb?l!vo^P{hDi=sIWRktHd_WPS7n&A@UB%Ro6j+BA&I9Ub zomJ>3Yk=kpUL70($TO?hR-mcB)vM;+VD56C^!n+LFUBF|hyJm1-H6FA`qxoV?9dfZ z%Tqy>0Y*Bv$%U?BC1y=SP4{(>AoR~%AASw8uxNr9DLTfa1YF2-Yr-WqYUp^*O^q%P zIbHeYk{D$VLOU;YdjFV?Gk-_&hvR6FhoCj5J~|0eu8o)(GNl|veB14T8}2*=hn)I`w{i^TG8gKoZZS61o- zakv~{{hz`oP3LQ`hOy=Fqijw{8vFA3bwVeN+9+i{Sz*55;ARJpeh&9I|M+L2%f*Vk zoDul|H}xp}xf|cqx9T_N86XC80FLKnw!%0=Aq2<9ds2)XU)=83i?(Bu=U~88>bVnq zLCXhfmSYU6T10I)K&~|RN#<>4v370k_$ArneSE%fp-ce)Lwzlrx$L)*K9T}Y*r z46-}WuwEs4mQokBp)qNpWs^^LrV@;C1yK{B1{|ctQ{XRi_Nj~kKAZ<9d zby~f5u_z8qK~e-*1$`vww{Oy6q(&2qXw4sE_;NAqM~5nY&NLnIbMt_`GJkPr$j>aI zctAT`w%8^#{*Ioin881p0fauJC-#Q$K8IhlDKRmy;GBj1?)#Q+#igYH zb-d{dR|XsaCeoBDJ;^nai63jc^0?_OSb@+~KIVa0=lOe`)u;pWA{qP-F!Eh@Cj3ZK zM7#!)?_)`Nf>!&2i)7MczFZ1gDnQBskTq#~Zr`@`VlJjt^rm1nV*?%~?k{jHF9Jx_ zckl;}j=Wf7V6D0e;S#XMNM~6-BHjM|83&9GNFy@H)LEy&8S}Z4r)iZ$c(#dHk^8?Xz6oa9^QCJ-m9N$o zxTAd0>`~zS0BDjwe$1Dc$F^VMhXQ{4(KCw(KtFR2KhOhG4jc+R)R}@}E9od%SoQ!w z)i!I%R;~J)Rb-)Kr~~j0CqbZ4ME}McS)=PIJ+JfyU!;#t@doH(BDk_!0>hLqUV0tr z8rdY6sz2_aNXH=i5pl6^mG#`9bzLT!H2dl})$Q18`!);+{0qRYXAqh>$nPTG|FJtb zY&wN?+KPu#rs&F@JIi+0w?IA&)+7z*m_9{mX;NEtKBd>`VEAO7XC!ieZUZC8cM*6P zuN&2xinPuO3a!U^kt-yvl|{9^$EBA<6o&}opr_24iaVD0y1 z4=eo9s5J)rgsXGHwB|Uj{abN(Z(Bh|V5{^D8mJqq^QN&Wr&mp1K~FCJ)ll_=o_oyc z!yHyhS=Eb20h`<{NrR^_E;XUAP#wQ_KAwyf8lxZ3rE}}z^H*${-+II;G$7`F0!%co za0)03Zh5Kq+}{wrW`Z4*&fo^#b8<#0tJxVE4&w{gRRC1k>12Iq_>Z2GWKwVi-We-~ zmmbD|`TGr+zqe~PAS~i(y*|TgMxb)+bl^_mVlHDOp%oO|=bSLpW9EMXC_`-`NZvtr zb5Ry9xB2v%b1B7}lR9fm6SE=LQF2|MC+*Dfm8oemhA<@pWmHakbqBum|E1Ei;{T#y`8AW=Ua2=h}2P38xp@=!u*7mDq0{@ zh~75Tvy3u^d{&;hUyHsjVTncZySsMa%MhvYk$-xo_#FZ`$wJ5v!42ilKc72kAX_s(^i7z#y`)v~(n>R;257^z643ie6eP*2n*x{PwuEqo@Hs7=6)0k;AE( zE6uU;=|>rbM*u^&zW_FJGKDl}k?uVR1bDk_VZ2OvOEey}!= zU;lC%8^L_4cod>Ov2I4A9q#(P#&O*HK8gHgc<(x?y2akz0SJ!kyD! zX$KPzt^xbx!{x8(dp#j13p`Jw=J!YMd^|ng$PPI@(l5YGK5FjERPY_gxw>UNS@s=J zSy4V}n0)j`>TXn_NA{no72Noy%E8n#wcmS0lLLj2o%zlLwNv@0L8m82R(GD(ZV2(95f*w#+yDoqJ|sRbhwXV7XOS!PvjZ+yalWa2%?}(QA)guGqqoC|y8Hi} z_WwTZmi<(sc@w9Tx03(@2STE>^y>E3$FtL!nVH8;G>}m{-%Fa8dadePEX%OI{R&`Y zWxEr!)1Acs?$I><;p7eNTNpS}T2c}M+4xpr3tsG#`~G6ZBvYON@~s@1=V22Pa`rR$ zp!vz}G7_^Cq6#t0Qhw5tEVvlxKka9geP<;jvUFs9+%3DssXZ#iMD+;>{mA|Oq?_DL zv*FNwwX>7mex-qtfv8KI68P0zp1-R%9d^ITigHaB7^HDT)%b0Ei{?hV&$UIe*`o%L zZdeLPMxp(arPP~TJ{`H*aSfj1kAKvU($dfb{S5rxo=-xHcLf^!P{6OtmEJ-!P%uf! zc}l&~_`}|818DxwH{4XS6T$^NG0Z8?I};s7lhMGd3Jf*peqMLLOC{;}-5lJoLs|kG zUQ;-n>f9Pbf=INjYHt2B`;6{FD6;k**lN3DcN6|DT?=_~yjJA({lgUqt@3v3%#6c} zk2V!IIO4;?!kCQZoxuLJa(E)>g9FcgFcatn_U9f^4PuxsO>wrfSvw#aD;@aT^OrDD z#;axj+ieq-eGuLEARubH=>vz^3ExIWWbb}{A%duRy!3_5Az<$Jq?g+2+h^O|Cr7(! z*Y11(FSpaFfs8n7Bo1^uXOeq3Q5SUdtLo{G2kQ0=$%!qw>swnwT?tBCjSs3?e&aVP zK}vl7bde{s@7`!RGNs4G8~y%h`AwNs)o0v0Hp1RDWF&j+>WvOVS=Bqdb!@Y>vDQZI zec6DNw5h=KE`jF)M)funZC zWzb(=N5ywGc_dX-yo;iI(X+cGm488ETf9y~EA zTthA{JQ%H1Suf6`tX6N{Zi9mVx;|27|95Y1&o1QeJ{?%ozX#tFZhBr%mnz*IQ2SU` zwy~mi*5r{Pf2n(K4BRFwaO*jRG46wlbh@2HbV*)WS(!CFc`b)LXb=@B;F9GBrDv6K zupzAn1~1Pv)&!`1=@H&Y6xQJmF@9vT!u=6Le8&FQY7md$da9{ewY$A}l49jG4CX9g z_t}karQ9O9$PsH((cnQWeB<>2SjhMjlo=B?Yr43Fv8ZGxcCZ_5a4tPFfj=uY(5l&I zBUQn!e-4Xp<$c)2{tWuNaVJ~Je+%pu6UDWBDkh=BkOfc{{vHj41gF{mcUj8018jzq z+U&oZ1O(l~%iGrnlo9RWH+i0-_ErZSgq3Ouzkf`s9nL;0xm`5Tzq!87A@^u%qw3?< z(*21#4+BseJ_H@@tq~8thlD8W;saH0CEaAm7@9DwE5b$v;Kc1rD z0YFO1H+WgAY3ffG)c|X|Wb0itnlzv9ii~ms$2F=*F92=hUakTR_@oJ@^!M{?$JY`B zY`&Nw!~LS80yN^Chn$dYG5JL#m-y|9ZpJz#WXmq%5f0~7ohf`Sf{N+Uhoj}E2h(g2 zTBXhEo7_)2WctqQ8CZGMKby5Y0kb4L1K{OFVYQ>4F780j9~1S$nci)nuwQFhWURV2 zUFShOuTldl2aa_h!b3RM-iC*hQGeNZ-blMr-w-0`#B+c$*A?BuWTNcz_jv!#-*u~> zEe8(L-w8*ruHMFo+eQ^3F(V@*+M@KjJbY0rDL+2N<|-er=2_YKbdG*J-9Ot6Ii(xF zChBZSso#xg@=pkvhS?E-E5B&VbZ@jQ@wIFID455zc<d67$CeS^<|Iv)AG^ zp!qw6zFjM>Tr;Qod+?v6WR~D7S1po+dc5_9@eCKUGODPRHs&Sn1O1ge`X<&Xq#AFzyi)M$x^+s36-_}cgo>df!tZWN{lrwMcCv* zD2te7CTPUHY1huaz5B4lAv$D?Bvn1xZYObE_#9t3)+YI){=q1K5$U@t16}-u9;e&U zYIZGXKLbyOZpzTfMjw6%C=mH(C3jycEWcFR3OGGlS(Er2%OWOY1PblKd%HQBo|Rot z`IkVyN*!1^nDQM63nS5Dx2BB#Bv^)0#2Oc^y=$(pu5osBoV|0le`hD;4rpnh<4M>y z(1cNBpgMKkx28G6@o*E*xm3j-GX`S}wF~h|xE2_9;gG2)f$%S8z>zTfC7#`RU(unz z(+QfBUc7W^9#96O0^7Ip%|+*|wyBjKm324zEPyn9JnowI^mlEx%4T)@d6Mibbp6Ej zO1zQtIt9>RYH~8LmE#%u0&4FvzLMJcL)Z8CZd zeX`T_=g%KdQ3R%@sUaBGzHGHKxsOME$NL-DK!p1w*jV7NYy6-#J-*Tc5RZqWqa)ai zfB*h1v#j7y`foG%&yS0cIJRrS%js^c;x?!WkD|WUQh?;Fv~T`>@bT<#(@Iuc3m+Oi zO9NC2FpeacWXO1Y3a0>|f~}G#-Oo}ledWtI5*eHpRHI`*zlMbR57P3ao zTlkIRKYyB=n}v-t8y_#}S1osD$UZ1EOxFyhnAjYzz4?->3B7+m#Phs(N!ZlIsQx0Lbmm+41N(yi&xf0yF?tH@w{p&g#|tqv3qj*ghV1Uh$s-Ul~wnj73lTjXqt! zoele=#$Yj<#b)*a#8!u@t3bJmlS2$)VVVZ z_TvE*@%T~x@nq@R{9%k)B-I zh`6ooJ{7I$uK5|R#gm_X3(;-n#H!CI&GG0(w~Ms(&XRjW1Q+kscli!^TL7Zu+Rkbu zMGzOog`oRf?_y&{xz%|7nTZh>S{3jV<5}AB{~dU?4E{UBC3%-n1V{>SdTKFe^v77u zLaLR+`1>cjpoaoz0oLXRf}ip}5F>R-b7V_^^H!C}uT7PWf`3o{4~j^RRQ6)zX?sQF zZ~9zfu5U5oTtg}S9(^&{BHu$}&2KaC#PrPk)~g^@9!=57t_WH%rtxX|C(Ja?eZj%# z@@}ZwgCkUf6{AGrZadE@eMy`{6M)LRr&~ZExY7TQthbJ;Di8Zd z4-U^_?|Wn2`D>PI z95#D@_YuIbDQk71<*pQ7|OXBR0qBKoggNT)v0$@U+Z{ueYW);}i`9o$I6*U4g zlTxqKW*DP7zUv2v2wi>h5DdSU{E|~*CiEkm;46KYBMrLIY<1~=9<eRs@$O7R9R^?^} z9PMa!=Xa%}-28RDRqYXEY9p^`VaQzUh(3aiiBqqf$c@LW^E2vH zY}M{5-;04r9Kg2j6iVrRrf&VeSsaq@0-hZ?nPnvEoYl)?1pcpWKU-g_y=i_hp!A<{ zEU1Ssj9KcpT6#K?$Oyqoi?CJTSm)5?=~Oc-(e<*$J#uQQsF71-^TU-ZSl`?y7s5!A9 zj*d_fBpg&-QSUhb(FPp2pL_rv8z*z8r6&Z+x9hZfyZd>V;Iz&0*d?z{FVD6hDrRf4szfVS#?)09<8U=>;-fO{jk1hD7R~Lce@q~}bJ;-ptESx^ zdifu=4a?W1!7It!C*Z+L`cjmMau??)BW6Fc+={&ea2jOvU~%f6C=w2QQvk)RUco4i zNg4o%-p4O)oy?3H{_UR<1GZ4a^Nlyh6cj@LzP#Kvqx@*T z9=28;Ui`Q>JxyKiIu8!ma_0|`@}K>G{`ggMSgT)iJn9>O$=H?+!Z(apdN#!Dx*d zPdlPnweOL&+}}MppHX?(=Jj*Oe~RwcXfRr1{nW8*=9%5I&Oz|h5Lw8*Wn^pT{e zm_t)@XG()s`6H1q`)w?p*iXHL&|$Gs_g5-@+Q-wGvSFIwR`EVlbPCitJf5q5H}+1u zv{m?n;pkw%_Zqo%f+8SDU)_{)4qSXLGyi8wWIl+=5GENw+?G>3!>I+4`4>l9RY%i# z4OM5HYe_Kj@Fn&f9v;3*DEB@)^b(hFUen;+HI2fI%9KjDww5Y;KBpYb&hY-j>uAE* zgS9XSPN)`qB{1`k$&zi@lnh2)gQH{3T+MSqZ&bEG{^{@`=&1A}8H`{E+xA!da+*8? z?|IQVe|j5OSOUqbYXbG;eGC8E%+QOSHaVR171S0xZ{tu4SRZ#=h(p8q0>dj@XgLKp$L8Jiaf(7Sl?9*;UR%Cs34zp_G^_(BXTEYIB#ZWm#K zuq8VG?c)A>IWama=&`?&l9GZ%+Q>XBx0$-p4UiI#!gHE=KyS%!-RJg*&2tVnW7X7z z6p-@G$|r0ev_ZY<{?u9{jvoa*Z8Pz%~-H?4?^^@MH1;|EHk2Wr`^OAndH zp*}jvFw>~_q(Z*Z_x$zaFRya7{V@_+rCCii_3I;p4CTg4JwNVojp_SYk1_PlCV+Kqcg z2xWmYu)I>p3MaG3H=S(qe>86TNOAwk^~O7SX(2i?mf%x-x|ZLdA&kJ5moY80NWxhB zvv*gQ^YXYIZ8kzhJe%#QaCFn^4lJJ30g~*7i+d-p$~=C2Pd+&OD;Q46XOUsB!f4VM zNyLOO2IvCl2cOzj-wzpk8FQW#e(R3cgJx=$2b+U!ih6GS=aTt3x~>eFBj5wZYkdD4 zZ`E1AQLO-S)XcN+%SusP?R{`Q#t-LScb#HDJaR|0J$AhwxL#%8axzEf*?l+eR>KjU zlQY@_F?fYxTS0Zi#{>_*H8l9bigKiwlJtypsqUzEqI8utzPodJdhhJCvK5a<`S)cC zc=T8B_psjKX{X5trWX#Jd~&+GFfB%gDY^~n$mRb_wZH20902%w8BTdHq~!Z90GOXRkp~7Gs#OFj@r#j3<8RwQNgeJfB4@BG=?GiHjBIVk7fjw_F}`R zvs4lVYV;3}8f}W+9lZT7|0VFrZ_d^8ZZZM3O)&hf2~2ROVT=DmJPP!U{a`Vg`yYQE zTn4+Rr|rh(8{Pcv;=R*TEoAxMgRQ#2q8_fezV&rIexGwrQMcqx<}mt!>wYbG&|b~J z&TDVkdjs#wR-PO>5JQI?Y`8etdXM-$lKbc+^d&%U1^z^%f_HyV>Pbh)Rr%~S_svj{ zDhwistP);O?0db0)#c;=o2L9DAO^V<`fpk8ON5MdBH*G@xnjoNwwba`?CffQN(^-l_7<`t=Z(WPwReh_*v_7p~qQ%4ehuYgEk+GiUN5+ zg^vQrR$r_mKZ!p{LvJ}%vx=p@(G(MLzIEd54NoM3fZMdSc%a7ff8q|f;>cvcYSvmZ ze_xbtB{#6X=GH)~SvsTv5}2*wjkbS1I+k=S;Xb;khs-cbIgezOj!yIPi83?bIdwHP zcRP&@WeXT-rliz}A&~^s=vWCt^7^#*P!j}amzrkT>uqvXE;I~3tAsJ^hXn-@T`A%4 z%G!)M_EDGHW-U{@$TIEdIK(Z^o7{ z+}I&u?!LBzHJi_3Lq>^?P|oLl*%1OcbwUrZ3I8C>Vk?%ta zg-!*6r~0uV?EQ|Cr?)xZxKt%kiJguUGZBn{CB{6iOW!T|4FstuXbDJwF^^>tv@Lyt z;b>jx>;C2pG1~pTYti4Wj_K^cAlqN8@xT1(3U!K%>u=rSR}aM?qzSa~Jyhq#a>H7| z5aE|1(|Br8U1Lij;qxn;wxV`NR;W)U?&)$>tnlQno|9o#?Ua~y2c>UX_VM5e4%sMd zH+{;*%%)iuNZlv+al(4YotE-z;n(VB1lgkO9;|A8dcn;SXopK2&KkTTzaLA`L4 z6XwSIUx=BMUDX8?%}ahAmY;m5ae^-8e1w{mP+Xmw zW(;osHw^Ptr1xIpjMwa2?6m_Amgv>h)p!6|;{Y_NJDkA1^628Mm~>xGMqyJwbxVE~ z=S-ppuLufVt z?ojfXTP7S<^?7r}b%_BcU&0=8L>~Zi zfF9XABt-#I0PA=xgbC8QjJ-glSxo>$v;zQN9ne4Visk^%;ojmBg#quk967h4ezt~I z-gEz}X}F9W&0N`V8hK@*lf`IQWUsooYP$J*fC81+X(j3X@q5g?WE|!j_^r8g3XMKuJeu|LF z#NLSZho%nsKOTtbVpAE5*P>X@{Ism5TqQ3L3!H7ch9xA0F z0Pg7B7e4{#3DeF(bAeapfF%f4mrpGA59tW*yYGUGGB$Dxpf>}`z;`-j&r!&`p!Jv? z1j}+8)>b~x6W{&KbW7i^>0~+8ciMfq$^+<<7MWWH`x*2Xzki7}BU8+D^PkO)aT!z( z-~R}E^ErTTI+b>F{(!dqqO_RZuY~bV`=^dQ{O!$}t$KFt{8}JXeRiW$XKQZR@foM9 z-tLd=nxsJA!P7O|lZKW+q9n6bolgR9x;Mwfhd;?65#mW7L&uF`c9Vs-wNvfWzv>`A zNvQ*2Fp+LP{C1~b@uX8N_z$fKWe5_9d`~D&fiMSf6Nu}V5n_j^6K<82>;mVT#<81c zh`Z8w;CP1>E~ZY7?&R;xrOR}!JKW(9B+6gJJP$Rs4P=t6#|r1_j;8g>ttl_|2@&ew z6`OoT;ogLNZ6uaU>m>i>vYuCw;<1*E;zQxw!Ib3M8_UTaF5xrsD+~+~eW_xe0`|_( zfh*9RoSgwQX6jcR^SSJdVK;J{^GCs-Y3~V|nE<*s0%i*S80gV$6!)!6NW!W-3G~kH zQdh%aS;0Vi$DI-x8G(HiG?FSl#oq%QeP6Q3Q2@0K0P}6YJzFYsmpvPmO!<^TJwo4-@=>G(u?-GatK6sOd0Kf|_5r|} zf^0pB?!=2OOD_~D(kmHHf=nD#R0t6()%9jwu>eA3ou2eDMeK$QWOzbn z_de83QzgD=f`tiQXm}IAB1L~yTG_S`AyE%+#Wh^=B>P{K+zscuAg~au(f6@wyrY*g`0{>ALuo%8ZihbeXlMSZ^x5a z2E2gbFn7ex3~mxz*wf)n#UW8BWKS~(9}|jw?pv;~ow-ETYynmG3FLyQeJaVqWfc`i zzzejjO2)oIHlmB+c1q3vVF})jd?nz&GqlP-z7^^l>hmz#g!j4Vs`I7ic@UDw^+G+J zQ=VAI<@#`+i4L*-Cdzoj6*si%B%aFpSaLL1N+lwF3t0ts?=PGUpx=MytbsV;1mH2W zMm<0X4}GDE<+~Nu)OZ3;RQdB}UxsYhX|HSj-a&z{H?U}M)Ff8FMYAY=nY{`FaPtTl z;u8ZG;9Cq_;-Qcm08&4qsZCq^vi($TRaI&4bol?nvR)cVHzrvnpa6Cd7yxAdN+c^c z_cdTs1r3+ganZp1a_SbFp6o6SeOLMm_Yci)HW&mQ-F$t^=}1T?0fH1@Fm~qx=~K$AhLje5(e6h0y2*%`b&^)|@VpvkYj|e)d?ktjpC5|O z81%5UT;N=xICOuX7oT>GW>Ko%A5@6>?uR1(JGv+xs*)mn|NfXjk7Z}Ty(8P|$fmz~ z{wgOsle5&lY4i<9l4P@#tA8tUlYxu8AEJg5n}mLGlnOEbxO0~__TX0EHn;9LV`U@l|L#NcR3Oj@Mc+Dk?lr| zV2e>d!2ACrSd)Q9o#^6j@B%G}!0ELjXi0s=KY?P77e9vg!D}bf=sW^xlS;37FXo=( zKjW+bgQ`&dc_3NCsUEBWUR#t5u1OSw0sS?K(nJ^Da=t|F;8w#v-^*p+6ra;o!AUy| z1hk(?97qUlToQzs%w!> zRs;fnJF;)TLgTN?cSKZ1SjZP~RLwocP&%d3WpoaFMB=mM36@1Q*E3%%B5*L1_>u)4aE_-kdLxWYuxpN+L6 zX1}})uUZHfN%6gO*Z*zrq3^i~aabrmv(120+8Z+=8*81&UurHVHy@a*XX2@;{8l%^ zS*f~cFY30uZGmMyUWmZi zj{%~C@DA&w_XnASVX3FXS+U$#Q`c?{gUVp|MIh(%(v^m*vj!B?2b{QpW6uLmkPHmS z(c>eQ98PeJj+;MH^1=PHz4_%}w~HJ8n@yDH3Cd1AjbNHL#b3EJcm6Hs``&ni*;wTU z^9o~@vNjLSuEw+6ktgkZjQ!t~28T~}yQFVhBmY+mz&+H6j+lm`h)*rhKZb53>H?!d z35fK%0%;te>gK^!fp#qB>CzuP+bL*m-SK<{mGd`Fd1$c+{d00aO9tf`i0P@RDVPuR zs~j>NC0Jc}K-&{|Pm}J%rM_ zwz_(PY6znSuGu*raHB8J8m>Hno8-~0a#%uXZctS#OX32eB4F$}LAo}04nLD4mJo8U zy`gcnLw~gcuUKpn4gw~O1il=YnMDtV)j?6nIH3!@nl&$ZNuoKJU!2DRlGX;2xeZ2( z=9Yw6p4tifuE9`y|MRtgbqfkv$RRZ4J@HD( z&_n#t$_;oy8Y)we_yO$Nl-5G5l?@xuCRJyBWS-v)4291aaK-)i1q2{nfn3T|d!`vP zP2bc8hSl#@Ao;qJJVYiBmUUoh5fnTonC;0%$D5U!AADpU+#2G?H*0tk>MC8r@p|~j zGt^mt5!%{U7?cMD;>X5|h8$E9bco*d^_N-P#^h?GnK~TBgl(PEhKTJrnY!TfjJ3f`t`H|H{3=Y@+z5M_o7$9f7qnDPjrP2E9c3a7bCTKsM- z*gY9^U3!ALq()vj!_n=P03{p7p`?eWMNAii;Rr)%jqBmR9zkST9D8!JfLQNz?a5>? zN!HO3J%6PD#(U3j(~J&duMnlWc$j4!2ZMyokQlId9kzOq5b6cv$)m<;D!toBI{ z?GR^SbH0iBXkVqL(3``ek%zQzTX0~_`Q6$YBm|Nl+)%Ys!2I}#WYRq7W)_kzbSHB( z=4ob}kUFm)z;VBZ6QdiLug99&rUm{lkruD z2B>Kp-cTzhIMuiGLDO|u%g#d2-1kpUXd3z9WmvVl7_BnxG^E5`x35;DlLXr;{73WL zkGf0lt6lCBuS-yfwKhGx*dMCvO@(7Q-xjJ@@6hJ_W&&0gSTpH& zV%XtSnKF1Sbwa-iX04PR*^OaO_zp73=*!yzH2q#48RbnoCb~e(WNWA?IflQwer9?1 zDWp<;%~Bo}^QnB_m5qurc}n9KZIc@$w1c;C<`8cSfW;v?Y~Ol~M1(sZAN|S{+7FaC zpBvFOngVUvrLDI1C_^9(iixO)zO zWGR5`P8PZP(nEJX7kK$-?VdAJMGCe$9REV5Zz zG2`P2zZDeevze>)B8|33w*e-?wxzl7KMfD`~z)MhduU$MUyRvfu4{qr{c(u~k&`(}5jer_uS z?j34(KVMoULb9#nG=t8cR1c-8mr5dK{@->RP-uCt`PkGX*ypUzj7aEcm+ zuVqALyZlSEo#wZhJUTl|*R&;-&jw8G$QBvthjF3EKdPOd1jEMbKR+Q)@Ha6?S-U36 zUhkYcRC$3Os4t!mJ9T1!!l+8BK`K+9YX(+dz!OmW7EEeHddEo-f8bDXDh$KT*_F83 zlR)X_48%NTMy95e!s%Tn?MDqgH{ovyzWjxEzD6j1jPP{;QotdKNPFBTypP@UITd7p zf2#Mdg=j5XvKLz~A*Pr=u(@3K^UKC!gpj2X$NKYPhYM0&20N|WGx0CGd2cdRIvoK> zV6faIIPI2EU|W!tg6{XM;VcK-9|21<7w(~T=J8nOG>g((67k-_1Mb>=WGauxB4?=BEMA>xbAG%x3=0zUr=DVKf(v2#V;`i z!W=zRp(}5Ev90=rpv1c~H^S`YGRzA4&fjK(9;p$hcaxP#qa{2tb5|Ow?7Oj|;V~t$ zRl6Ws39IW}b@2(G-78JOvGQ-A>~*N3DWzC|FIJ>g)oTd5wG%8qW=$5hyJ+O~%gHI} zTmr~%+_fNSbQCDA$<{diPlf6hq@6zr z3lYc+H$gZm=D}a@;D1f8)N3ikH(0onU7M?Je=xV8-eYYzTiSS-J!j?u>6#%IAxnq1 zmU=VoAy5(U*f4GhDGj;6TIG9I;me1&0PB7AVw3{xNu5UzKX^)AUYA9gwa5-P4CwFP z{>h=){gBbnhT&IkP@Dx4mFM7qmQWteKsP1k(ns`?LoZKj??lw5>5c%PG@tnuAu%`VX_O*-7D8}a#31& zmESr9-w-!ew$Y&RYq2#>D5-7@9H zcHDpuYD@$m8l6dFdzKtPo9k9|@=vX_>@_Pa2xsB6XAKB?>uEf|&Zrx;{$j)GVk8^p zkykOgoSsV2ON2Kf=lWZ#`1J3}KMd)>DLp|Pr6H&lb?;RL>J|DI3k6tMR z`UrCN9lTK`eaOhUq%J{lz76pW+a-C@m@angi7X9yqOBeGl?^tGQ5dC*#0U1;xa%`> zuCCM@I7n~&tzZxN+MV=pRj^@Px`7cFae8`s?`1oBBkU}p30u2g47mDUmESkV$D^rz zi7h4%7({Q#pF6p?&-J4s;S{Ya+&L`2j!2prl?%EB%@vjalp0$cL@}_q_AO_rq}pT< z7gAhw#-Mb_u=US?Miwy(omIEmX%(t2U}CpQYnBf>$@q6WV=}^p!QR?x3WqIr>|I+D z&I$e1G4}rmRPXL~l6m~p`l*v{HDTXQD|f~ajJ}NVZ%%=$YLVUOl|PmnN5F;{-q3NeCNDOQdM5J9HnAL8V@sex<>~%_0(ZtFE`JHJzNk z4{}rf%Zs-krJesf#q@_^@L1q4z#gC~D}>5uEg%B?yC;j%^E&gMZ>i^tnN3+BFZ^{M zueloX-vK(DKze(ZuNKv7cvNdNJN&~J6YJZopuaqrr4=#X7z4(HV@Ncu;fhF!sri%m z<0%J>dvBN-&h~}8@Z6J88x%m2l>pFM3xsmKg~??9-~a`OoFDIGYrKNB9bh=MY-rKg zob>Gj;O?n_vjtso10{EV5lHLu|L8+eGxv8ZfWhGyts;>wQ>-_UG3-!_Sib&(I}7^} zm66uG8I=Ckqyqh>tBZjMpKt4@;8m)G>PCM)iw!D+DF7c4#^7)11Kb|0DX0MgAbSWv z06p=g#9_VYbjWrqFk$&>$xlp~!Mie>V#&huj4ym# zk%fiw0S=q}9V*HP*YlsfTIwT(N&-z+LAihUgL{W5*7g*6}!(ERe(0W%Q>`IT0_e(=L! zRkm+d&q3&zdDn05HyJ_)VAGX>3~Fj>SL7s02hacD!GJuI;V;3eex;dr3cTZk(x%dQ z)sL5u|I|7C#F*y)yh~DaIRqxH*LKuev=7LIbB3aMtW0}Gw0u9 zi2*gS-v+RAX=&*O%qh@fxa*G@pQaPiiBin8=UC&Ep6o44z)=EFTJ|uRw*C$b znzwW8EWy*E4b?s)hFzOZ$S%$I>JldM+Ku9_%4JSJR|m;-p3g#JzTsIR6kkq~las6S z^`7?RikoI*(iWoQWreu$)#Wb}B4XwS#>aKJ;z)uqaife!sdloLw~RNkLMnOTa}qLb z7nbiGW>v@-up`QxoX%l4tvLyJ2<_FY=(zVqpqSJC0Ljrt)rY7N^r25;1Z~`PXW5Le zqvah0PKDQIf*z`3kfOWi;LYjuQN2Z_4`4Sv@qAf%dHC1v z41o4q(DQHum>(~A74z%;zf`g@BU^Lfvp5{&JOz*PMTTVuKEEo~IPkOn8T_ab{D;G@ zoBv`fLmryPZG6?fpksPlmtu|INsOP}3MUq@#rIV(#>isH?Myyo+OLLW_Vlbrb$Cjg zT58=RQ(*4?D~m{Dh(DkB@vOmTt8Ue{)_#UqZc@Tw(X7TbSF2bLI;xjkr&R5fRQZ z#WC+C5JRtfuRHB)v+$8}?B^|b?J*Hu&(V8k@aTQEh~@N)z0bbVirk11fw?dUh-g}2 z>4I5swhe;{zHhqPd1qx{eQ@=U##(2cC%nOCJw3f&V`K5nySwt0SS;ezsFdBqrP0Al zdSLcAVD`%wUiae1DJ|Aeo1vmV4!{Gpjxo`MoJ;?>Bz4p6c<##eVB|5Nud*{BHXDYA z79TNf%-3Y09 zcYIf)sMfZNJZV9%!dlYoy7nR#F@j)^Je7BZfKEe`Or2MIW2(zgkCNB)H*%^5+^7p+ z+5)SIL)GLTau@_IL$G@#cAxn4CfOKKg|?v}z`B(14>Idy9Iv&hA501W$@+M|abRS8 zJX@=r$kDo_GM}izHR2^H@0y++PI9d0Ib3wzC6-a2Slu0*Pj*lQE zX@{}b3VP)qXR({Xw_D=QJARtTf5;kJXOhYw!uegPw_s3jcT+&nib0Ao3hxgWExxjl5Ls)&e%lzDpZ{yG zFV&EZ6sU5_h<0e71`qnun`+T{lcV9HDEK*y9+BU1E;%Uk+c5UpE6y+S7t{qO$PUr; z6J|y!*C$_>vBjbk{~^i8=Kb{`G{u&KoJp_CEet>13K0y6-`Umey~WK{3^W8HSwHP0 zu50n@P+1Fx>d?`LY@2j0c6`M7nA5?~vbx4$o(6Hsi-zh`7* zWM{9Q?TH*Np$;JIQWM|A6O4#;Ej%74 zp%CK>{UTFJB%kKSc8T%#%1eAkldAv}6SH=J%b_-8N{en_V(4jvKa1Jj_MB}*Ls(i{ zdtF@|^|bqVyyjDQEBV?tFb%`q827*TQQI1`r}D9TclVH5n@3kJjs( z(UWo~S!VI;=qITF2BPCnnQLzs!F}q?p&+_Ik!B=DC982QA@(8823jCiff9?s4#P?T zR${)Lka+1U^!NUT%ZHDMCLT`B{?WKm@}RU&H=9&T?};HZpJn8dZoz=%KH!{xD|a5e zN4EWUN_KyjmzRSB*xizO>bcsKI;p?U_hZ5q-k4-i)f z$o{PVp+Y#eQkMGT;`6nA;qa0mhKIp+B$S%}xHaoK)#T9{t>6<~4*S6^kDrl>K9VdO zr7c#})ug2(NFUOqZQUT>KudZY@^y_s(;i{-_~k-%KtKf+fd=WdCd{=kbbsdVa*Mtc zAfmyH*8uP1{sB;oN(ua{;QQmBk?=mqS;!w<@Y&P4Am?7Upe4Ow}XX>&B?{4*>4m)@vm+s`&wxVQi= zwNgA)^BCj`*~1g2Hxm+b=X0PhgGawIJG&!IUKsKLpsYSue$oKXkXddjyhj7!rZyI2cs1%0^MNXm7lqa zfjP5m)R6k2gXsvCcIMuK8m42?{&}+*ExNM_CL*Y>%Of8n@Csak z#3fejrx^N*HB1|R2z?o`7+JSmC)>dz2dBHJN1MEC+&x3rAR5tsCpypgV|Bkhh^dAy z6Szqci`kb`a^2?9gRDEMd!3E3X+1RlNv@$pyq#YMGDKjwi0isIJBnyu07odikxQU7 zv6-3x+qvoQNSK}@RcJjoip+JLjAJw3As^z@Mk%tqCHcc_ga#*;tb)jotPCT0hLI8T z8+?ajl#kK!`n560*@WPHnJPpQ>6&vXj<+OHBk`lglw!5bqhn$jCM5p%T+glmJl#0O z`aNWgzGz7|BizCXjT;BTn@AoKQBDius92pu#e7<^iKN&cukvpsV-fu+Fp{VZH~Uo9 zvFMHPlnp-zhm&0^u^b+~IE|~7fyZiQmA(yVXkeBZN$D+E2bzM}xoMXgI43_2XW7=t zj(k!!4wL*^tEBo^;-blgxE^yz)W}3es_5oSV9nZyVbga}1z@icfZ9OByxy!cXRYFT z9y0H>Wu@;hnVeNBtGq5aE%|Kpt8*P>}kNv&d!NQer!Z?w5-hv6QTR zFA|TEcM)ZwSl*knt1Kyj4-S9EsK7{=jq2rgew1%Z_Kf{B3t>D>u@YI#+kcWb>iCYz zAZv8MrpZkEK8SGKzvdt#*`ula_MEA^`&WOGgA2`0+r8AGAM0gj&a2mL{=H}6Lh~st zgbha-5$0J#G*iV59!GD+-&wqWs5k;c`=Ui!_u`G}x-McP#zlX)Ys1%#4A{87xyBMde*^8k@wqKIwqEzQ}f zs}F&RF%FEU@RSgZfRdw9GT?aOQQSA~UlzZ00=CnGX-~dTnLNCU^^|kq+s+M!XkjMAVd@dv;5lV)BW#vXZ-X;U0oeEL(u#QS`9MOFcc6_5M2U>bpCh71{4B~erSL@ zZlI_VM_=ZfJCVf>_w7ku&5YM*kDOzgVnbs6utMfdo4xcGAK25Z@b5t?MQ~S85Y{Z# zaC81Qn~L@VI3TH5@9_{xhj@eL(xfsB5ivYw*A6|;)4qye>Q_0>*0UdOj5`AMhN5{g z)vRE21}JhCZ0_25&qpRFlb&Ywf+4<{r(Lw(_v#-A<3f{@IcH9kACi~QfIIvUWlDxI zHXpz?m|jcE%3z*V(Hx<+sZ7t!#YLbbTA;*u1@M&H6&Q>W#-NP>{(ZLTJ&;O+uzKJl zdXF$3nXC=(7x;=XM4#x1iZevZKM=(5i}x5$cxq7Zd6bNSZ75(fi7`dU1sM=mK09sL z0ygJs?#l+Cmx)*W2imv>6i-qMj7)xIb8#Fz#&h6UFCcWGsTtpe_o9+j@rxaxIEY3P z4D3rz@|q`_ePbodIa4)YQFjJO<*gh3&>`v}+5wdw0!XV52^tB-#OhS{#o#12d~1($J$Pmu3fk9FG*=$@S|iyYy_kbgtVE!7eVe} z_v`u8tQp4UsGx1phyu3vfV53lA!ga;{^17S^^cT@ZBWnM zH%1`*l%^z~eO7SUsMLP;T{*sZc2?5C8)Ph=e+sS!)m6M-4ztzr+D_M^pb+?DlxO3c zd~|m{3N-^K5B%|g#N~p-6IP6C+*DLlhGLtgH30+$#ji*4MO?s+ou8i%qn#qlvkFgG z24G$Ti64q0dakFZ_x$-Z75_@e9fjxWcfAw=wwneL;yoIrsErdp8 ztx7WX7E)F35_@sTh?@_yy4f@ z6j2`ks|6sRVjwKmsMDG9T#C_#Jek8m1@5xa(l1vN!^4;ee<`$k?-GxE_Mh#KXcu@M zj6m8A_Pv6XllM%rAA_VBQ7bA`EPziGL;#>)g5NqB!i7MVh1yHtS>UiHW5$qlj1o|O zWxG*K{^a?5j+1aq$GQ35ztw1#I`Yt-rCUb|V*Jw)S8Hz69pENBi?USs(=7Nuz( zDmG#ao+JRK0r$}%RCfb*TgaZtx0--?vDR&WMK^m` z=4hs2apJW?<{;cc6s>G+#~bej?;2hTHUSN6>K7ZadNwCer6u5yfveoM`wLI+X6^cb zoZfPuuiM7hXqM9JFMJj|^q2h})9$Kw?YzwUQkJT1rXzQMQQIn}iqj&vt|9zT?*8}K zgys7xeT>WWLfbP4Z5sE*2FnT9v`I?LDX*vF0Va+rMs31v) z5=bb57%%Yxw$Ao}l|yaAw|2)qerITiSrpd-)yuxLA0b8_3Q{FxMua${Hb|^4m!IbZ zz81e-lBoLSnkag4l|Vw@GN+N4zpngLRg({T@te<|Y{*y15J>gdkSC+^17tA9@%&m# zf&H%b-D`W|J9dTy12ikna9?hsky`@NsgkJ%Sn=WKyQzaj?H?)k!pPWr6C~(083#)& zBmlsn7PQd^upcC|&S#qyA{s}^;wOs$++0H95GsEN$U)C0XgrWnvyZ>jiFE^^Q{yRk zzI8>nx7&rVTS^ft09~`KZRMPWw!s6-{@b-DN_)2u2vjL4y1#SW_;#y(0psk*bp`)1 zt5JpLQ7Wr4reE@>peb__VH;CkO4HSsA@d-3Md_bGJG2kN#QCwH_63Ds;hRCyPjdUQ z@TnF>HLhL$f-WGqi<(G(1;7F~BrfHr94DV*Nm9i2H-Z-H`4eqBoZsc{p{E>D zp5L7VJ6ZT9ZMEBZMXey=tOG^lnq81PdLtcEnX=+3JdWXAMS;CCwH^<+!}@GE z3R2j0@0q})sLIxA!lS0MJZ8=?hR&QMb3C|?_k}o^lc4%B?A52&5e2Ux{pKAm7>s@~ zRorE-S!Q%z;n!zfB5di|aq|MS89@Ymo(O2A7tP|zE@!a={@$nlP>?wFBA73Md!g`H*#@e;40?+d#px%Z5;53E0-LfybiYXoiY^3^HH;r+urRRiqRL*#*< zTR@e-a~CEB8E*5)=j|0u`sl@*C9CTD!poo&xiR2Ot9{dFO{Y{|EOb9Rk1^KdW~N$) zr0eep4$U6sXdurn0g&&p;~UiYJ24R}AV@D^J$5p4(u+F8{@h-uL@tmTv_oQ1bjKoy z>p(L%J=YSbk|IhOcZ|}%z3NR;02cv$qL6@%=!Trcn(tcn@-LIsLzm6DKqEqbA3)f? zJym;~Z(n=9+Xcl<2#6F_P~-8&cSF)cKAZ&*e1J;6Yi`up#Rc+AV$ZiawrWd`&-b?e zR>!3D+J8W09srOJ^$5Vs=OCfnhD#uz1yh1@!+glq-@oCO(C=^Y7r`Zm#L>?o`dydc$Q`e+zIygX~!}qmXSu)OWjvvv6XmkN_>t z$p`&$+ci@O#j-W`(20!Es{iKwFWEMhmW}JR!r!nELPmX(zk^_7d9fz-?DG@m=4V%= z%*!GbWo5wO$oWgX00lF^v2Sc_1TzNP$)E3HuB9EcYs?9yP(hOi#J zYNza_6kLQ3ywOk@a0(F!O-&29`NoZY5YG4TC9059IZ&gLZxj*}x5jI$Ca0*VsZptc zkU%6o1oD*s1b}mPI+`O5VH7)kDx#l`+qhBbKXGt2{4Jf1Ki@^2n7dg?YGp5r)_AU{ zXWVQ??xnf(e95~pt_UfbU(fY$OOa8UA z#+9K{bHhKy0`Q!-Ke7rx?SHLd>*O=wd35fk1uhQSE{T=u>5-94wj1A9MSb6n!>Gv; z4Uh>$Ac15La$4Xkp5jPK(UGDRF(1nx`9uBvjTP(wxIwHPQx1fY`00Z^aJh(}eIPG? z9TG%FoskA0cIUx27XLDu_0ZLyUKd6dgL^TB>_X*~Mq-+pFzw9b-%t9}1HmkEH|Js6 zL)idC$#D^ImjVgteSj8&c8lH!>TD4IWck9Ah11A?v4aYaN)(I`zfF42-G|nA;7?vo z4xJIl?&gijb?RX0uLD2!R9bu~97c~mQlKHeHKGk`yq#Es=1h?c>ZyrIJOsZ&i&xv= z%KLU2bl!XKgi;xJ&2~*|h7>(+-`+D22@aK_L+8a!HX3!-f6Q_dr5Zke;rI9 z?Rx#_#Y0R-sfy)}xag{^bu)24`<`$VS_c@=c5P5Pnt9{<=_NSLQ!hl?DZY1Th4YY;JDCRV51GpQm0m$>;9-UL9#TmV5$cA&}ls z)PcFY$8JI>UM4MychSsLqrZUeW#99>vht|q)o4S{ik9wTh#A_SH{1+rbIfJIk>%2WkNbkSQ+D>fFwYB;VVNSCJqB@F?Hi>Y@Jg zhb21J;%oVLwLHCYjyE>nL#d<>CX@PZSCO~isySCY(Oe){AMlv{j4&jy8VHg~hYM(w zCDD37Y3O?S*8Bc#z8AO=c_lo*WJ`vN{5r)yxl27G4R#7#qa-e|h)uVCbhEcu;|X_K zu#o<*0H+;vX_Ik4CkA&_oXC73Du$MJi@Nw0DjbK7Qeqm(=4fjG^dEgZAE}Had@lTR zOioVaTXF;oV#CVja&|EA5F!6-?H+ z0`eD7jg^dLEYFGpGPa{txVn~Z-=q|G+m0@=Ak0e=i71En9i^QZR#$-qUxCwzNR7Nm z)V1EN87GvsFB!$8Gky9E>52AI&f=~qN!!ZSFj#>8U4_G!%3$g*^v1X!j>~%|=de@k z14-^Ck(_Vb2E6hh@;W=9x+v<%x6{aHC3S(xRUA7e`wKPHE(OR^UiFshAmj5Va~d+7|2Rz zg(Lh!k~lp(JL5gVh_qUN5PepR5C~J^5p`!Ip9Fc($AL(#4~@@2@-&g+r6=JJ+LKjl^^*9QO@Lvr^fH3jE+rl zYbu@n5NROl!QW9s@imff6z$DIy|+ION&S8ceQapk1J9$&K3F)GJFE6!3<3e^ZHgZyDwuprB5|Y`rox?v<65)q%AUn$s`GWqS?Nl5vQ3+z0fav4=NU znlV{<2qPfW{wGpbSg@_V7^UYG45E5GZKp>)R_KK&c!Ecd)38DDk>mIqv-p zZasE7J(HuuK}dj3ckX3G-QU4?%e|=M26~Qplm*7b#1GT-&~bT`CAv#lf;Ly6ce%Gf zr)VC|mpZ^+NI?YDn#hx5O_&|~$^V2jRi5pKlMN={nQyq-D!^Bw44lP!$m2ZBh(tY_l z0xUv9QAsaYS$KGo=_+1pGk&V9D z`7KR&-C@K)0UC%$FLArx+PD25n!YkD%I$l5=uQD?X%Ggag#n~XB&CLuE>V#VMYQa$hLxxPY<3J)Eye(guySTqH2g!>&|}JdVTq z`r{j)atKA+qHI2sjy=xoKjawMbri4ms`rW4!Hdm!)rb?s_VC%&JMht-$oV(0rHhhN z@fa#cWhCHiMjGuz;t|oQhv5+&N_+DT_AE7dkPM+WK~V4l{DQE;X_rF3g2uhZ+8Uq| z@jq8SZGj1)A3Rm*Nfl-XD>@TI#gqAeSI3`xqTt?~lPHv11NG2dxBI~dzu+qHfhe`hil3@ouZUe?erjQMC{FIIu2~7XUIC zYO+azW$lKUQu#Ni4hb;|InPFKOeATPp0=NSA~32abbRqi;>uX0@HS#gsk=h49CdIs zE3_3tAe{8UwnT!2!UQAXi}J^3r)#z(k8O&wQ(mnl|_Iauvp z^4tOl_WIXH77YIZ^Yx}4Dn)1&oe{m^(%&YyD*0&>3If7k&P-3=w?HX4{rwZxAB~kx zf>C&Y)DRA|pRf_mHb=@{12|kdnGbf^z`Z8BleLEzU!OS_r|lYv;X%(v&j&P0yzYVSrArm zZF}2~eLVjE>Cb?n1rEm3Gb)Ml7uN^|*Rs|*DW>*$T>ww&Ugs+wY=L(pj*lXxggdo3>Li91SkRd z|Lo}hJ_%%0WN*Zpnb&@>XhFV%GB)k>*ObuszJeWWI|SZR$uEb5c?g9ueg)z)rMIdme#^1AEy3sM52c z>_zK}2o-&g#l()Y|LgMEF;eBtZ#LOn4vZm;kjG&PQoGpG@A5Y-2_b|{m|yfTt>4yY z!0PHMCE~+S zN}=MTYh+W;JenpOL`##*N~h=T;qzLX7$Jr8^(@nMxYCAU5y9|vi?IP_7Ba{3(~>-X z-3G8Y=r}#WkNu~}>)=;r@VQO9{}-#!v&<8x{epaDwY`B0`uL&1=sb0d5{@5y&%C=F z_@Ek?9J?1=6J}E3PR-=g|2_KaA^lVT@2NhvfBpULc4L|c1p?dug7menWuotQVQxj* z2{IN(NmUiEK<-kKMha?I;KgUot%;=-gSAf0aViC!{{0LmJ=~wTXIOOhVwjFdlArXB zb6QcHOHNv(T|laMrA6`eKvh~w3Q8?AAQ0$< z_4bo0ys)?Cf;c^IF5eUZ8`;s<&oI3YFgCIM3*X@ZM=yK!AIN(D0i=0)`Uq@#F)Ez$ zuU0&wy|={fh{8n;u6;X@rh=j7FH{{@I`_p93ojqa3Af@@cyHm@g550gN|f2?$7A5o z&Ve7O?(TjFFEubR0CKTjd+?1$llADy(21N1$k<;h$D=1fyCGX)elF5b+dz30d{HdEQ%qLMnp6&B7z%0S_ys zevaq{xF8h4)i>(xI2L3C+?T=J{KXeuLW* zbuumi)P(cckc+4?6OE#w7Wc!Ng)c`!nb+|pj>VaFd}R`}bQ>jDN-aV;ouVLt#`R6g zL2CI~b-JU;WcMn)Gmjm=PL!37x?#z*z@nwz&nrJ<#*BT$p9Fk;wS++E&I0n80h_gJ z>37?-)!jn?8o#D;Vg+B$-@Za?O35rayj=4?E$4we+kkV~?7YX~acyseul232hn?Re zaw|Tbe8=u~gzv#GpfU&$pX|fnF#q)(gqEx_p6g&yfsz2SK2`%hP$10BX_hz&r3w#U zLU`i?@14kVEO7*W?%zOJV$Cj6x_Q?g{=g>Q0xPx#LS)Frw+0yJ-lLyOAiDDd$Iyo7 zxBqWh#iHetTU4wvhp=*`E_6#ujKyxuG)p-s-(8SuYa$>>1)BvJ0K`ZK%0C3SlTB&5 zrJhHBp_0`HG-`E-b5l5!xzwVi0&*D*FH zkT#kM2CE`#v3lwrl_XXnceSy`Z0XRoc%x7&4Kurk>#Q1A;gG#BDf1z*G34xDJp%;r-|f~|ghv43iUW*>mnb$#;22m68qFL(^Ab+EbacDN@ zN^^B})#A%I3Xo?=*l17$ZQoDjoB+aD9Gn82ww7sL;gS3kj775V2S9ec4=Smj zJ7Xj#MpKeGY3KvorSP9)z`7l)j66&}QkiThu)ZxA#Txsw)gy2V+^m7;*2}SmNIFwM zT!-4dHK9}LjWCn{x*#iL_A>HP;P;PKE-sJW+Z}I?U}HCKKYA789dQMcq@6X1+>=Y_nQ43^zTqBzI<5>RH3a zUjs|P-*N93=i4`Le4zHh9xMXZ7j zJfU<6gAcChYk)NbLYF5#dd+$NghKx_L-;76Xf44y>Pp5)>iMO623CUAa#v+St?gWT zjT^m1lMxf5@}jJvbE^JTN#Y@9_e{>^85?o<2Kj~aMp!GD=MNPRAcx2 zzk7)7zF@y(1#pEkdLH(KnahV9r*akuwJuCx+1sGMf1FgKr!=1o*f)a~N?(F?CCSN; zKTtf8WydE7w9Ht?QH2{!F0pDhw|eeA_wwOS?A2a`%&c$&Ja+zUrPCkVJw@__`2j(` z20s;R#J~o*d|hTdm4VCd05gB+ALHqqtFfUz5q${rD| zkVd%%)H}B3h5ZOgP`AI;=HnA<|9-DXZqJee6bC#y`w0gVDP^1|tMpZF%sO`-M^avd z!9(b>D^q*qH>2{A^t-VfFOp-e-xvN;yxwkFb^X@6uO5WCPS>+Yv%wfi!$kWBgjnV) zdpP#^S3Ynhtt~JA0yePDsy*EbpmS#VfO)C;8{a-&=|n)j_RkBc&o8IBM_4v*~?q$BxGt6vnhYCDu$`<}e=h0C7h z+kaqr?AnNCc{(V4xHJT35M<+|8gA&m?Xyi(IvInG608V+Kd=2s5?L#fO}JxjzX)NO zz=ofI2H`MTPL|NIL)rq#;N5QZPs;j9jxBlLE+`*FfbYB~{iBML)t$+R#7LfV=|ml= z%t{`V=@bvdqOu5_k+-uk;#5>dtv%NT(nZ3Erg;7Yr!j84ZcB_0`TgSw87utmd1Blk z)Sn$nTAF^_DxDVv!Vbv6=f7!I-?0wy-y4w^oYyr-fjbm zlHPB?CI%G_4!Ck|UoP z`RrNnpk{}o5u_Pce>4^w5YI}t%?|Wp1AS#mn{}IKafKUH3B2eq=#K3HG66h2 zz}vgUF2Gh6aU1{H!YkN;8fF4_f$#(}dgH1-q6gPtu!7qUB(53g?j z`sP06f@v~8+bYKQdX&%-XPPK*B0 z@Fa!^rUo)3MAOz0@z~w*di@mtMNf@>$5-hUf~Q&<4+%dtnKO z-I^zyuiX#1BmRBfyv|tU%I&*0GYbJ^J&zDzKvU$9KtuZ6+JFC*JoxnDza-o)NZ+{) zel&Y-5(il5oNG+d9*j1ePkDk4nlCI>xf3=g!4J&vV7{4v0= z4`GVa=^y~j=6V9JU%$q}ZJ-mA)19@juy_gX*AHKGd1$e!G8lYO05NP>Z|a~lHh~Se zA#sgjU@Lw=w^Gt56MM%B&J6f@HGgWE4Vj8sXeOQ;dTY#j{mR%3UvjemV9LK{HBv4| zN;hHX!+rxAnLP#-yE*ce+1B;>GoY+QvO$dYjj#)Y46NABl3`$B?9F}AVGii z;RyJ(3)+nDLM_$X{G%H709381VQ#JedDZG3-1@GPFvxC))N4Xea}xe4z65ycuMMIc zaTxkD<~U=m#PzC2q>zgYlZj@6I<3e%q5MpFVc(nsZ<+X~YfBFwkK3CQ4___RM&EDf$Y@h6EDZqu$HpgRLO z+tocgE6}!OTOIFDy#V0xPaa|x}m8y3FDvrB6MHm9_N@v+18HlDp27_`e?n}hp%gUYnocBH@P%U@A{_D zxT>5#rvKAe;ik5eo_)XZV7egc(?~c|6nr$SMT9*4U#J$KT+Z0RMgE`^oH6c=BD+`s zr#h5eB){)VOd*#F#p~UeUrzMLP<`3n7A=${Lx7fWAa(=7{QLfzJmyAp*d=IZ@KLbJ zq!Kb1sNyXCM^Mu-)Q92v`^ww=%|YdjpKdKL8f^1DHgnZoCznmnwAu- zCvO%PAKQrgRL4S@H|(@;%x5nqRMWgr9x09kaW=eMl%t zQE)W3)a;jfC6~=a%!9)%hY@-MVI?My{l3jiC20oK4^e|T*Y>)x>FCNVB z+E|E0?o(V*mZP{Ef9u9uiP+^=S{mX|Ha(akm)!9s?)w2I{YW~gTF5|v$nX`;2qb-j z8p%$2K286^CnaBkPV9NTWjn#;_vSWIn`!cuz4@(*4APtA91q}RS6G?xtxG`?{ zv8mJqLyk**{T)CVPWcu$XNd|)bE!y^BZdB6z=hfkZwdH zz`wvYC~fs5HZ-xV$w~xxtr%b8w#pNs$MPI&(h-%Ri*O<}H8=9gAmRM-ssarfD0FfL0G_t0K>r}rOj_c6XSD1aJ zn_@eZob;+{B&-Mnj2m3g-zq^;7Sq3hRS;a8dHeS5%nZhROAqhS?6Oyt!{R{!`mUck z)^5_#pjm3jg+f@(b8@At-pwEbYmZe6@u$(E;IRH#nQSdX3zC#G!ang02=% zuNG;qsmAXuvsmGQyW_61CmmNo{_WD}Nr)qknqk9~xg6aQZHC5LAU@n>n#p50mD8k9 zhi#13dz-WnW8C)nNo<|Bhqq^ryNGk{y5zeVq;CwIjDad=S9Eyqfe7n@E<+F-6FcL9 z#1r!O!efoj__?2`;al&TGU7d~(F!kMWXXwGK!XKsw5n=9sDL1k8@hH3kkT;X5G5*1J^`mCZYIrrTl7u` zc)CVEir+nHzJU(>$@!cub$a=uu##u#PC~R&kNFU8#zE>7!KGU-&Ju~mVy3je967)+ zsz>jSM&nDgj!frwe6tEZ-xZzLU<>8T$MikB(4eZ8DiWMyb2a2wz>h~DvY0qn{EX~# zRjxW{2RDckSI{ARbOT?i@W(Xw!-Ulkp4}yKk?%jpW(yZSlYOY$AouOdO+;6XOPn)J z2fiM@oq4L-oI*&I=!K`d1wuf~hi{r^*~l8}q8D*=gV<;VRyY|hTooKJIS-N{m3^b} znjLwjfB&wE;_@wb?Jd4nKf5=D<*&X=4od35HRKO6)s4R7^RruuyvOUC~NnHl%a=jo5}0zXZOepB2*h|A@7-j4XS(pu%EDU z5y}(j$Wr~{g5M74PK6KvIrRL8E0LK~4XJcv$G&4PIzUPQWcVfzx zw`{GR#QAsk%-#L_=!^rhX%+rMbyMcx$jCuPFHXbF_B~31>)pvK?PW~HSr;Q8rWDrH zRLQAP<`3=tVY%`FIio4gWdh;hyTw@VNCSLdQ=J=9A`^=&`^=V83sc@~V~1thHM1rT zj4b3~MC*hXR_GO|;~wDZzQ;fJ*;c5M+ng=Sc=aW7x4`<&{KVPx+stnj&9{54 zS{e?EIDJ&wGBYumh3GuSgu!uMwiElIK5If%#|su^YlpP5_)LUAR<8jNo_kiUlMOZ56Sz zj&<7XJ5@L3hxfd&!ePRVa1yrBBii+?HddMO;Gq?(~=BN8h&5upP!_l2?7xkAqoOujK{lU%G1Nze8vLR6Q&cyiMeK&*@r$RtxOo)vfcxO?&JI@q`3CSp&6 zeD$FQqVz@LoiRu5P>vAv#CdB=RQahc(oac7;H1Zi+9h8m1XZFhoVh~Go%z8{OvkR|FeC7S)hssKQ=(4X(buf}yYwHR_0zh!I zz@*dAl<6qDg@z(z7|2w7_2+%-i;Ke~t9FUXDB+2XsyEJp=_m4T(g;cC<4h2I?#DAj z)c1`Bv|8n+2kOoj^Uh_&g(jX>Fn-|p`Fjy&CbUyl_2LPNLv2925az{ z-E)1xp-;!fjqgovz$6&G1S*h;J?i!j++Fk5&dXnEtD@zshi+Hn$lSW{j(%O+ziOBL zKP?_y#8JR9Rk~;%=wyD7cQ@>^D=rU&DX@}8bzQEuw8*#742}ks%eI}8CY9P#kk})L zt)ms-`h^?i!*)BBJzDk7GXnt*mU*O)c1*H?z$TlemSn;kBaO%?QT3-&VF=5r8uw=N zve`_l8z_WLt@+!h8C59xH}MD5=_2Zndt^~Ocd?f=B71}rTagYMRoBF~3A0jyYp&6I z=T0$>Y$#%40yl@I`uY~X*|u-2)99#FD=`xBLPFbxCYQ>teQ5F*&GUEciYkMj^T64S zZL_>5ZP8|zg_v)rM_r z_~`a)YzhD|h*A(`RI_0{aEfgk;yjdk;<5FAv(pr&L`wvxc(ibDa7U(!v|nv|>7SBQ zw99U6Lp|OzojTTZ!&iNa-$-MDeja1VC?Q|q>wZhMG0oBzU58`~E z0arf3{NQNJxy7$THN>$R7nqe=2Q=Q>@hz;x#IMtL{IbxiN+jD&-IDI$0b%0cv(xyH z{C?B`eg35l0td4Icww*>m%E67ITQo)$TiM>ku)va?aeul9YnyT;(4aj$dBxb4{Dpo zwbjmlBXr;W1Er=q`of%|_Ne8%;F)w@M>ju1H-sW3W$Db3x6v%Gw@v`bWu=@&ae>Wj zi%^2^B5tHG53fe(4)2k{koaC9F5;~kerC`2ACai;WDFRXV; zvyl9&v}@uC#-OS@)5B-Ttq9ckA9=~ald+OJ+0eA~0i-g9{#w)|B zz_ddZM5LVusa9Yav&vwiF$?~cN=1X^3UVG5xp{(#{=#H$F9tHNhC_R+`|{E|oFaHv zI{B?N2K0R=lP7M8WVb9LX%x>=F_@NBieYvldy1*OUvHX=j?2XEaq9VNp`KIG6a*85 zn(3lxB=B*=Qy`F8!8Sn3+;#i$U1KF%m@wp6T~t!To-*VVSbaO-O99cO0EK~XS9{3` zw-n1<)h`Z`F+$ZZpI1(7F5m=i`}(Nhw&*)u@C#wgS=P39G^i< zS!&=aFpr$UW1{KzF0)fJDRLXurfw)#D^+gws}pR$~atw+k?yV>{;1nd_WWpyE42a&eLFbQDZ(-LL)t zi{C^!{REtkl8m^lsh{O_BArGWl*DOkaCJ3MiY2745@Ut-tSfSdw1o^#QJtu)4ffMI zH)g?c&+%*8L_I(0x+h>xZNpwweS@h_jJ!a#+%xp6%~Un>EXjqG3*Muo8+^O5=DU=E zS>?OFLHR^`AI!7Z0XziR*|ox}Yi=ZxVRn(w)K9ipRxzY@6Q$X>J*!1!*-eA&9xSXr z9jY_+>Ooh1EGp_)qhl)E06At(dSx%Rjw2cO7iu73_*4>OqZsKL(^LJ@X#pV6-$`gm zC8Qd1n$aCIq(bM>M#rh#&p#z8$><(ctuY?Fs^VIDU-f22WHBcR#=qxua&j?I zt#pC2`JF_zfMd}K@K5c8ec0wX#FrIlHMC zR?>|)+AAnE-t2^El(cd#t1%m#_0>9;Zxavdw_Kocqj$^e9VQZ^5E-jA3vP*hH0gIW zpyI?qDS6O7=k@zlTeR)NEB5I56Vm*LeS$Iz9Lkdz4PZsd{b7ug9M6yXIH4b7PC&c-gBRNuY_3Pvp2w zfwU}NV{@$bysFu3P9CKq5TiGQ`>Sp1g>?s7ZF`v5Lu0@z<OwBuICe_MV zv|9Xb0o1uK+;QTyINQL^@cn4~A>)MsG?IZVD%`S&Bv~izB~xlXc`2S2R|eGS0bX?uM_v>VnpId-BK8=a~-GXS> zGy-k&T%Y0=y~a`^bg@BCqreh%1zgA`d4>*%`N?2KA%IA~8VhsF*d zU@MU!d$I8J_ET+P0!NVvycv3=Pe?bHTa^H5_g}KVqZF3-q@iYK$Z4d#K$rw=g)tTku1;;p*sbD4=DpEQb zEii>h=MDD%M_%yVHUaK{fJrW&uK<<5rzwK^aPV2Ee%Nbu!4+2^Bp)+|Lq7dQ zliCRDTV5;~zG%tlwfZ4O4W!8G+l4B3a6JDO$o{}@=urLi*aSQ(FH(G>T#B?U6`tI< z;cAAc6mZl++XW7b^K}yBMo+3d{9-dGOuR>WFsBfX@Ju45*D`!A?XE=Y%-z3W(LV5K zcJCA=`3gOdhcrP$XtTCUn#~YqA7d;NS~^{Ii9j`ma13|7=!zX$Vyf(>>FJLFPtvN% z4Nh(`mF3m$9(%M7O7SiUBsg|7WHdAZmcz(ia?Ajq`wo6+#??jW??rh5?!zE{-<%#P zhBd1hB-?sSFUIVe{+Rw>EX2zWRj3A!?a^CH%aGHMZYs}ykuy=qOsOFdIW~b&`F&G& z7D$6jJa*EZo47G;mo^FM`oIhra=NxQT4l8g6+(R z!FD2d^;SIl!i;p5+zqu{9!a3&{kpG)WhZ*%TSN)smxob{%%!I1Hld8QRRNf(~IRs%e zleRD5`gq*)T40uCN^+UJ7Z9got4Z*E4L9`vb*fHc{(5HqiAU6IIs=gmLp48s4>;nfKDI9MuyHcHVjEHjwl}_AmJCtdUGoh~G(eQ~uo_~V{eQ(nZKY;OKQ2xh5 z@GF_)R^CzoGF3XA=4jc%WfF&EII%!EJ&`z>dOs3;8<6j~Mi&DpU}GwkrIPRrHbspi zu?HvY`Y!amkm*~slStfYfe_(_>b_y`DB$V1zLkZD;kMcy_bF=ZeYV{f!apoAMA+@K z>m5K`b8uGO9X#==QV2R{XhxqC8YIOlIGM}^$#9$mc1KBDup)*Knyae!e39D?a%!9P zKadE`3>3G0rQD`&7AemgHd5TslXY3*4pn%u;ONhL4be!D&&2ha$B-`lS;v+go4pe4 zr?>9|5;RKV)WWUK*WMIR4L>*)hBP6Xj+?~>MLGqa>Naju>0PVmu%&b!d~BnXj+ z65s;JQYHjV)BkPe6ePWFJGGdgQjGu)6p=(5_8eDgxFXWcAuDq zGyRXdq*V^+43|U|$FN;17AZ!X7u=iI-_XOG1N!e5(6J)vCw@1Zk7JwrF2vJDed;GP zN8~;EPSu_Z)FAwaEI#~RBt$zs{{Dqj4lM|87?=UPUUN;oXs!#JxFtY`Sws>#M!0}_ zTS)pYh3K4&LqY-m`Zf3lH5C_`tdPUSvor2FHe1TV2ppMok#?6F*ud}6tXWPFiZcXUe6ceh=@%k}YfztSeR z=+%3s;EZGu}$2`T;c^7 z2s_`5OmpU7Q*uCV@Qp8HX*Jp-B4y*CpJIiv14N zKf2php|)zA@YMAL(zGHQ|5Uf_vRCAP`+?sZrKjo@M%Bs@$p4^M$LL)|Vjb+2%^;R_zK8f7ql<;iQ8RyJSlaMFNW~a%iIaa+ z!tK_~;i85_n|}LpgRj($Iq+<`qHe$L6CQ>L>L}VtAEmdULq~$7<#IgXKIlUAsUU*T zV8UE?Ub9>CY$_K}f*gz;s)-n5V#$Bhd`~6|U{EwMdTCdg=kSu{}-_CxFfy?y4o(_*lHrg%z(x8!KMV6lpm~ zeRGNu`2_D@ZBp)U^S2sD5z7*5B{~-2k?WD4M$|MpDR{5+76g5+nIVy)=&^1mVA}R2 zbPl;z2qG;Ea#hI2soEU@VM!u}W=aI4P4JHHAKg^jrPL-}=fO@*RSpGU$X$O+bA~r4 zgO`FsZ{#Y^ehxpi)k;52$>4|knQJ?8UQrV-H5$DQ$fkte?bg&p_uH9WuC3vdLu~C~ zg)mNcBnu~9#Gj}X1Dm;XMyLZ3l2E1F1MmXU%0YHPswe-w5pgnk{=8!S)@yK#8cQt!1>=pv3koH zjlEdEKUfXD(oRlQjA%%93cKYy`4pq#&SvaMsBcSCH8tX@Qh4dK4KP5oU==>iOh_^X zgJxp}BvI7=i=L2V9rxP7?nciGsVg(zr|U}ue*ZY*o^OAK78nmlWqm$)aufmp#$~st z{td_;Q0!@Ue!;Gotjrk%qy&F#h*#7Z|5OMG zQuoRZj@X;|HV_o;rX>%ZL{Gs46@6-rzalc#EP|JG7}^QpG<@!SMOI3O-^R=y(=fs% zdOsuLU*uLU(RzdYSh!Q%fdO1X&Mg8|>yU#aq%*e%*a4p&^DU8}d_A^7+X~AckD;mH zm~TcZKRso%)ke$Ceh0)U(gO$E47^x(6EG{c$q+h~-?)`k^A*wNLMEQ|{3e-_va0T^ z)=@ZM(aRRvs+CseSrA*@gXuGb5J*Fo?3XR>SQpz~@VV=xk)zu7h9Qz+>p>9Sp~Il^`y1;p;fUwiI9xk$`@{55=G`ukXB|9@J3qbBh7Y&Wc@2$$ul ztKl7b!Kh7rwgjuI+m!e3i9$1i*e*|u)kh0*k#o@e-;-M0V21|!qZiXWC+iZDyDs}# z$I$kZ{E17XQ4lu;gNxPRkH`-wd|j>861_%=fLE_y#*)axS$#Dle*k!9*t8ek(3j!s zz2(}6a!?^;z}vGXcpqDyk2E8997Qn39Stya4oh`{4rY%9~O9{elJ02wj6Sh%##uCGXT|8@TQf7fT{4|i>xHi%PwIfnyCOEeK54;!Fe z!$t&8xb0mOLM_U2)QLqL=M_yB;Wp&;e95pGy1x%FICY{E$rYAtiV39)IXk~{RzUmn zAUG#kMa7-V015`67io#1{M&6zcJd=Tj?|;Jno-Fagqv?aY3)PT5LG4h13yB>4GPy8 ziUwdgCVQW%@4J^iJD~pUYjV?c;_XFT@lui~bk7xz+2@8l))ss<`8Q1yDHw>>VfH!N zfWK*nviVIjxelqiAxcip#DrH$IGG~4C|XZ$l7nBYWY<_%pB`{gI(6lOT;xBz;toEQ z*h>yDE}&Tke+MJrNRZM6iCo;=`N8yEPFwE`iTqoQTCUu>OYo!@!OnggNS!)ZjQQqm z{(3UF!U#fHr+WjZ< zXa;tE?E*Vo3P6w=SJ<56OAo7&CIT(>MsBL-=1AyH$-T3Z&iQoa{1I&}+SxS4+S$1k z(^`H1A?d5D$LLW6Nu;R^W9bQiL5pi4=Q&}0)}&8pCq=Q@%x=DuqN|KIO>OnJh(ZlM z+M2JEBGrgCjjGGyAbKqJ;`)`IMZ-ozJJ3g!K=K%h;m$FxXK@uToQ!q>GXEeV+FZXH z2lO_y2<1OzKRiPNdG%A5btk>MW9xOhmAR>_Z{xnr-^UuXS=QUc%wH7(QZO zQdC|%Td3T8vn%9cLYH*K)6KOh5zz$_VF)9d6^dz%RN;iH3_uG*K04st5^ylA-HfwapE_!Xp*5c!M$|yd=E>FZd(88a9N{$b*7LCF5 zVu=fLgp6*tA7O4ZuR1#P|2ON1aV|oh4WtfzoEq!`NoL;QvG!+Hs0xder7K z<~B7T@lrdy|D{20pa271Ilr;L7JSLN_@<9iMK+g77@rK>2`1MHFLsz_b8$o% zc;H-P{@gXxXeDxCh4Y8@&+PIVZN$MWuw@(?{#C}1Lz#K~GQE_~vF{(JYPFaN==1LR zP8Lap*In5z!0@f@!SO8(8ND(=Y_|$IEPbhB!fI;%iOlda-$^d-WKCR2jwkD-9QG>+ zvn5Lo>D4W*ecK$HHk5eH&_DKrE7Ytf&)EaC<+mvT7V@Fi*Y`(d!P45*+COck3C?E5E^#e}e zwka4;SG8aG>z`1g&|klC4jY(cK5gm~`pJXxc?)whaoX6>2q2zocKT?O-=sUZm_&2a+IOyB zQCTXVkGQXB%d-=Sm5ud8>*-7?3up%&j(bVy=C|PIy2|iyrNuVv0X@aT*!B!+B8eP> zH}7|E!trVolZuI|aQj0b;)dm~CvnDO;Sz;cL!toerSA!XMKY4;)SF$gw$ZT}`YaH@?ok|Heq^)Akd&thvMQV2l?(u5m7eOkfa;Wnf1% zib$jZ`-45syha$|fy3mOpkjf_*DE)YkN?C-;IMI!DyZ;o0Cvgc~k~KXm3ml+0 z?jdOXSynh>-c^DA07IEpA~fY#HcxU~XPV=lQ|nBW>n@d*xt-2%>xHP36$-1V8d8-XAdLqp9~*5%-|d{SInoG_q-gUit*w~Q{ti= zk|xW?zKVzSa*q5u>RQHNwzL;h)at{$1_0q9(U@uZNk-B~89|U$T+b$?{wGfQrOBy- z`j3#_!@R$EHUbN(2hoNu88!+lHdw&2-fLsnSh7uR;z2t=78H=g(aXJp-IEQecef8i_^-L>I2ig^dT*AXamw!wDW@d(U0~B%6U&QzLgByq|eqUsz z3c-ThBXwo|bWzu;wD<*1+tnA;9&g+#Y_`1G?VDYv*u2zpyzYA1cJ_I|ax$@ZG?X5s zT9>uxpqn2gOl?xbB;_l$N<2-vRvB zTwNupfdL%LK>mZy&I}g5R|U_jaHZ45Z{i_a_}`w3kp$Wx&-$zUmC|!kV`l z(>0hn&x_4vO$DX;l7Y3sK6bc_$@|4i&J`!BPOdm+f(MGn3|Qs{K^2tGd)8qr=+v@k z->mf#w+jy5dQ+S>qAi@sBIan#f5yA4G4ZX(CMGT8vEQyTxzNOkbMah&*&FvS1eoyj zRz13Y?fp}lBWGoY8WX9V66$0zd}`Yg%OFJVv$&~Z>IC5*xZM-Q$xdw){8YNL2Dq6H z^j0R4VKKB|=S0)?3l!9KkxzJy80( z{!d9_MpyDrKn1#aFIlf;Zn4gH0zXms0sc`mmcz%t0EGY^aXRuc?UmgQ)%P0eUrf&d zYX#YoiAnzkq?{5p$d14UFjoR`?sG&UEpW&T3HO)36G&Z5{cS$gO;L9NZbLyehWQ-Hzm2r0~M7F z>!$+o$PUSnLE%#yI(wpA9;c`-59|30agn2Ep_DY%M=4VrK@1z)H&b06RDH_<$-Q7E zCK9({*F2i)7!Q%%=bYVo*6cw|1H5tbnegeOE?luV8yR#6USh>$x4mU}s3iQ5u^n zq96eJDTOB<*tt1J`;XXE2nn^(JABxLA$pyN;_Ly5t|y7+SvtOxRxH4bKe-X@I-V-m z4THP}*iHr%IQ(nllys-|g$AF%jOK!;RH0lQRZ5$0|yXp(rkz zOqZ1k%#9DYqo*1||F!HUvxIe>t=qck7N<*m??v!#VSYx0*j&d0C`e{deVykj7HP^T1 zee*qpGNK%M9vb`nTvWd81CrSC_zjw(niS|)q)0&gGZjno4EmV3WG$GwNNJ|4!MtAc z{NHs4N>r@s5e2Qxv|`lyeYK}6;gP4v0~{u~Rx3OA77h{sN~{Ck&sn_l7RAH#cI;T5 z-YrG%n!9dl&q7a#pu{+-!CJ%KskaB30szjb2EgAB4OePMFeetgGLD*g3FA%=v2ySNG_jCX4-z=u(1mV9qxb-f4I(`Awtid(1;yiy9= z#XyW`*DPTG-wYrrtf1HjqhF-`l%u-=ni=*770h=4A^^k=g{PaI0mHmbMeJYRVIXZ8 zpdV}N;&>ZKW&_2K*p`4w5ES6i#+tvOq;(>1*5m+Vy9Yyo5dTu-Y#B# zo*`^vViLd?Bg!4wtWSiu9+p4t9ptZHTsI{Wm@vIuQa8>?(-XJuWa42blK=G~QawnQjzIg| zAxaHFIpas8gE+9$i4vikei#!uy2m44rFS(pNtoW~V$U!A->H#yJ>h<7DfVjQUVj1$ zV(gTp6HL_~f8=cDNT^cTTTltKUl+9})6Th5J@cJX8)sd9K#Z&Q38)tNEd|ZlHIZzX$FqWDKGrk5UoLUiVrNC+hv$sS z_c?|s;{w&g<8p*2?Fgr`c@|;agS)Eeu<5^a`Ly5d`UN-}I3lQ#OC^cPehFJAft@QI z4RZNkZi9%OFB^BkjvnpRrs=8P1{`cp<8*9Z} zjS&dM%RL4q(mOBp)zIu*h_(W&<(kl{m#V*h&7Us$4;Ns>BV%W(BZ&jqQp{HreiOT{ zo%f5T-i@{gx9kv0XK+YbF3m_+=X|=Q-|Gi42#$__i`r#ov$ersKz7QOp5U}5N3Yjv zTsiBOTcP$+^*!Qgo#5tG`cAKq8U3QyH`M;#q}IaQ420Q|VZj$}5q|#YW%%c_15O)z zoL}0T@(Y#W+=EJ%HGu_7t!zVvj~GbEX%J6iwyg)&l+;gM%PR_2#EgEeYKcGWg#V7Kl znN=CJcC=Zo5Su*NWm5dj|9p^1k?UDW#1~A0ZC3@y&cLaozrP`lfs-^y__CA>* zUesRLd*CUfhSg8fl!0ixD zH^PuVC=WwM8d9pj{BmLX)@KOB8ui}ivWA#}neT=x3jIuqWtdm@)l4D_Gk&EZ?8bS0R%0!DnLZTDVdGubR z2GK=tL9`$-gb|`gi4tXqHd;s+B?!@tUZeNUyXE`-{AF3oSu^LHeedh~+}GY4%^0%I z>O-qFiycK#vY}%;_d4eG`aP0X1Hqd?^gPnmAhdv9dG)GLZ76StDGYa+o{q9|1Jw`^ zT%+qd7aSa1_em=$J1qTKk$y7m^QvC;e%9_QWLibpdfEE2-&Vuc`T1S!?w@Cp@Y|4D z(uv~zl+p2XvSH)dmB@rVNAG2#)?cg?J#ERs)~@ zr#n!b9XXVe6s8Fy80`6!3Nlf1s6bhwwdf`BDpIG?tHU*|z@714Ji$aS%Qz5}o9~VE zWv&c)OL|Vvn_Dn0_V;rKv`|UOeyv*Qh!iW5`nKrdPd|#{^Rspx(+ai&E?T&aXDRMz z8-r>GsbG)V2<7M`UlsS1%37#t%|Wjt&^EO0$oyQIlhIKFs%&NYMVdv=)K_1gJ@E*> zs3>y`B-?$ZJ;NY?tFcE`0}m6MTWXe_tI-`h`Dj)9@Be+(eB#f^ep%M&ek8s-%pFd# z=AFP+{<*iL$2SEzPk$oI2_v;BUdO#EWckYS=1Wr-evgZmKBx(Sd`;%%E}njE8r8VFWHllG1GAJZr?;v6)&G{@sE=Y)bt48d zFph~$%iN8q!je+>>DB3}!z7}m@fWDfa(#GyuTq(1%~vp;^CdN9juQODL|-^%AdFJKxrtZO59$6d3(%<@2JuZ8?Mfb9JNpZeYhXwD z6-MlxxCIvqz1>Uks%?8vIbwS1?(plrN{;JMu4Y+(Xhuw!?e|=iT|ueuYLixa3{_MM z%V<@=`54=ze~xPTd2=UCoyUiro()UYSY_hFfw}z48J7KvpRomVp3C`NR^H&_YY6Sf-SWhCv z9bEwM)95;N;r(u1T0bUvmp!phg24ym<2BAyZ&ZohQA!Qg{eF+w@I|jB`j5|<46vU( z+Tbq1V70rG?7jTx8=Lorqcd7%pJ}fuzYRLJk#s+f{D2pGn=3RjcET>-D{s%EFpGKNlw+0EfAK zO}UG}%)wZWvh6_q;;Y{SyDuTGg5dgMJ$!9~I!$i*rGMweZ8A)!bS;+8YJHy!AY!1j z}p;a?&Y1Z90^o_NvfcY+<#+ij3MPJR7_5cJNIsFNJw2?{eYAI?jdr$S0A?8Iw2(fUT zZby3b#|XA^lff|lkOieMe#(nccg9ZlA1Z~0+4Hm)SF#>1$}T_ctj33@R}tHqbuc}P zH6c4bL&T%b;%A@>{McpYctND>N(;yKo45tK{YEnBN7XN4$4*&k`k>P;xK)eyS6i z^xhlWUpJbRLJ$7Hg=HX5NsOAMm>AD$Jmc0!64>sY;8dS+PEr;583%+xNCTOVaAK~y zjLoCG8To?8GpvHPc$0P)0+gp@U?luB@lF`N(WfJ&!ZF|2wq6ki<~8=gX{L=MOqVWn zy_;KFcAqL9V5xGW$F0V_oWAe=5FiDrdpL5Nyp#N>M)w|Gt8*er8nIzs-INb{SUJcm zq8B>|oRP9l>2WKG^2Ind@ znd6)rQV=-zkq&w%l+savada{Ofq+mXRvHyjg_E$9mVDSJ`dDGbFIa;4&~=E_HM@wE zE~b$Tg52Kv8nfe4d$)}wcnzqwSBn;za|x#Q)O+7)P82N0!1ru_H#tPic;K?^>2vtq zo5D=su~GW&=LWZWQ@Q`-zay#C^K}YKJ+qZRe=R1C_sH94BG&l1fV1QkxwE=8jy`or z|3w-4^aIG=}_=UZ?U@2&cSxd)~T9blGu-U*XVk5ff+hq zm(?78x&qwRs&)7~PB8Pa@RIOvZ;eBKT?&2yw=S@_(PbDxJOk?siXt6!Ed0$3R=Y(; z-^@s>uV3yk?Rk?Z{c24%13QIg(iZkKZ+}{Ho{gNgcm4|gxq%867yappgHL?HGwD%l z{Iv6cJYWv;@U#xG8C6$`Q-APq%1HKsp@6xjBExEJXK&xiJdL}DtqEZa`ix%Z zGS`e~bcKJNUfuNm^wHr~IF?#at0D}o?!bCpr0M^8LF6b%BdRx9`XwZdY>X{%K!TY4 z88C-HUF=H~?lZ-Fyc5&7az`bBpjg}RQC^u7gk zxSCIs3I{cA59~Gsu-51X%}6ufwh_}IY+>X0;Vbi5&`Q1gq9bgBXQjNjcwKCxc%Sxc zEF4yY#tJcs%r*;`_Dlv$lj2-4LR_+pa1myQOfgofdX)wt9ZZ6aD9=cn_aE;p z;9(?9iH@FDJ(PyWZVHzT;ozP1P2*@Wwmkf!4MbnnvG$Yn zUD+Ad#@fSYTZsY9Y}u9J95rbS|U%(RD-CXuKP-d=b%s?hfx%Wo@4T1is!sYV{^ z5C{m_32qm4@<~j)fpWWRT~XjKO8Y(jH>0<#4zmmvz39|Y(IqhI=7IRYC@a{iHDrft z7yHlrig@+?rp%Plns3grdLbW%S+9xaA&h}^h{t2vtev9o*&p+J2Yg1b8qg)Ij5iHO zKcX(_SqN?cH_Vo&F_h!Bw1?%jKajV#v1bomdss%LXsEGO!dhZVzT#9uE-d!Mq)m97 zyvf?#fM2U=UD{#;4q!_-13cAxcXVe*~*4<(SXDPJb#EA+PzNrZGafdG26&hwOq?1lxTrQgln&1 zCiQte*ttM00?E+18{Bf|E45?mJnDIT8YdR_5W%EB#+SlEem>C7*s56FDr zoOzGdd?8cfiRySi_L*{xLw@zWJtf1#R*UhhH5c>Cs566F`Dhq1w+oQAS1gHBt-dzy z+X@v@2>-%SN+-m*JpJWz4_(YMIjst2tnuT@6aCOE!aXLQxaiFB<(`Z1oiZ5p8Cixp z=O44dl~5dno|xsFae?XAU4!-jo}}dzE7klHvWUB;drAl4XAXA9q!d!rCLULobef*Z zh)70-aNhV*V&vwpEYE>8uuQk^<*ysK6eGr7($B`q>Q0-`6TSMt59oVqotIb;i^Bil z+nOAZzQ?aPnq?6#SO>%dLu*N4-js4UdFxyG?MU+3*J92kr4uz71x4!(<>lpEPw%kT zhB>XfvZ|VQ21TXg>JTRyGHnuI?|db1h-E?O1x8^z7ySSeBX0?G1?LEo_HCI)pvoXb z;ojFe9w5#n2sY>C^fz5{xm18?S^9e)%3vJDs2^c$94 z6mK-3dy+un+CdWxnd(5q{AFnN;hdUHWHVa>SBxQtxK!jnn_+eGdTLv(V!%pSqF8Cp4zVTXNV^M`Ma zYVu!BdNXzot?6DL)f30{sunVLB1Y>Q@MeYiQ%KIpewxvNnIZMVp3QW#tO?J93C;>r zUv?r%DNEYQw-S}5MtvIPw4;`C=smO=o#h>(N<1j@LGd=c36@;eo0Vb^Ld=sf{}->Z z%Lh$qKazI900GM=C74fIVjIcc4`cSL@{4I_a>~}{WBBB^**e>iz_kX7;f2&VUtgTc zW_2tmeA7%GpLtOyet0lG}mQ}wOWwhEGZP*?G(MvJJiP75D+>2MM|Lg*-2n7E`gPF zloBtxOgQ>Yi~MYMZQkgjI5^`Ov)?Vt`R@A~KbX7%@*+0qDO6`zZ)eww&88QIDSk)N zEsBS~A2nRV4&{gsMB9AY`7k=pN&5%0P>AiIH^%bTjDlJ6tb>8)Cuch5Rmh+^WNI2w zT;_PdB$n0hJn0W^AsJ5VXPd}$?*uawu(RRMX_|7plL+_{t0Qe_{12o_jjnyIbid%~ zh|oy+#yHa}U=$s1io)LZ7I%*|UF7+t6e&)x zYwa;I0bDtJi;bWa2}A+-eUJ6S=1?0&WoZ^*Nq<_Jn^}sV04-yoy6$UtEWp@sSA-&H zW+93-sI*naL?IF~#Q{SnjLtE%e!>kJXbW{mNPd9!MKIdx0=5jL)A*DKbx67z%$g36S3wVQe>7lj48SHfEcgu>rO6QZ?f4 z>rtQIIAkcqM( zRw-$8mpxkrHt^c^RfmQ?2_rGW%&Nqb!TJA};S{sqkZe;wqbyfJK#ZQudo$0OIQZ6bWunAbquj?^j?UA&m zd8a6mImy5p<&qaPMeBx=EW`sDa&FrfNUPO%0=O#*qaPFn9(_#X`)4$WrGD0MR??*8 z59!iTTbYW+@JL(t(OABpKVi2E0X+7IR6R}U`wD=8)grhZL|80^(MTjEr|5_Ae+-MV?62Hvtct{)^Ab zK@|^dm7Fbf4h9~s^6D-&#Gs;d#(y$-|3f*o3*U?G?x@-wx@z0ywrC6TL%G*h6CcbU z$oM6dE91J!n-jDYV&A)OkPsdPqgLhvUSE+*7MlDWTz&dp)-O1;~$(p!F?@y z3a}Kg`}788Q#`-2HSemoh;57J+PFi^u3s2tO=!ikF~T_2(y^MJp?e8 z3?o?x1|2Ei&y18~0kDk_O7ZEMmYj-AFjmNiFk7O)AzkoCfV(9?2{Z1i*uT^Px)FA= z#rC2Wvd1)E*xK$~^q#m?lI;~Qil>P=a&BcsaTn~ARNDKQ+E~oiay)uZ%+~K9DBfAb zp!=myE3$7ZzXPe=5Fk{!Ad<*3nv8rzO1Hr={^~chDcF5zxgxf1LVNC4pjgIWlYij` z+o{FPva#%EZcK9wsm@zyqkO_Bcd9%0 z0=ntkP-Q@QeicU@GYD9ZcGpf3NH+D_T~wGaj$o>KK|vz4lRJT$XzN!vt47R!vZO0; zINJ#eY~0FBMQZPH{XV{2*j;UKG4NbtKum@S9}?s^2*A!OX7=AJvL}??Lc@H{X%G_sq>NgRMdLzfJv4&B<|X(QmD0?E7I(Hzlxi?UB*geHW=a;W?vWISF= z{%EzcX}N(CdqZ3@xvWqsqwrUqwQ8)iB*ZzH{DCy!D%J@N#P#0C-T<6K|9HKNX9JEm zHyyL!z}mx8eSalu<#g9KJDw11Ueevo=*o|l8k^sI<9(|+7S2L;{r35?*^%-JblIj^ zmcLG!m>v1J%Vf~;TrhawD(Gm=<`H8xO3@^u38n|wE!cWxwWAJ&a}yM{N5wH*AOcCD;L@ zFRug}6f(|k6~EQI{?Jo0H6po`dAa6!peT5%RgJXpZC`)yU&H*NqaaAHX4z&_*O+Fr z^UL6C0t+D)L>L028l8)v*gLe@h;$zcd4Y&#*;VrF>;C~FxH89U5yX9ohorKTHPx15 zI{GG6De9iw4a7gyJ~g0(VY#ZD-QBz^oWE_Sez!fNZ(iTXly=49Uh+DcnfYtY{k;A0 zsL-#|Kr(=?@Oeo>MpeZ{t<>X40TmGp7Tjl=|0D+x)wn#DM+efR>G$O36WFEB1<2mp z3R-YO$fWM}@btXdNIb;)#Qorv(jU4pIEkMn2~UQ}8}Gl?yU(85K8v}jY-_1LSaoDV zg_0&b7er@d@{9n5?^eSV*Q$a3`W5StHc=x#&_$tu4!OU@=22O?e*2e4FqEgm{fFM1 zoY8ok5m%{jASabc9=U?%i!(Rkn*6lbZE}+OJu#^+uw>l{p!k`2U7InC$vZGo1#m$vIt63JeYW_Vvm_6ik&B6^>4gRrZb8{?)AP~AsU^iMI z;4r{859+EF@n%^;t0x@1 zgJX0J7#BkR6WuXQ{bdn_wK&SP0ycl2c>Z{I#x>pdSHEE0-RZBBIb$ielSlo+2ez2i zA6-maIdMJk{c!CN)4Odeci~5JF7pW#kcqPOoyKyO8Pr$3556TNMxQ3}owz{9z`?hxm5-2+Ww+Y79IRP+>I? zxK#Sg?$I7MzH@AW_=Qu~PZ zEd9}9Yis=rDS#O^4Z+~3i_q1J*ozkHSl8nv*rfGv2%*xUAZs<~Kgn73INKDr0YL>e zuN&v3v@CuQD7D5acdsH9=|3Lw{6oC}8=dC__7a5<=Mms$egGu^7jtb*@=1yYUyEW< z99OG<5u6IHO-C_W1T2bTv#i;uWtE^lt%R5ABZ`nuQnKcDQVUTUxpY(5k7HnZU^Mjt zgm2QMbwKf5PbQ)*5jnxMUrBOd_;qx`bi7bQedwN5_k;hemLxrYk`6xLw=eD7Xj1(!t*|qlWWfn$? zCSG<>WbR+ba?P7!dZW6qR3s7N>?hPy;*&di_Kd^2vCcDcQrjJ7e(;;-1{U`U0s*#s zV}AMB;E_7+Kse-9UT0( zK|fH=O)q?_+$dSS@J4zz(uG7#YJYP+_mx-`ckkpS3~8r81x(+t#?RXWr-AQ%Pvy4R zqJAKOdbAIn@WLD)`;>loE@VN@6$k$&?5E2Lh8z=gxtrLSIj2!R6Ea1voFz}0tZwK{ zahbsO0}+GHStVq71!+#BvFTn24MhZ}NL_#tj_80tRZ6lBZY&$f|GEp|*o64Eechk4HGu8%1ojmq_{DJ%- zwLj?Kx%>U`k|b?k*lkr=ku##_J0?Nr)H#4M{}#-DpvmW&)K1vG*V=o}1b*22j6NZ? zDdszo{fOg@mQENjhM|8~$XhVqmx`fyn6r)+i0jDYd9Gx4rD`*dy zLdDBJ2=R@*O)yKx{}fgZ&}daYPqVB9EV>P8F?s_v+OE$pFK=l*o>0b`g6?cgPm&;; z(p$k#9KT_y=sA?#$D-Xu$os36{@p0|icO4IUe%Yia4wm{dv zL*x4GJD6T_hf^WEi%Pi=TW~aOTQeFq+}Jokkc&+MCz^f#2eXfp#^~~X0Zsc~k^r0p z++;QemP!B082asF@tW3CUEhOBeRgh9D&em`%%Ea##Q3iYhlJ<@qtZhPDCaGjOnE;k zc>>m|?0&21_rwH(Cu=J+h>BNLi=VGcD})0thVS8UkMB64WWpJfjfaJ?TPf|68r{6K zqTsCKpRaH6{-=lyLR$8;M7$aJYmbkav@HM*0zM+1gTSiLQbAANf`$zmFRJixngxDj`~e%s||UgFgIq`*c_N$`DWSI zR~I?`pGoTg-v67v==Fr>fwBAn(05Oq@%8o9s3AXKb0WC$t zcI3m#2suBdD3T8b_ejQmmKCc2*PEBn1^4AS7+k{@T7SIu7$T9?Z@40oge99Wiq(3) z=mkb+wO;udSp=vYokMM?o2XU`pz()d{rjNHuI^E4*%HjNw*;fK&7}RLousG>qy=%1 zbCqx)17t>x&z>hFqnj0Oxg-4*SPJkpZo6Y7t>jK3yB~%FTdmBu^IDG4t%1igt|~w) zOg{oSP1>YoYtenxcI`kHx!OZ#9_-EEC#zfodZU8Di{f&^L@m9n8#?3izRMtUCGRi! ze&I6s`5}`rUK%`V8_ojZBE1J)EwhFJDsX@2BVf~-$JJB&~QMH|FJNNe>1Pk{~| z{*F+?USSMJbaKKbr!CZf~)-xJNREnqFO*{sL@VXh#QpUWWeGF_BGw7-;phX;+HQCY$JY^I;2%th2J&(i=*!8&HtMt%+BP* zKeof~)jc%?naWDz|roS{#@j#aplD#e#oQgAE_!-+R9<5226VBJ9P&Zx! zJEj02=inDm!SF%M=Oqvw6ClN(VFgv(pSR}+=qAGmlkDXA(ffNU?xtOVuYb3cZtyQJ zeZMzi{SIN*SI@GZKyDIpm@PSlz3eBIp+1Hkg}V|Y$e$qrQ?iybRbAm3n3{)bdak{L z*5H!@zyJYujFjiW4;n~FXP`(TVRgQLil2goN7I)`#brzrXmei%D++ukaFR7jeWU*~ zlmr=MJW#;KP-Dnc{rq0+E6Ub^+URuyI*J&~;2M^YAME1_U?p!Df8VF#Jr}P!bu<;W zTcZit_z7k>CJlF@w_Q1MBPS($4Lj&!lpXs@7VSB_z#;nL=qaTM>w89jdUnb?CM?^y z6S6C1Y#na-Z~w$RR*&h$I?;b2gCfKXgPbD1Nii&2+4@t8!FF$DP}FpDPD7+UZx84A z)LlJ%g?w2*GMOXsvu|%eS1|Vnsu(_f_c4xIj3pg&yyB>r?=(Tfa*AF|#idncIK3cu>LY z)9Xg%XXe|8-y?RY05WHLjA6oF2TV}Rc$1tSmzj@-LGgGWy6hmW4;N%~49gA189Ynj z@;FaB_8Roo9*@jVZg#xe#R=uOCjdHGJ?Z;=xyK5ia<|2PT_L{<%~2dIB9E~hvVE@? z;k)lh7g^(6n<<+f!f{O$jqT<}$cr*haXvGoGu4X~nO$EefmZ(uN4bIHZU)ky%ZJuL-m59$$H>PSDeB;V#lQSJa&yyrQl1iFcOi8v#5F?1 z57Rg&PPO$qCi;$G^n(odA#EQGAp~ag-QEntg~r+nH^fx?<9%au+6OQvh{)~it^m-& zoi8yt`Jn6z^ftH$lezUdiucyvy?IX!X5`duHsAgQv#4g-7q-ok1B=qQa}Wvtn;!W6 zW?37hZ}LW>EW_C*}D9X($tDkc(t+lNAzOAjO z#oPRS!Af4&4A2s?q`SJhL>>BEE?#VJZu!ZHDd4hWrjtM-(0(Dkn!+v*%xcqo<&%{E z(TflT;(~+HG~|BXM{{SWEL(50nd&=e8vQ<%{IjTVAPuuYCQ8&62^=WwirUac7C3;| z-Yae4I3pVQjLq?;uhDJMrAG=Np*OSLE|49s^j{e3+My~>`3L*V??G}@&^b%+47UfU zPblpP&ND2~SmMe9=cg+A z`dgz5a}bwJHS~kIw9GR7AM8&5hxH;w+Y{RN51CYmtH3+8pRNv?!f`W=jo3nXC!o{S z5nEYWFI;@P)O=rh32#L(`K1Jmk)-FW>(sp(?B(s8w(Kb;zYOyxU`nLHHE-d{*GT|f zG>ro$hEAV zllD28++uq1dU#UZh37OVj>xEhjS8g7nNfJr@|>vkGgul-+6;=u+F>KE`K;O6w;&~4 zw*&oz+uxBUE?7hJ;&Qc@U^kE58PE?`S)9TE@D<ERKYSLl?)=L zxyq{u(i;82QS&v<;812$K6e9o?fp2}Ow4}I#u;O-L_yh;mWHzS#}}8EOnCNtSnVF` zKxBbB5G}pCd z)9<_aPQYqdM2LJyu z=l}A`b{8G)jFFaJ)c}Bdw5Dp{fsBC!ITpagt!*HgClNJkMJ_}=t@8L2uv%+a$xq*8 zeQ1z^%6rm3JkkBpNi)CbWuIv&{TqBhC6~A9dBTYUvV0H?Eq<1&Ene6gP-C@>7DNV% z0g3en^MLQNQ=!#2;WJ+e&38RPa=ByL!-~4ae&JNSCA{&?+wOX+GeVmBD4bv!AO7HJ z;{pMz2US7$N=C5dC(K6Ovq}uD`)WMPI<%I&K=5{izQuo+slmLwaEK^(0tbqbM`F7z zqhHd_B_(4E`OUW3r{O=I_!qU5GL(6rD4NA%gV@&?3co5m`6pf=2zqX{(Tk{dzXXtojUl9Goh=e1 zT+OJ9Q^f`Wl1SXQh99PiKvLvfv5dCZOa|BeGC@|DCN}MqEMijo?WCr3D#h>o(G#9q z85BlfDR@~q?;~KXXKS~~n;+5<2?hYfOs8ECmYZ;tRwxFdymBc@^bOkj2d5(+BWM<% z$Tx+MHdKMcjoMH;*y5G3e?`#$?NbRj7j4MB0NQacl2)8th^8KOv(I$Da*$(h#D9@- zm8ElWGa~QfJFU|=N~u8|@@sV!>fed=EMeQ<%H(U4{nT@QS#}Z)x)_n<2Jp9>j`6JJ zuQshIJbv{5_W5GWHviEU%S~jJZ{7~b);kTO^2bbxsvLzq>2J2-zvx@QZnR zRF2Njj75-Xl>l)zkKj_+yQ4wBi&1zNg}*n5OKbf{ zLBMz($Bu)G#$;2A%&J;A<11Ft|8{hV8wgWukN<3Ona8`m`w{z0P|$L`eMju*vYoTu^UT5(f6rBf3~O-n zBq0GeW|KHfl8-t`0U06CRpj98Uxh|_m;)hF;Lbl+5%P%F3Bz8Zg{Sm)Cy%O z$82gPzo%tC$24$vZ=ymosMai6KY>o1`LGd2%H*R_$873x5PeE)%&C&aq{tM_-s#)m z$`lRngO6n<{1Fs@jNPeigIrxu5Mr(^FL5L2aM45JR_xtHvaWwJPkNZ#9ossNZ=N)_&}3jj zCm0ilT}7Vh<{rRE!oV}J!Mvc^>MR6U;k5B`e54;mvIH`eVv06BDeRv&OEu_cdj$}4 zLl99Ce_e4wOuy-9CvORH#LK;=i8tc1`lhpS&eA4SyT15AKm?gHUhWubI4Ow9vk4Xx zOO_5dVsKMmU*D~*qof3lPN{E8n&$ZoFLEr@-EnNt1YBIB#Jn0(ux@RCR*K5(QS-*} zCSll2hzu)KN=N(hbBb)(aNRNO7Mww^k$6(=6; zB_%@}j#MG|R#5H==tSSqG&3u44K&&O{b~$ucRCj3x}%Vy4slCJE-Ev#QX>|nQN|cb zl_wRB>VCRu&vc)SIyRff@fP8ETMYq6N;$YywkQv1Ri3SKa6$qc8AkTbiha}f@cx~u z=4P!l^J)*z4M*Y&vYx)3n9XJnXbije)HRod9Zr`b9d2&4H6AoXRV2$zh^)iwBIskU z8h_J%QXp;*p6=xjD?ZZbrGra3I+JfY1XfU74G*F|E^!3MiDSwyS+K3CKK{W)F&kt~!btQWsyUB^-kRkDqeXXj_rEEwuy4>~ z)EPRh3q-cq&E~IPX_}YLYxa}iF)7IWr;%eD_CZK9;s4p+j~d)GH#c8zWm;u|(WmGo z$k8E520*-=oVY`sz{w(qpirY&DVoF@6RW!9df`UAhEk|7Bp%O`g!Skmc@BbyZCbvF zEM-D@jhr}KuzWB_TZ{U_{F}~!n`mlrtrkRqd)@GigA*U)1d7ePOj*9(!x88?=tP$K z4n#U;LYLqCkx~b`Vj6`L)*vfft!=T5Kq7;rLnL&-zGJmdhmhyD^@Y zj4;AnyJY{Z&tZjqlPEsMM|XGG{I}KftY|@4teVJN8_%V}NC=4wBH^k$-Q5M<#*EkG z*Wwd`7;tlcRce%zt-a-@t8>TYKu9$&RUg@KR4rK>CM8ws)Q%{~2%=$}a=(j5&u)|# zVb7xFu=TG{OmN)uL8l<9Px>w{@C;CVNcyR7fYP8-=8F9w2SW3< zqo~xUWsCZ^b*97%A@j^ul0mLx=B;SZ3DjUoNu1qM-ob!Lb0+*coysK^g!Z=!{3>*~ zIJk*NGyw@;HI4(Elaz~lgx@tnr}$;j;;V$EC2-P*`=L|t7Y z83%t=d^tWemE&$|1w6v?L^jk;-Bsd7E=>_hz`85U|E*YU?< zHRexsLu*X13hjET`^^h4JAoHS)CBEvj8JV#`6QDlU zjzxC$WS1B41>P0tY6SE|Gij)8IHqn2q3M-+W`vl>;_GLMNgrb<%8`v3z1=~m^sn(T41~xAf>3`I4AeFCzR1&vH`gwnj$n2Po!hd;TC%EzyiGy1WFv;i z(8L>bc)?evmvN;;e9wtm7q1GZXLix~VR8{*+^|p~-_+jYKNcarB!%5$iarp1SCH}j zbe9Ns`@ozC`K27W2sLF$MFpMx_=A+5jp$18bj1U3OG=^9O&Kc{@C&w>~?(lLW@ zqGhPHr}LA(Fc{U%ZpkA($`6o)HqrjH^uqC%!+fPYr35*3E0|_`Hz7pJ?)@yoSb{m2 zB8pcjH^3>`*!Z`I`&_#@C*()*RE@^v`IT?qPv5(f#r!y{Fyrnep5|MWyZ#`!vN4ld zj}C*#{-E0}>#1TjvE7YB-ox^vHMFqobppS4%uEl@Y|!+j3aF zEhW03?a-T+TO(&ln3uxM7B$Oa#Da9dz+IpOT>5$6>8L`hd1y<6mW+CetlBnJN&x}8 zh6^#Xhi9U_c1m#op)K)wZofsycl?xTYuMl%*&pj#TzSDarl$FKf;OogiQTaQZPm8m zdibj`bl<;I{#z79lwfzKJVJ3D3x1p&uG2&@QchRC8}>T%$hIU#`TFuvv?(7cRcd69 zsk(Ij{V|48tGPZ!sJbkq@{_oBqP+rw9#sm-S$+@Rw~}n0tNc7w-a&yrE(<@t>c@(S;O1j*kkeFNwhb)+ePKJprYL?YF}m9l-BuHSF9j!4!0I_YA$^S zf^$?&jfc9jAU9W*#=H1xxIpi?CIKKPpkyYs-PhWVOQTtol`RQs*kP%ZS#oqby1J&d z#2%{)wu0W#Yr&jmIu+YSbb~Knag;JDN6wNJ$g)%1W`1^JaZNi&iA20Vk03@nb7UqkgA^Nmb*F-G}`XE`I|1q83y|aHl zK~!i#60^v|ux!F|;e`+6yS!}PUc{CrW?A5SzRg_N=&F48YwdW%N(8-v$lQ&)-HHY+ zh1@8@NPVP^9a+!!Q4O%{j zLNT^>cE}?X3pW<@E8n@Y2;XFjIti2GkT-nox4|;CswKT@iXUHFGUgVzuR)Ds1y5kDZgICD+6AyeYC*gNolAK~Lcl7eymlp$ca7MBfz@@QjG49l&M2 zurhPOW>Sw$h~N<#(~%y%6)ax}D=5?g{u`zg`5p!RS8Rj1gQ7*aUr(?e?tX-FatBXI zCRn4IAcSn2|080Bg@vx$2~p3XI}!~jw!qbN*!E~7=Ezte zGo!KN(vgKoBSDV={vL8uMiT|APFu927%a?|`UMl~BW6kX+MIW=SjVdDF-BVX1 zwm|}9@nTovF@NLGl5QnxPhyQ0DfYKv31{&%1`mlP15KLZ(gy*ifarb`urqbNayk`~ z!4*cj^=jJzQunkV&FAHkpp4B9$qBmAbPCP#_2ufxTT4au?RKrwSqB~164Kq}#$B2sz%e%uMd&k;^QmKNA{#Jea>*xg)ec{?XMsdR=xR|uO zwYj71a$xp$Zm7aQ+A8@jzh$O1=D#T3sk%XV!H(cZ6mWW?^rCnLbcM4{E^5H~koHb3 zELe5t48ys&qD1wZMD&{so8iiA*DyRQ9l?M$ByI}TH(au~3^#v7b&(YJQ)y z8CLk;=hG%2^KUOuzvo)vo{uZjTQAKCFE%(PUKx1#h=#(QU=#xiyKwcBn<(Ey<{(Cj zH!|6x^#dLy^<8p(%_Ol z9&KQEB!QNAw5`}XJ|%@I6Ul2TZz8Pl;2tA7+(13tg2YjSb!@J9LfFu*L^Vh@*u(?M zX80d-kC>bC!!%4aUQYpn+5U2BwH-!59ioJ6du}iURUr)2pHDh9a zSLZ$j%TFZ4)ZK6u$%AO%gd85)dw~QeVpaRALbzyDyK;^)JYfpy`e*z{PqM)clnD|; z%B`k+Kd>vks&kM&xq&nUD;kfe62uOTF)mk=Olt(>C|=<$R~>d0Jn*wrYu!&t_-5vHyu)?;SF>n)N3lI+Qfr>`jRfU5VV7hn4IV`NNV+l8 z6FU+kwj>n*hv--x*MBH3b$g6Uv4WIutT8T`prhU$mnsSWJrOolFrlr&rLvc&D|jJ8 zEMz`3>N2osHVY9gY~Eo44cN2Bmx>k*UHuf3yuImXE7ysdH|nJfV`oEikWs zkLu-u%AIbcTQF^%rl6p#pG^VFrz_x;;n3#h@n(cqupiXxzFFBhgXfI+5N3RHXZ0aD zP5JJ*U>_WoCQW=dVPymiuaE6l?5G7XL*sBfqCt*i6kSc|TPxHX%6qB-NVkU7Y~ZS1 z5ARYVoHp}NLc-9hHHYJQJs$li&y+NmxnBePQ#PPSHtDNlYu@G?jV)3=mYauC#8L`7 zBf0#Sl?D0&aDdozt`$ue#rRND2ixlWIjH9gi&n>_sB2B90@ds&ap? zi=5uZDoPPN0L9q;Yc`L`f*gKM0H`2oepp=o&^R|YXU0PkJ4lWE!ADLJVH+bGkfUK& zQ{pgz0TOgt>spMFy;1W__^5jJnuMv({(;zBF_oohluE@3y+DSrPU?WQL4%9?N5U0# zD!4%n;*nL{UlyVzA79lXv}aKhD#cJv?#OXKE~ zAlZTaLUwNj6mC*W-MG&Du&<*d~1o4AXczd(MY`20CmR?0XiF z%_3Pe3LO9S$=cI&W8y|A#yz~xAsgWvUELQma-mYxE|zc115k9>tc?LDl|XyjE5tkA z2=@@`m;41JO{r<-|B~in#1c4{Ki$<|MUDw|r@QACOUlbj6s<=j+7Gzq9y#rQuOQ!j zuX8yfu>ry#9IKd3udBmooX>-s>0hR;k-d;OZBf0br0Vgx#-nP$cJQpjR3}HWDYS<% zvaD*&jt!mk|DyrFu(2L&R|B*CdJ{%{tg z-KU>Fe>MZO8mFwzf8Y=Hhpd6+2A4#fSpi>~fA+W$hlWF#kaPvml+tX^Fh#fbF1SJ6?#jPQ{*T zpYlu_qS$>9W1^Fc&ZW>^k$XlP1*69=taFQK-y%mGTJ+p-N3xv@FOsmpm-j_7Q{XTR_;Fvj`1j^8>?0o@a6RbKxiD}vRXfUldC#F z(zyR@SHfYTLCZW2>{I4b{UCzMjbL<@OTgC0wU96Hat~#4zA-mMD%x3f+ek!s;>>YM zQO`hwSS1pi(|k*%$l1GlmJH)_G@#|NC%caETZiSbZYql5rwF{54(ybLn0(0v7ZuAC z?HhRcj>Cf_iuY&X*mc2+u$V^VZ9`S5JefhYZK*sQp)5czi&8p18Pl+9DF4S<&KVv6 z@ZJG>@kZYIbR6WBItyY?>skh>!u7qCKm(V7w8pq2>~5LG12>+hMH1zY;_Zl!bYieu z{ClAM*GhL%8zmOI2DTM6i7Zvp*0y|ZFFIi_y&xkbauN?z zyj1Mo=78IEk!PYu6WzLpmL2kM%k@uQik+me zJ0%nnW_;(@HER5z=UsV979JAg+HS?^Md`H<>3RYOu5m`vK}} zuabQULmc)wBP*EDbUTTR%JX)q$^r0@AZ_uPWm;pUJ*A+qD)QZnI`-Wb@_K~Jd6U|J zKTV4t%r2F`l7zwM#u2`uSeK)lnw=$xmU*L~;E}CTJpF|Y*T_DUIU|;5p7zU^)jLe{ zkDd%XNoQ^n~lA|^8V*F<;H5QS4i?HcYyTt{NS_B4pk>u-dSg$)h(&hp^J z%{XFl>)CAe4gOQ~rKP2f>eHK{+@oLnM?Qq*p|fj<6H7#f_9cOwY%u@xKh@<;7txAPRXZ|?f!`#O`GaeXml%JV3qZoaI<-*5CJ|mj7Szc z7)vIMfOKxgV;vP`LY9j9EhIYD z&B0}sA$`un^eNrs16)xW=A1o7syIlogRhnk&`7panasZF(05`}p}LS+HZ3Dd%F*H0 zYDMjMawVM?scv2i+kJ&JEU2>p4S%Cx>MtUZmOQBSYXDNqEQDY!u!yx0h zMQY4wk6g0GhZxMyp>bs8vws|gaHOb|Rb4={#v}X61Bzefv5Aa5PR_*SbGW{VXD|%7h+m_%tl0Ah$!3d& z8Lz*PFI1EGs$P_fBv~_>Dg5p}lFK!T;DSj`Lk<)1CDCuTTIQmErxKV{vvprlotc@Z zdTVqlbN6r~kQzOsI1m|(9hS-IY$5nvF+M@DD=Umt=+5p)Y$NMorM*vvc=m0h{-&xdbgHgLB~F`{h7 zoJy(GwcaDBtEUI~$f7Me3G-PNyZU){x$N-jRaaeI-ObI7ZB*(iaro4fT7>4o&z7^=K?++No6oOl{JOfTst&8#4qNX|VS*nPsQf&B ze3kz8?OVialaVjXZf(Acb>=k|dpnjB*`OO_bGkEKq~yIGi0aIap3~OheUkcI{5VD%gy*+HhrWk5vX>St9sH~`{0C)5?lB2AuY9N*#H=?yw!XB|c~D=w(1O|o|L~>qC#KxdZw(FI{$wwGn*ARi+zIaT;3g+u>lly{>0EEpAA2Cd^_PB-H9*Zh3b{?7#OiWDW$%pTVp;XSFr zy^eT*{g}t5*&8kLLZ;sp_5bPksJJo4b^P9T?1l!pa(@HyowumXo5+5SAcirn0zfQZ zJ!20;iOiu8X{P^j zx6r!OZT!%3I>JinbKDbw$GG&NQJv*t3f$J&xTwg#tV9MSFAzizcRS*5ZnKDa+_l8M zchb^Ulb{TICSYx}U%}||`xU|H=ITlnA`?pyjYG<7p{TFFBD|AX@AK~z#@TgO*|gfL zxwqQkO~h#cc_haQw;WA+jdb4s33-~axinM`r%h|ZMw*dNptmXsr0;84j3iZz=kF(vVrpLsYvhR0+d=wN$KxgB_a_9 zb%XcSVPm5JxN%pJjd0LjMWZfxLv>a5?c2rmW^9UHCtBKTc)*#OOI z1K;06@sS*@yI|3tHsI#s?-ubh$N2R}-8ge>WaFcbt5p)qOm5>|B%VD1@5_C`RgX<@ ztD`o$Uy_nme6LnC^_-+Xe!QsmyI}1vxB2kF=VDg#DF7O!y}d=Hn1~@#Z@;vFI-d&& z?P#G=D=RBMtQx?Dfp_7{*r32k8Ia_@lAx$SOhy+|-i=4D%lFCXup-U`?FouLZ`&T( zmYo*&K(=5kGc#r$Up4REJTdC3Z)|kCIp2e&aE4Q`XJuu%Y>&2V#)uktAMoF`hpY*M zQ!sTaD{`I1kz%`0W2{{KuG1Xl1#;J4=ay{0o4x9;hnon-Y+^xsvgU1oo^6j0x3jld z$L!z#^K)orG^|D7{Mp^bIjT`!KU0cCP8;SI_gZeW%keplxNZ^=71gZN`Phrjh!oso zne91X^DWzHfaEt~xkjZ=H+$E2LRw!zu6Mqt!c5%6|3Zc zh@whTn4Ou?b6@$$&(D8u1)Zvk?KfeA6{(dPah1qFiEFl?@R`aVXDUWSz{(C!Dc))N z9$6D9;Xm0MFx*=K8j2&u{U-H_puSy)RvLCuxIKKEH^|_#o|-9rced)Y?#`j# z+R)j#y5~CbPZ#4(xu(T48{G0*Vjaeb=*DEFR_!lF@qfdUd9M%y;TreBE_fyRF%~4! z#(xS)7&#V2mTW#~T=si7%YiK#C#{=fAJDOTXSxE+KLQIFTrttq)MWT^_Wki1A}W^8 zcA8~bS=n({m0rtfSBO=ogF~?Es290nTLz*y%KWSZo1Ny;qpB>I`FV$zE%6 zSOcf@A?NW9_2HU27 z?-WtSMUUF~ZRhSDuDbj|fb(n5mKJBXn)H8m73px=9O$~+;uKu-IRkknGBR@OuF5#T zWyDDj_!;`RYPA$a_AIzO3W2POYQ4DWEE3s|5dA9GFO}p}Q2{$IXFg0PVfvU-sNhkPrf^s;ZvT%3S+& zMiuCG`Q5u6{!IHzbas;^gdd1=vzs4FB^nn9={lR$G_*<6_;&8MdOPz*0bpEq=^(y#$$GJsMdrTJHNTs@@qy!+t%Nz%A z){a%QB>Oi{z*a|*3FG#L@44#WAW7ozjIXqNo{a;R6;A1=wOID+T#U&aL!0p6}Pux4i{AuRq#R&1$WrUy6cG- zVuwlzWIVIaZMWS$JoMYI9o(MC$te$e^@<6v9WoD{H&ylW1dt49+fG?|RV#oH0Ko0rF6XjKE;*gfPGm?t3vg`S|%a*g*PFft~=%mC#Z98SsvsGowf&A>lz%U>Dtp z*m|B@Nx&WRA#b~XGgh1eBh+FvHPBy*XBzsyX5W0qLL)#Ac;L>e(yCUZhghfL_0$2#H+FV*s*I6o zCw*bh+Q1X-Sod&-wSz<>e)~ zW)ArEz@T4?^_JDTO;ve$#@nN5pauZ6U^syuaGX5=y}Iw-^_@ZK`hj(-;KjsQ0Yi0Z zX_~Ohw#$TAiu-#s5au-&W9hCUzU$qA;L=A=G?CX53+qViY?PNV(-C3N>GtZ#9}vVl zzsI3?TKbo5C+qb}Pud=kXB&bVp8$&w4m5N8x^jS#sqh_-DY;=LM+)XD6PgsTA;Pv; ztVS|dL0gnnGA(#fSj-i4rHABN$%Pz^t`1k${BGGpXjoZT(lat9M30~$anpNbU=aZ; z4acVJ^t$*nmLWJlGoubj?D$pKvloD$aSXyR$L^IE#D4m}yA<$=1*e%RWMpK7IE?yU z+2=6Z_j1vU=U=4Z9YTUV&NSg5m-D}@+~QL=R_gn4-U(>pDEx}w?ExTuS-h6Yd6ev& zGei!OTOU>j-42#MjtlRw={D|j`u~IJI&^#kxDDX&Y(kt;{CK|Wfq=!0HyJdO*a_c= zWMzx{`^n0N4{nlx;~a#H>0F4u>h-rW?^Sq07_yYr-Sr7TDX{TVjr{3*&((^%p~nVt%qDnP0sn_PfD^^LyDh<0 z)HshSg=)|V;A7fPHv~A&s#!JKR(5)N3V3y0ULL$ls-e$`bVA<~MNBdDYst^gZ*Fex z?DQMY6jtwN&s{?qc{|c=VH;-rd%;qysNV)TVHBR*gZQH2dt>*yXSQ)_T^&!%=GPx3 zK=VTh$i{fLpPu+dIN5wdA=eQjM!P|4q0pi0-fg~0AMo$P!$TP~NnrvhD-f=zSY*fl ze89ipKmjXbYpWMHupm57Ztu-1D0T`|v9gB%WdUjR-#caD!iow-mo7Y3=E#0rj8KeI zQQ2rx0tiZu*Pl|bjd&{1=UbPe)h*lUH#c5#t%cKjdLKPay6~VgMMVtGR+a``)4+WR zrMSZl@7p->U;*6sFx7;~G?N_aQBzu43Ld(>9g2fCGdnw$%1z5EkL2HPP)+obsOXuc zF9o@7b+C0UU`qbGdEs<12jGxInnGj6E>o}JBm1S?NT_oLnMxYW2ck%JGF`BM20GYd z-(=_uGP3a?%2x=gGaq)q@zo|^oCOz!zHK5ZQp4iI@>=5W&fGZCp7 zNW_6wrrKi!AjdBCBA%Ar=VdCNyvTb**K%=m3bMue=*e=cUWOFfumE5k2Q8#;1L!ew zqwn537~6+h)8Sg%*o@q$`xVdHgc5C+!D#Q5OKO(@^-&UB`BGa3egNOC zpiqRh0&1qR_vOIs3ZWeXc@ZRA0Itd}|NU-lZ3VPq>!u$ak0HR;@A}v{%$S9R#f(EG zozRwMhId>`T2U}9_tEW3>B(>4&*_AAl*Ww2MH0g5*q$uV$POX%K!8eU!%oNibBZJ7 z`S|dp`g&f{Xep`hPgX1{qtz$JGuhq}DPs9->xaddExiMSFXi($<1!4>otfN;g|17(NV!||}YE?hFWh3yu2rdL+j(Gb`s0=^BS z5<)Y^7%Wo{K(Q%)y^hGOz5N|eC6mjzH;($10Z3iZF9bjPJAqS5D;x8(#<(wA%=>d> zWTl3!x%rlLah65XGW`QtY7`C^FRT#_CAc`#`SbNo(@*CoGwRo*^&UI;Fnm73ca(SG z4#~!;fc0c^c>Ly+vbF(j=BNS+pz`BD(dDe5p@uFRZ$4MR`o0KnmWme8OTUN01x%*_ zO9g1>RZzdjh4FG*)q>SR??a%*kL(AXmuE={xT!);G_Iq3xT#rIHS6)9&y97S8mn{w z3{GdqI8sk#P45@T{?8V?(@vl_+#L5|i{Bl;Vq*udi--bo6v?9WzdHfX^y|a}G>fFH z04>~eZ3TYYG)0TiX_^$v8!8#AUo;ObYe(UnnxNg>Od^w4U&xdD)LO?NfJRA)PO=(g z?**k`PN~b%HzE%yaL^<7$_~r{zaD&BzLrK5d@=!=j#)#U;0L+)8mY>W^xo(@y$|=d z<>lq>`wQ`~UPmD^3B_5jkDhUw$8>k&X}p$YcYn3ny%TsWAt$mVbU@YZ-{=dkCnXH9 zTW)MwIke1)Zoill3GqS88&LpS?#gOZ5TY}xyA_(n*;C3{Cy%9+QFSqLPQ-tywZ(3J9b7l z;OMGvZ`Ug~H-fzk=0V*|@!Df+)x5anyqhma3| z2xJZbM5fOGW$=1+271gx0`4Otdt8(+zFLu1DdEKh6W>7Oulf(EFPnKYwW5`pj-7ep%q*pL{0t3S41|H2BHOfjShVJR!yFP)*l zkpozGbqf%9p;(1-bAsABc?-8)&#!nJNB2_IFXV3yL@U_Gh#r}+lB@@2D`dY|*S~dY zdFQyl@EP!Q#a~UG|F$zgwv&{m(@;I<8n5dONLBzg73YyXsyCnbDMGWeZ@?2cYhQvV zc#@cCIoaocfkbQf#W2G&&m7dx;zRU&C%}r%m5=>e=~ZAb7KKsg0#IgdJn ze>Te}hTYuzH7g)XmXwvTFf()845N|1S5rp^Mooz%1X}{R{{0`GTNc$P-V(y8K0Wn| z9{_U%`rjA7{cdIPH|Xo! zf#snamS2;t!N9ID4{+?M4L$N&TDm{YGW#E3^OpCT)$_|M`cbN1?lyWu*Vy!gwoe8l zFArB!1!jn3jP&dFX&p>15sapnI2YzPni}$n6Pnq&mV=2V}6`+5U zY8|nnAly zNR^3QfZy{po!^#@hZX?s$W&pP_}#FIvLZwM%x>I5mp_nu@2Sr)UwJv%dMzP|W97R9pMG=0d$c&(jKFcO z)OI=f9swqWy7X|P^g&7PCO!SiHB{^PKj%p{kDCdIO(5>|aE$`pqOCQCTM&?SzmrHq zke|)}%#ec*tnfaI`irBr$Fg!S&yK)9fA#o%AH-sUGPQ_D~Ir0zFNx@GYqDS^mRU3+D{V`@H zYB%FSKQQF2kO7v4QD|Zb@PpeRIYh%QM5|LO%hJVsgrv}#zky~sC?LePzPtnHxXos^ z43uPG{3HG?PBx&he)l);H8kv3THot&?mG%S$yJ+q?ztyOl1$9ZPC{fhe@@TgfT6w! zT_L#Q)!@HFBQ7mN>H zv>TC23(kjxy0fK>*#j_+@d|}se^>$KYtA2NVf3%UetO-Vgg4obol!{QT)uzrIafX3h}fY$ zZDKC<(mjVie@tI8ikB(ack0o%>$7%cr8s^APt@{(8>M+V+wtuy!8Nv%vWvg-K*KV* zKHlJRF$1*^cqFWFQ-HOI5v^q0o1kk>@r8(ZG&t80#}uWTJ%kX{b&843B9#&52SLaZniHmJU{a9I{6&0yksFzK|IzMMBtzi0;mHIZN*d5g? zEsB>pk>&;A^?kUGj+%bjY$r*L7F#!ii%1r5r~|THQ4!@M5#%JpvtkCuPhu#3z%Uc| z*=nT%qalD>9u^Kc--_()tkuREhxUT!pp^=wQhc=~9LKw7;rpY#h72^4C^x7}&epBe9J7^QUZY zzx{HOy1&Y^YPk1{1j$J_cNwCu0sXgutpo=(IB?f0H~0QPwchkm0p!GdGFS3=P20O- z%RF<#1x#hlxCEFx1lTG+Qpu_0iF+}G1 z;mPU~6=gx}I)V|#-Jpfj3EUFEgM%bFWXt!TT;O+EpYzn%YM~H1XqX0-IQrt1(+?|O zI-c@x7a+O1wB3J6RH2xi2N2QDLhAwG(3TSwrc3;MTN`fwM!RFtpzUl*WXS z$P1Rvz}JO{9F(%w$d!^M*SbwI#-){9*xvIgzCmkBy8Ayf@fX%o0Ax=p*9}Q{ABb`W z#Z;sE#Z{j(r4z(gVUW`D@;8NQS_138$S5OcWohb{mtJl*Ur^zZ>X=9Iv($?~qdt=lLO=bFjdlx|PoTu1xG>gFS#mm;9{csm zc89x`jN7?jU$#_q z%-AY$o8}tIdv>gXdSh=8+$~&wA?V;?b~JWA+}Jj20*nN8sLyFT|N*_2$>FV(%-B4{C67 zplF(zn-j1YCEh#K0w^|iq0+uD8AZ~aEY`d&!Vhe3l-)rou;&CR(8y$aoTG3xw)QgyW85d8a^9K8A9lA-w<9n<1#38vkR&AgzKJg$XG> zTX5S^2#h(+sMChv7H8H?9CU)7&-V5<&=UkbJD!(+1pQADXY6dMM0-5kh#%gUdY#@^ z+Xfw-9IWLO+$M@bPysS~eEzKhL-`F-(#lTlDSjfx6^toY=!E}((?kqK$B)17P1WFd zAebSlLS$8KdqwQJjjmfIyI>Zx@AY&A>|LMG^v8V{l+7rMp2UX&GhygP6NQ~Gn{pGSqR|NXN6o_);z5cX2>5-NK{H~;3n`!dJUG#7V)bH4 z9szSTS27q3kHnG&WE$pJa~;^G2C6FUb71AV;;&%c5-@%xg#CC0Qn)3r{yRY%Sf_(KB7oU6Ox z+NwdCV&lSxXYb$J>Q~*93D)K3hqEv%b(>{SvKuc}JGDG4G#zF6-cmFyR*AXpUS$a8 zeB7O+ih1WLDgYs_`M#4eyL1CH-Nbu_hyp!ZaR+D(a;CEo86X+JqXils1qB6QHM(wQ ztDXD7kS!>2&(SnWPwKMUUemyL76nDt7ypa_e};^F1_oAs7OO1_xPNw#MWlLnToXjI z{6@-9sV^^k_ z&5~2DTmfW+r42_g@IwidZiYEjb7Vqj>gQoD#kX&NjAw}o+AV=(5g>TvDpFonhVqQ` zH?9()9WhtHZ=wpPmUYHoGhs+Z`MDg})FI3+f1%l&k)N{xUn(lM)njwhj|up!jT1{h zeLDe0aMy@jo9MtW%mC^T@E?U`7NyrOav61#ro6fP2IBC!)OEP(jqmPc9-y_7!6^S? z6Ljhks&Zb_1R|@Pobtf^Lelf%>cyYgiYJYg@|VEt362gbcp)WMZb3|nkxjv*e!Tuu z@-r$=@6tG(gOJJpc>$6~;C$*KeV_?}&*Gu|0W&yBJ#UDGwPw>XAaCna)uJf+ZgByh zBu)J8{{Hu9l2KB%Bm&$Z)DVZCS`71I@S02YZSINU*2G*?w9Xus^b5Vfa)w5|>juUl zzy8?0TZpuGC?VR;?ss8$B9QO1$f94r>=fIOT^ZK~kDAa=ZQ_VhszfC2Q3bs*C{qg}a zft zFq-c3l5)`L*rLWlQIIEhvKp9VRe_1L(^(|v{MJ))eR>b(eq^ETCdDJ)x~;PkgBvC- zGx?Ru%3D+cZV)SP|80?ZjzS^mbF&j-hW3@8h6Uh4;diPVIxcfx<-IT!vbJmfB~3BO!hB0UV~Ih zkV-_i$q+Bs(9{G5qCq>f(31@oBQ`=0Pn+-c9j!hJ@@`~(_%JQHvJzop9wj)!j0afa zkOTkcSM04(MNGKPZ~7eYpJ|a}UXPo7j(9ikg!n*YGGhu&f&r+PsirTqY?r~1$t-t$ za^k#Ya;ruhueO$63Q_FfvsB8;s|Ky4b?L;=jV~56Thxi$vS*}9k(G)JDc3%}3GxZS zaF6$coxX!%eWA{Hj)9I)3e*e;!(_ro&!k~GViMnk;a~yXz5sYIfU6J8W0IcRxH;Hi z@x7?kiVUppm<5*atdh+ok7pDklLXjikl3$%Kr=;luC-n7qJ}S)nW4DfhJwIIJ=V7< z`48IUXD5cLa3wn2VKcQc-{Rqh1=Dd{Ewxhh&wht~t16hz)@Hy?rOOfl9B6>`pj0R= zTi5=YL>BQnVR{o2fbLMMC4m{oAtf_4_C+p?TN2aRd7)xX<4bS=7)nr00q<*Y;(?!W zAtKEdR~t;sOI1uV@VE`;Efiyb=T~&{-OEqUKPWttRoY1H03IU`_AqNQyr$jf_wnyhxI#2`0pBp-bRN znU%(o<-w8a&QSfg2P8*PCy*T`Ep7p}0u22o%XzKvW7PsKM>J-0{TEVc;vytCgsktY zad$xIV0r=%{9AZ4T+#U9c?jYtmXg(zX;g=e)+P+T^XqF~NPmNtX)Ydna@68xiCW$m zSHmyUM^zYlA>_Yz&rsX-EHCK)qp!70D`(;int6xRxDjyvI!79pUTE2=TtT(nM}~i$ z&F_mVaHdU8>f}fC42a-=U4r0TKeycq`b*~#IxaYYfzuungXJie_d{Fqi?DE5=&um8 z_xrE8r@@4Vgxc`V+1p*ujKY)OGksHr?;OCp2TWU=U)1Qt3;WfudwoAB0aY54iw>YC z3i1(o5*jnA+Zf1b6jE}!38_f5Z#zBXHIIQV7#9csEE)o4MS6VB^tyvI)zo?^4){3N z>Qd4$N&%lZOJ>JNmZcVb&bwli-@D-`FOMFo6sB0*uOQgR6?L5t2}92g0sx5X-#HP{ zEe+m7fhtydIR(BAk=mCZzA9>gbOyE5fe-U5WQ)9E5tNkX^^3o4;dvTm=C(Fc8#Qnd zn(`@vQhpRAeg~&AU@ZBsA#XCH{t`s@t?E>C#jpfgseB3`W@d;=&Ev)HA4~U!pIK-F z)9vzsDx$8cezsztN#Gjxt@iGz#4)g8zbD0J1)0T2i)qlQKH0ReJ$OaeWn`BlhVSRh}K~>Ww zsiiI`KRDU)fmj9s23pY*PKbKr9Ze~%Smu6HI2GT}ADk7u1&nJoqSq&O8|Dpc93!}} zL_q_-VTcwsJyIQg)Ao??!|+QZD8b6?q6GzZENPfX{cxCXO5uD;%$y7oFRJ!{=w<#1 z!&L@>hdRGyLMp+?%jvJJqx8fGC0Fa8-Y(-3&l+E`Y6F?2W#Hn|GMz%5 zgenQdr7EtBaXT=tJ=T=1Ma98kD{A^hMb&}0BM`NSag|qmw|ENrvfy|}ANjY78yJS_ z6u4oK>9UjVpZYqRXt-yj@IoKth5H8p(4oH$97N0aguuLtintfJHJ@hv6pZd7fVH$# zGU+c_^cejD;P!~QE3pFq_wWuF*iUI5TRIcA}v^pjCsNb%tG^7NB60$0` zz0T|@dFdUA5t-ZXA##HaT((T)9x3kQQVT9}7aa3ZKUp!4| z^jOi>#7ZNj?8VrbxI8a+Z&dX3l&8jn{X6v~k9u&4%L4?1i&#duKI*jPg90pO64O`elfz*(EZ4zu z^F{OD7-$VbJPe3bAF?KJ58W{E@z#hlRkcZL;(oJXWMugQ0|Wd|{?&AFCOA?`=RQnV zmnEW(zz0tjHDf0~!PLE6v&fiI4OTA*EoC@=PZ>4-W|mpWV;Z=ZHJsanszf`T}Iuz1q zLX^9;zb?Fl&wNF?nM&jIweQe{$!BjyKEmzR6=^EiVW?hsTXhq?v&!P;>!Pcx*?gFd zg^NL`$5iK~1wIBYgI~WrtMGozzO2fXpQbvvAb5*nPq6_;kbREHE=)=m64U@jl1STl zx2j-n4~SB6P?)PchXVUh`LcGgYyUYAm}hZf+j0KVz>-{0IK!^ z1`03;(P)}G=HK#?jm2Dk-M_flo~mMw>juDv7w0Sdc?Fc2rNa>5l@h>UL-UN!xXBAZ z3ghzQ+{l}{(x)sh8j0oJZHx^09WOQ4OgTVp4+@?XhR*HMS9>wQo`5O_S+o4|W&HZM z^_X7x(^*M*rQ>7hm)webFJSe0&e7zH1C?(GAOzl@c<%W}{eJ-P6Wz=;AP)9{@IoSO z+bIRIdFyFEwYj!XSM8!2Qb5{c^-+z* zjd(dhb)aU&w9S%4o*(%OPb#|o`JH)MR|sJ zfa|Zw3fsemqa(w=$ZjK&XX&Ezgw6w8A2(1JzoL-$uwr2c8-E|#s3Ap6Sn@w&psEeD zURB@98Q|E<$L9dI4bRX2tb~@UD-&6r+Xj*smjX&=w-LVdsyCAL2LB7Htl^!(n+*kdy3e1zP3?EYFjxB0UPPsNv1pN+<~ua?0tKE7t*+@pJH3N z3F_0(hRKPM>OHssVoz(ms|N)@-`xH`3S9)|Kl1jJ`sg=*|Jtj&P@jAqcjr}{T-BR& zl-^gXUqTd(G(_pzaFc}(X>LSxyf(?@`Q80GuI*EOzcu)sfu##6F6eC7Epk=NPvn4i zA=thZXiv~%x{q?esuq<*)li6!#W4SQDSs#js@K*GKrh~j>ALH4Xd62C{*J}w<|fcU z>2P!Y&AJZERa)W6*^U-IzRKCSAtOSYdB;Kl1R8MHBck;wp*;lN;Z{`D&xHb+^2EwPgv*apKeA^?5**y0o8z$eXj{2y9lMWuvSn zlHT!B3L%)>Di@?nYHFsexm=NSd(vN8IUF9n&!z{f{U8zXRa3*iu$-=bcDxX94d9Ww z^yMK8><&WjOhp`^>~Jcit}hNiT@URafR3Kl*o_5Tp9AXaF{Z7{6w2GY6s{%_07&kP zkdyzH$`_>*(1{$7IQRil6eX5m-A15G;H^K>ga?-Q5Hv#EB#{y`1&=c3q|UpE?VoKd zTpiT{-}WNP6n_978SoijJa^_n9uoZgj@!D%;$Ph-Pd;-Sn3!bkq0T8hCys>n%MO%w zfm6&9cA`i8`B9kCUBe09C;?Ic2`SQhTV7q9LPHG79>}ubzw!Wl<=V$N3eTsjBj2QR zpgYZ1>xsrn^SndWefFvvqWy#*qA>rRN{&a<32kvoFh=_>MImUgdCqmycNYBilKXDs z$Uz|+1C1X;hX^A>+LV=%?d^ILV})Ym>fmaS)s!heC2p8zQ6dM;ZLD`>C1ZBx>Z4!{ zSEd6_8~=PnqrqeaD*dK*G7pmWREjHV{`FYwyu2fWH|pjtdGM$9(Z9dV-&gGH?DU(w z_5fMhn*`ax@vJM*cTg3lYK8t!IFWv>;~a9;1+JTd=@14{kWicg5IoO?X7*S5M595H zgy@i|5h8sV{|n>h3=J0IB!EU(VP@JfL+Uccb>KM^OZ(WeexH4YEVu5KS=q?xv2pX& ze2FeZ3_)g?vP7ZKr@c5VknXIthBg<#J8X|Kf4I(eAqLh#YVcZS6%>In!4!q0fnkP}@f?GI80 za0bhj6#!O~u<~4jzQvTD(c9YUQ*lMb&>s(2dMO3#K_co}3m=BmKOK%d0?y6!hyy+J zkSaX;z$29Ov#$*$FW4IbpOgJgf+W(--5vCVN$}*}TT%bioFE^LW^dVPGAP*PA(apZ|_KN2Xfv8Wg%}55J!jmstEm9qDGuSKCom$ z#qK&;QI6nMx~8P0@V3us+jvGk~hED>r%W$ z>`V;*%|CiM#$%8KV@de@_)+~AP$H>`a)x1(5lXos$1?~TuH)o zNGgYu%_Zd2QXlIF3LfyvqZM+Xlq<~3vjQj@mMiUBajz6|9BW@fy9#=-f94v64gGnO zw_ueGPLH10l7uVyy0i-{{L^>vkH@e;Ndej2#l`vl`}fX`!`OsJnVy|w&J5-HHJhLu z0ASi^7 zWLT^Gze6uN&bl2CGnI!Alb#dbsGR{*s;-^c`nwZ+(Uf<70h*5#r@;^sw(O2YTm}Eo zp89iyTr{R@@aQBxK~m9^F6uhG;r9IbA503v!KtaDI$pvb0-0-Sx*Sh~mOq5?LS9KZ zt^RK0*(3rTk3Q@Nb?KfA&DvShB!BEv^@zBq-DM{a^kpkbDbcyPVm*OZ#X^z~y|}jp z*?a?m#|NU&#)ge3Ah67C_f}pBuK-55Wk>jLY=-LB^z#5#!wB%$nFkyIUwZG7C9bX@68;!Wn7i2o_F{06W&U-+gE-*MOP0#gm#(gFz4 zvS$8N=s64pqL!)Kq?$oVxKN5jxnRVg*3k&o3xM-5hTZo%DF!mcYpbe8)((^dcHz^n zHXa28U;EFkacz#3G(2L!)Y1dJkWQ)I9)z{OvT~6+jXEH@jhPE_> zDPJwQe(eMb75{L*G~bhG4B zrE$O|GCM))dw^>NAyF`6XouP7Z9-a+T|iZ@*QBq57Q%S!oSgVLIfG~JBsawwgc9{O z=28FBg<>_bdJp@ zXX(C5OOTpCo=tNg;p8)Bszdt33|AnbCY_gJghyPNdn*cIYgKNnaL^V(fhi<*Om4?`8@d<1rT-!>bvM)Zl^i5c*Fbz=TqK) z*fLIypmgws_ysA}dqZ`X<(*~K6zii*80o)ZWkp`7Nv5lMPa+3Sc1c>=L~Zp&+FAsy zSfaBCN#wWq+Yd2h>wd>S>^2EF=ohA;rp}eF>09}FdbNZDW$Gz9y1_RkKHpv$Dq!tO zgBrj?45lZIfTdEn88JabcAGQkiiqNZRi)*S75@Bu1tJVa+gO9@?<-*SZYxRGWuIC=69f58CInP?0s^K;2}3i; zTY?#pfh3i|AtWdyV2Tp2;27uos+N5@$XTQbc5K8Z(T#4OS+C`{6Naw+7pDhFWwotT?#mXzNoh)SWy1%r^2z2NS97ajKMUna44UhWp62L<}XP*03%Y2H&p zi$QR|+QeyOdaUmGqhpxw>y)@sBCOP?8-_Evq*#}RbQL$8Kst}c$R#*pp4|~Mu!eNW z2;RaN}h8s6KrNRHTM&;E-V4O2m4a{dRkvhagXipM$W>#x`R}E zBDs586MVB(HcOe31$iTNb2^HOU2uUgAMSwCYXV0#MZgN!>wPI7RslPKV#POli^qJ7 zLLh0C#K{KEIo#|gEbhQeI`>uZ+=dDM{XoL{8w_tiL57FXt|_ROphNNoBDXK>zSS!V zL&+wY&ee|z<&+v)mHc*J1MCFe6P9r^RV;sG#44t9387~TUy4q2C!pU)@Zz(vuFpVHT-1CXQnxVlya zTP^aCT;JUO%!mWX9TfI!@;WQI@rd!V2G*QKz^@jxt3Wy)K1Xsuvc@$~3xPhM)vTv? z>P&2+=+!zx_^`sbp_&rG7xF}cz=iFAwE_JNMe}+ z1fXl%H1Jk&nH=xp%AHW!Fhl*?ck=Un&*LLS`l8ZOiM?+(iRB;@Lh5IbiU!DPz$X)k zaDZ=W08OJ7#KC5f!Gi^fsF&@|E;%s0pGr#|K1nD$+ppT018_dnq>k>X(NlEZ5I5{a9`j`v|deC_n~ zU=0Kib%vh+wYc6gxelgdHF8Z^akJbb&uG$;v<1jd7wY3e#mtcaQUwdg-Z?ky=eNA~ zGvVMC=2l-I^}}A2?zsQB*T5O#MAF+yuTCuxiw!=;{0FK*0xAKF1|?a^(vBrcDPJ#TJS5rJ zr^l`a?23@PAmd&20#^{*BO}#Dm>{sNJJW&$|1&(MYY4>q|0!l`V$hK_fz}Wi3wY~l z8yjD(B%A!=DK05_3Hy4*DR6KgssrBE2@4Iisl%g@?uyKS68>`Y0)%B;Dk+iw=L=b2 zh0@BX!hK`(rFh0kZ68GS+lI%*xmn;^NiQ>LgF)b$_m?&%?fSN@yVY7;ZOMzG(c;AWu^-@b>Olszwu)yIZo@43z|`pS6@AjfYIDVedOl9z!DpCfV<0 zrhfBF5E6bNxGJwhZ%)_cKoMmu^B($Wn5g>(mV>eJ%&M2iY*~IsPe0iJ_6h1bMni6f z{WRYMy7lZ^xpj4v!L?6}Vw;viC_C{2U6X6jFR%BZdxZO4>i89ydf;WkuZ41S#Rpw} z#R6|xXPWSpHk6Y$-}x|6zq^7{<5Dszt1r?%L;Whb{Z;rA2=6iFi}%r59<#qqe0EHE zW6*@3jATRM3mpZl|A@N3yfG$ zF7XvevFoaPk)BiF4;yJ=tvdV`l?BkE-JUS4)$1{sIc?! zU{bK#W1ksne13o#6rWFA-@CEx@lqh=w|AyBMA=GFNha!w(qoTx^|@2rxJ1})VPVMc zCW>OV2QWu?kmHtxdj$VzM;*89r41B*sb4Bcsv6a)Ed0vD7VHzDkNXN=O|2fIJTGqr zzMOCiwet3yL50gQ;cd(ug-$HOzd!2S=m@%ik%he0urMY%2$OGO`@X2QSzz(%K|vQ;C|U@z;ct+PU6M3D>wADf-%u!mmPP(E|Nb`amDx3da7KOkQ86eo~mJ%I0S5`uXFUYv;;>17JphCY4wj_|hmX??6^6%p^mm@8`2Dv4i zhTodKA9i5pI;d$=pNKT{aaURK~W4V7Qt+8%ii>TERlL;5speB5(Zow2L=ZeDT!R=W$W zC|_i}d+_&=DRS$Tc<0?LGfIiBHoF=*5|H-5`3GPx!{`Op!HzH`Teg zn9lAEjg79S(XUOwa`m3O_vf)8+tKAcjmdj_8S2<*LH~&{6H~0HKGlDrTh}TY1(+>H zx15)Qqvv10Tv|Eo^&aqYb35D|+d6I?m=9X4ZOsrotnI4G@*SRi4k}rGn_P|3 zAdv`0DEN9wIM(QkQr#2e|G}K1lBdNH8rx~NQmUgc&s)aBQu#tf;3);d1C$VewzO}9 zqe9P(o8-oEqSly9Po6%N@|UnUz20uNf3H23#49mT>{sSjWJIb>s{P@^gBv(6I9?Og ze^dG_e?pRzKPLE5DD~*ogw)1>U`FWgliw%rB{sf26uuvWZ)dN{|2VENrb3i6!5z z6lS1$3L8>nj!+rtdtUv;Hs)H=o37p-C>|1JL)MNGV!Q4DZ^78k5Gjj4+lnP zn&~U82}j1`S>VR0_tk%YAVTZK?TQze{Y(1PEmSb}k2$xTeS$e!=~`UBb&!k26GX_h z8fS_s>`_r_R^q@EXAxG{o?l+2J<1sN$qEVzg6@T-v-57kBPmg+gM+^L#a3B_=N})V zqspH@U82Zu^!IlW$;ByI+S;-^AJ4r^OI77|kYx=aDvtj00R0TRz9aza4KEW+&6uvO zGVfobqRh(XXd}Z1$`B9E#1h+ozILG2sGg4|M?-=Qp?&2?H-klBPv@dNPH7|pz{dd zKDyR8{@@$KQ9ULKLx0eXd>_Gx2(H>kiZ-?thkxl-DAWecGca8BxibW8aDKDaIBaN^ z73i9nK;iBjDkTc^jYzsbN0H5O?ksr|EI5WTGxs4=V>|z+#|>9VEm`17Rf>^`uITD@ zfUJ%{UQJ6)jkZ<~>eGY8_7DcY(Km{`j?MpEBM5r%1(XaS+8Zf&RM6CBI3#DY*dy>V z=W1wGBWC;l)R3VzqJwHzH7YEf{v!UvQ;UeE+@uvz1~0@a&y%1uO$`I|V$MRT<-bO1 z9v>T>!^t0R%rpJ@R4=MuYVkRQJgN&XF;UdgSw9TXg!OoqjDTz4ub-bE$Ty|Dc0ar^ z_kl{yi&JNQN(q;*Aw*}hYlBVR560?SeWk9UrNCp{G`Vh4tP(YL<7T3$d|;b67^Z-5 zRZv6(XD6#~1q%yn|MbEtndkL=>y~3gwv#r(a7Q zCi*S2!j7{{-LciMeK5Ge5e&D8#{%w2K8sT?z?2;nrF$^%d3N`uDVqyo_dRXCauRo4(GSPo;}px1xA-&NM(WfNa#Q>K|fU-{oRl*%}vlvs}4VixB=T8WuA$hqEVfBlYo_;W^Td8MAji@-mg z-A5wRlz(z|cKi4oFYGH!L6tlb2fd$@rkfbiWV#-xbsDD9c==-j$@M-9x$jXNO|u3l z-YS4E2P003DsWnD^;%r%PlStT$W{xXt8`g@ea3f&Momra^YSvnZIJojL&Hg8uMo*Xx-BTjy8&9C`eF|Ha?<`6b;H{C!>_LVd-;3wEBs%GCEIjXCp7K& zN|}I>Ti3FRr=n{i1@rgRHB__FBT$x!Sf6u`MG`Ypc?C1dpi_7%w;er=x#X5-e@mHh zYWUmIac^ya2I@(x+1c4lUrcJ&G>1-wHz^*p1$9aA+$_2p^6?Pcj?leZyAGAt&CQQ91x%qo zB@R@}0;VU&J#zNGp&95K6bTBhAH)dZ7G1ps_u6A45n9%}FINeFiV2uM@vlPMeL2#X zKy^n9i{g;QIoo`;&gH``1g|T>`zITyC2GB5&d+&37W*8(+FF(b{jVTJash4*jQm2O zIhf5a65{i$G#b|Fc;wcfivAdwC^f)9Lp2TE^p;OC;C5YiXn4j$C{&cXzOyr6T}5H! zHU!;Xj>v8}_M-Z#C$w9b%8Q&q+HwS+dTh`Q4=ZH#_4PPU*>g++iJUP3s}cOy*AAWh zi(E2>4|kd zgKFEjOhtkDT&UmArE`qs(=>a$5-tL<=y;Prkc=r5yoPMyqzJw-pMI#g`S|!ex$e0? z^p)VDz2QYfej5AhDTgoCBX3(I2qMPIu<5-8og5r6)EEZ=HkVbNHBj4-CS06mnsTeE zu_lbs7kMQX@Nau1lAUSUB$iEGoZXf6T^AbQ^9rcS*D< zd~mVBo+MOLgK7r)rF04J7p5{JpZnd=iKyLC>2wy^#R$PY zd{4f0a0<=*By)NeaQqn?8hXr`1k)2*2Oz-i-h9AFw7#)%n$~(sQ?{J?IL>L9=iMni zr=HFS&-X6Yq@|QLD*0*Zm_A~Lo>6&iv^?Zrdr)U=Sl`^`aux#)rf|va)9vu3wrPjC z+){MS6U`#h$or-w^E$ohkt=)hQDF~dHCHV$4OYtC-l@!_rirWE=9k&|^XGCT>nc}& z7&JjW9dk=a;_Y48e`=F)589u$g1BnO@7}$G0iWqP?+we`MIwZEzb{Vdpm-?i zAHYp6eh!rth&jNABEX3|Jb{cy57uBAO0HUM&ZNZ_d7SnD8HL6dXNNS{*w+i=;}NWn z9331&xg{tOLig_dubXzl_tK;s8cyr>OZ2+mbbms2UY}dg&i9Cb)D1#5mo3y^_`W$ zf}0W8X{461{6X1*Mbl)s&+Y7IN=3v%YRb9u7_Ut*EE*iCPwQG2Bji~h&Ib)ltQp8`|iFi<^;th$pw-E4Lg&yyh^ zcio&)r3?>6)l*9-!avI#TT%)kydu%53+$2=fw)_~SvMjt_-ODfyjZ z=xMeU<~;dnnZiTLQSK-N|L&VouqXGieqVg*zO&v30v(T;_!M-@OTDtET#3*B%s)m+ zBmI0WN*jW$?@({}J!vdetybN9^D7bDtGX^ zk((1AE4t_{# zC&tL5g)g!HiU^v8%}7VQ4l+IUc}x*>JUL|(75tfv6`}YOS z9kH8UmHBDI1}gOu!>@+^t_+!-cO|5dCg=@q&Zjg6ozCX+m*(!OHhA7k#!gEt`(nBJ zVS$HSrDig>ScBcoZ(sRA{1qtBujGUgc@?^S}U_u(mH*@{b}@=`%gk{)pptG3R&t^w9OT=^hHp}ECZpI6(0 z0%)Y?3zf<8c+lE5FxP@~i=NPLZu{EIilAoCt71u?^!$>xJbL+Y=vkKkW^SW}5k@8> zPMi|PU-2{nh*w-EUOmQgDMj<+O%`|GBvt*|kQvd#C~E&rNsh8ahA^dBHOTna51#RP zvdKT(r3Eh)gQ6-vVk0a%4Ua#q?$=G{DlN9%{niR`Kp%^;%({E)g2sqil(n2W%=|`b zn(OBmUYY1S3;O!>DPlF**+Ht@{LUkPB3*yhUyVG<-gj~=?Q(-nzgBN6_RdenbjA+G zG)Kp$tV77`Zow0d9cpuw!2x#IvpFv@c9tL3=)P@YIevBQk#(7irHD=2a>8P@2Y6K9 z9-mzL6LkPx=<|)AQK!yNOrW3>O`CeN8dBYDoBl|%MYY%RvUq|*Ja-!;%to;eR<#O* z+}-;Zs<6}f?{F-fdAfAiWch*QuK0;#RF^2CNjSKj$0k#hFU2snuT0!qb1Ug&6%0Tb z!{bz0n~nKcRE4yyp5i-AVQA}f8@`3D0)bUERT}kWKmz(;s!>GL(*8{YYv^}a_FG(! zw^k#_qq02tgCGmnDoSL;i5)-SpGJI+3wixuE0D$dn1P!zv9eYUmKdQSb4+s0BNm=_ z31>7nShw%X60s)r!FFHp`qyuj2d!v+iafaXd797ES$@({pY0uP1T!9P-sSZ~IAMfb z^FNiCG+o6fY!alL(CWpQaOXpU7|A4Ob;eloJSt$Gr6 zD&3;&Y}q22o!&}OgoIhClmlx5*cXG}4JD$%D_d#uy+PgAxe3T{%t~XO!{vCQ$vRM6 zMI>E%2*dQr5Y0EmALe0C&+w_`9dZr!g?3oA1qs({J~iZg{$8RU)@HvvE=bb2?pia3 zwulM39Sjm(rp=Mz(jA{NMrk(hssfiF{RXHy!l5h{j zgxSU4brp<}-iQ140kQSZ$Be%1ffEO!@daKZY~46wx!ZfJod%^S=`Q5t-&Llznu4~h z63oqq(Ie$->Luzmhjth)iv~Pr*R=dBx;WR2>F4bSqMhGYxOc>-O@Zbcf{pWnm1PVPTepH_0?{K&c6FFK>L%x+4h?f znn%-%dW1Y>o%Mi@@h}FYy6N6xnvCMNx-W8;)R%>H(%s0(NyU=fuGf8Lb4yWrn<^g=6%}RA zJw<%`mpbfY=tii#NFS)nz7+uN%+l-)3Og->G=HC0b8qfO@zZ&Iq#k)&Pl@HUeV+=v zMegG}aeR)da(aG&O|R@uI{J+t@IJ0LGzOdG5|mBxv2^{e^@g|Nh#nCC6{P=e%+I2m z5f)Z)APZ%H$y|{~ODWiy4iZ}9ZXL^*zlRJ(ha!2(pb@>jg#T!L=TYi!oqy&Q1~d6z zzMS^_raM30LP$tzV?W)?I*jrE(A+GXFeKgedLUXtw@)Uh|Ndee+^JvElceB!B*WNd zGlV97HCp^`thY_WJeTHxf-ryUC#9-W1I+F zI7;pPN_CQe4XMPS`I4P!FYUM64yt;B!5xf?=xEN*k1R%U zz^|*ofE$ZTpL-U$+QI2*g~fYbvpdJ~ZSVbUuA<1pZ7Y8JVZY7vm4XH_0HaK<<|o1` z1Pz_3DDY7fxPM!KcI&Ym-VjW0T%@l(mohi0)#%R^LCxI-nIiuW_V-n+z;VZstk(Pd@SaV(9kobB zozsHUFwd7&KI$5t+kB$|s+qp?MOt^&(+9l;*QR*hi7q2}F}Wa(@pU*iO(lQGP$2)t zjy@q*O_N0TT>8%ZW79c!Q}G+-DlJF@$UBDx!*nBm_?&rADhjz@(M)DqYV~fYAS+AP| z8%Ou$3ecLrH8aq=D&LmPmLdEypg^`xY~xrV@Lx#JPP*Q1@H!Gr@*4Knt`Sl7E)Q>R zzLbA@6=aoq?UL%;R)oB+jvV_hxI}P-`Qf-gY&29#J*K?vxv;xIx+7fog_4C|de!lf zpx$=Sz@Mm67JX+dV?LDDBQd(*B*-&EhMRIEg(>cSiF>FiS=Tvj_Ds$CXmC5CUIKnX za_sM=`V$?^>>QEO3SUYNw$w=Wb9z7Z=7`$y+}jvU)~zK#2odC~$6& zSP2?IE=8jplM?sU@}lg`v{A<@cf9+9@`opi2iI=ZGb4c#sZjq4`t0{9WvcP3$jA*= z2#mUa&_}N>@>u<~b;MqN!vy8cHyhu-L1@(EAk?>AoYI#|iw>%4%~hpq+e<}^tv=T_ zuTH^T)|KP)?3-^mTtA&FiYrHzJ#AHA0K*Dl7U>ucGFqqO9NJ!Z?~a@w5HJzZycaHXCRbX1o&=kvPGFv$M3gVTdWy zuS@gMhcM~-LE@`apjOw}yN|v#NCGF{ypZu)^?2yw)phgjD>LWJcXxy#l(2hcw!U-Q zWt}`q7ArGp<*5_0#_~!UKj8W}z{&6PmuU9Udt1Z5P0Q|T9=XeA>%3MNCGBuFef|^M zSdB}7B%Y<281ZH8U{1<}pOQuSG+3@cGjd+lTFYRjfO>yv6{UDSjgW4$-%_bv_A&^& zC@J#x(4SGvFcP6r{GUWZw+0X3eUNXK((PK3yCH||6U=KQbAt6qq`f2bvnqs4WL0VQ6B~n2i#6E zn4WNxGrZicK)O~#4-tBwU{q0}Ti&u5jJG>8?>3?~<=_Fnp1TWOF$@57Jmey z9J1Q`wI_`M&M%VC+G&$4Aw>=-=Y?mHX3pyF-(vK?d}ClWr~JIU)OHXmm+c^g=LGU; ziL+4$C1+lK^V>R%Co=u4$$j8L>Lo#d&7nhuWlW#NatDq~stW>SO=4TNpOq5vmBDIc z0rNoeXTmqr&5!xrWZP)2cwyI5qoFY>&e`CQIgSTKP0(GFsf#q%{r*#H^72m&P$h(E zyO$%TlyrGGEqt+5%&_ZW4Lgz*_6SGN-VPC8U%9{x<@FfMaDp)`3y`XY6U{l7QVMcwEYQ z`N4_Ba_iuO#X%m`(b2&N9{s&ZT*PvR6U?E&1hv2$E`+~SSH}hjNbw-8{cGSnaYl0Nn&d^b<+Y$e$0&^R?c{ztR1v(pOT8`6uhe4^wGZA`np z<}x|pH2Fon1V8^S;MQdT3jvLtY~(?OF%OWo3ldic?@vG@k)V}R-BNf#rjN=N^CX&i z_z!+YY&2fUh%U$9SUltlp7-vapM4Y)2ng`~7(QheTOY!L%FB2e+-Ty5LS2HL)T_H~$Aq;6+@*&3wFJj{O@k533MMay||=roXgxwDSJ+`R2bw-tIPo zSoIx(h`p#Kx%MaA<3+hPM=tq(whq zHoj5;;IYySM?PF$UfN{1k8_nb2|OFl1;F#FqG0VO&~hEREtM8s(O}lj9;amJf4b7# z+zd{k6jvYMR)Q-oD3^jHE}28YsTcLS!e)T}F)6s^>`5!fAA<4)X`BrvQb?u4Z}Q#= zdHGz=>N668h_!*Na`%6`+s=a2WDlU{EXN%b0(Egfn7}Dw7L1jE0Tu%r(~~mo5*_fD zHi91;{*SMAiSG;;g~BBve=L2{PWVJx*Ipa4%Igb2rmrRfE+>#{C_sj1|Iw6_fR-TH z(Hi@STcLnu@ilH&0)6It+GjF;wl#^i;2Aqr0FZAF_ypDH!vWZal8oS3bXttkqA|Fk z)4zRMASNag)kzf&wuY&amPd!F&GA2&UVk$*W+dVlOYV|ZexqIbO6Zjt)P_s~K62#z zi2PWX1&zMHSjHdbPS*Y}3!wHDhsvv%k9=%(AHt?hLt{`g?9@y!R>U&V$Lvg2pto)V zcY6h}diG61dzj^*pK1mD4Hw8I_Pqf40)zgL0rblB^8XlKuuhXKnc1*d`MZ$NFNuh>t1^k_C3>MSq+#J5H z-lh2zmP9f(n>?kt5^F;AojJF!?)uyh5a{&Y0on?0NMG5RQzouuyr`ZESx_o@shC{p zDXGJ5svf27u~VuvWufmNLc98Bn91Jae<~hp=8HoQDFpPlzd9UIQ1h|I;5Z9HO|Lh9 z2708wrblw1pTj^!1vWQGvaN`!h%_@8#F8g1mFPjY$QLa&uYH~FQUT+45)(8v8#2& zfkOo8=K_pQzzj2mo~Le`aFl$FWhpY`^>lX!r|A2noiK53g?1++7brrb`wt)DAzm-J zh|%Yw?v4!ogORt>?IQM=H0bDujq3uLeyPA65T5k9h-nw+C>|;F{-v|xjmgq`9Hz!{ z=W?zwwNJd6WAp?|Xreg>WyIV_wU1qkLP0TY51-qxg~11876;Mfx+E(mC~k;jx3a(4r7zQrt|FEu^Z zXEAi*N8osVy{X$8e0>f|Hdjz2K`aP*rQdzf)%f%B*l3>(NLEIgn7Z*pIlX5BCviBC?O~3P*h4}{elI^T&ovB>M4@8^*e*j?9tIAWJ}q^K zh88Q0gucyAP~?juh;iZU=Ud&?s`V0H=`STRfvW(e6nNbdbp%c`DWT9w`cxMbD7x2B zBEyyK%08rV)P^BheUl+CoANO7kP!1u+)xdBdoDR!TU(q~Yxol^mk%)~QK2E{Em$j{ z-h~?;UYm|#mrz zf~_ZFtgc^??Te)FlZ}$DD=HvgoJp@8>|$Bp%WJNFuvdR= z*4AUN#=62!o8MXNvBR$D`jcbyXvo}ZPW+}hrQvhU-eJrz@{)x5*Q9E-;LDT+(qE7; z(68u1-is&%Y{kO_$7b_;l#*hr?SL0omb&EiAw?cGAJYp$*n_xv$?f(K3>}bJFqTNUzwyL z2|XWxvsRI|(;!2H0*#Ld{EKIQ>OCa2-mlFR3jX&XXL!m@jud1dSIDWVH832Fs34Ig zf|>8XJ+lqpPU_e-AhfgV&@beSgR!5QJXBbF`DeEHty`RA|TXA5=km>-O4A z1cl&P#cvV1RY4%(B`$IWoE{&%bv`EWuh%R>QT}n%`km1b4l$e)&!fcR{o_o@#X|d` zQ=hnFvA`BJ)(U}$losiLzPJzg#jo@6CQ(C!W)yTkf!X{Jry#}7aL_aP&18z)Fpy4qx%~gQ#CaIK!*S!+v`jN zUpO79vZW_e@+IT0<$?p)R05fTR5}bV@BPSyW{n^#3 zjz#4SV^IbZ@-K;QVv_kAST$N1fz!l9{Rur_^6y}8Z*lK*0Qd7(RaSKG2Cjt&7G(|D zuwbF7X7r49^9stSPD5#LE>!IiyBgfSs()3cBSC+S>J7tP(XHso#d>2af;4i<`bHw5 z{4p<5?50d8aiNmK01A2#tr>IMGs&~wJa4_OrDAex$23Y{+Zj$hgR`)}%le^Mh7g5U zl}&$qzr{F)(QgB+6KhP_KPkN1s6Sft_Plk(VfJ&G572JXoLUF;o0HV5vB4 zr6BqRAK;NOu*Q|7;*~pIcvPnrs1Z*6!N4T^1CtfFuFVchWyQ#=;Z5d34PAuR!_-zu zLr+O2`Tl5ZWirk+oY$A5d9J577wOwkB$?^ z%Kql9-NjAsX5(Rl-rZA_>UkDrxb}8aB3yd{&mH!~2y(~-QA`xqF|Myk?GR)6Y4HMG zWd_`;MorNEk?@XJkNVSZIlg(Mk#z0oRL=gIO+DQU+%sJ=0i-{jbwv%m(!@DE)EZp~ znEy8s^Ae^PKQRaO+fRBGAM5MupKzw&H5p|OqJ^xG%X<_>-bY(iumS} z9Kj|5p~hRGI~>byh)#Y75;RjxDBQV_dc1ttV=6p0bStr1@99Xtvty72<`;iTIgiSh zx~|@aL*_ky=_YT%as~TVfhz@HBFSGkz>!8TNuVp1ypI-oq>kzQOKgfU<1AN))&f4h zKcAqE_JVTC7fu?3j1MBGmZflt7}~hjv8{3juH+n$ERR4Kn66#`^Bx4}AhQ)n5o}ie zLlE}nta$61OgaAkV>llHJ4vV$4B7a<3_dQjK3ZMf2gB_NvpARhyHR6OrG3UaQN zcwVKXF1tA-uYD%Y@wXi^SH{P)sK=z@B8vO;{>if-1AXpQax_Aa(5<|vH?LTCK8_}! zx2+KAh>bK86cQ@dEEJV}#Hs!4*%VC_PXf!?{$bUFL`q7^N!TEukkGeMIj}ZU?`re% z{I+5=t-|{5Bt>+Zz{)ptLWw{ka9$~IKAMKG+y^LAaL!I?@oALH?pN5)22U?M0V+uj zQu2WekoE{tTfLkq{<&hN*^(*PXZAhiF^HEt1)lf+n4iVUQ;Ax^Sf#YF3=c|i^mt{a z^k2c&?w!QsHRwW8kmWV<&|@Ieh?GBt_AJqnp6P zqE5;wtZ}vOC3ac{BM$8m9n)^Ag2L57X%(*;>qgMTQ%*mpVU~>Lw=6)JA+dIllY~$K zKhV2&kUcoBoM7H*&Xh#$cdhO==N^{8wh*?)-n5OBF3G-CPGC)`bAv9v&%fKalplU) zuj!g5RNmX|#>f5JXkJlx&=SIf$@Farhi!iE!@zUDF6JHaP$CD7B$dut5xU$rVRESI zT848~-TI5NA{<>Ci(IDr$zR#ge~Ii0q#z`B-<+CgOh$e!ELPCcx~VgUlE7(mU*F4u ztvB#lMZ4+X+Ch-AHa{hIa-XT@%I8%48&Y6xRVZqxrIo;l!~Y_wZ>3Fo`p?)UCXtux zvP(sFHXljRsgG?G<=HEsRN(>Ho0`g;K7w!Q`tzBbEiR~MSMm60AG?ouL15qy()cV9 zm#>3ssUgs@NC;w!{^@1zD#F-Ul-hyHZg*Rvjn(6$(tCkmJCV8E*DVrIzFz$di~)|f zGUI!Qz2f5H40&yg93P7D(8|1BA-`u4_JcprBvZWriePFPDp7?Lxc8#T;s%Ql7-+fb zdGAnRv0eseMxtP+o0R5I%v0Yr}^pSz^# zJu}<^@$w10X}1qD3^85pM^lGb+7v;$uKDwAG6`3zDo+k#@)?M^(VR3T*nE2R;^x}& zV{mj#k6EpVKMTncfx{5o;&*7xQGFe0qhB4!H;9ge^lM>;oB&63KcJtYWw&7daN+xM z+Bz~PX|>x6LA#kyAm&P(Vo(Y<(heD+8owAHo@S?AG{Lch4qeP@Q~zj!=}(amJSYuA z7B50ej)6+L-k(vpb$i#2fS!`(olv53q`LA{W1aGbY` z*|G!qhL}Rs?U+@52Rubr6>Fzd@-2kJ^d81n0~MPQHG6Nv`p$U%`%z;xZnRvixQ3+- zo`)+g;&0y^=IwrgNVAX`_g-d#rqG-!192(eBZa834Q1<(JeoJPLy?D%rSjpyNxi(I zjv?-Nmk;pN(Fhs(nW`dkyu~uG`ftrs+MujKaOH=s=-R!ly(3OfX`|ovMA1uca`fTw zl$+K^-h8aM7R9pp*{|lMdfW4lKje1mdke3^1=SS2R5ZzKv{4kRMjJTIPb1NHtnk^# z%HY7knmeIE%EpS%zA~rv2M4D8bQT!BdFM&>;+h2Gn;JhyTU#LRLPA0Y8C>%I4`c5g zkM$eA|I1z(8ByFuviHc|dxnrz_KJ*bh3uIg2p_I+9GZ@72EQW0h$80@ z@w8O zMWz{ruZSV~Ft=OG_@w-pjO1Z)-G`gAYPd8*hs1f-zhh9tEV`M?x+2S7GASoC?os=C z?>TCdf+g)r0)=p@&2KSw@q?2jPW)Q=wqYZ@O8s3tlXFy=Ue7W4T(#Ut$ToU)@nX)+ zB4H+xTg0xNNtBq50b8clkQ&-k$m6ZF^*%*vHi6%CMAhII9oGA|aF}V1)CgT94lLCL zKe~t{xb$pHU45)Vhl0GrT^gGK)e}hJh#hA7XUe6=vnf`0o*R2t_)p2!moUIlZ-=% zP;BXbyRVHE=Sb1oVW)=9gXXZ%P}&sZkxt9k^V7+n6-R`gJQqlv7 z*hypecBR0sR@29JTeRCWmqjbql+<(IwJ>`~jiTb-HXAq=XC=WemT=_&4VZ z6A-L)M=(S^-onyLJox-7Y2zaF6L=x%-b)cFWSi;^+5)P#p*Sv0 zV9@p&q(;I6!;QMq{KJ zu#jA9P|#gE!EsLfhBg-Ju+%=w(#dDFhLA}w_BAqb^^4_if|zygR3_2%?*oihi=4DO z>1ZRp`@e%cc>l@{Jx6|i*|WRawXC|V+?AFssV6Y|_dNTRNKee%vGziEGkswF$n8hT zcRbHU+og6)3{M)Wp@4|CGus%nk<*(7WIop0bk?Q_@_w2PmKMW_Z7aQ=X+F@v zI<8Rox<$b`0yk#u_7ZiPgSHy&&Vud--GulN*+;mYPxij(tM*^>PlKwIpA%*niQgVI zRXnYKeJqtcCW!Z(AQbncn8q_F*tQtex8*KR!swmi@lR4t=H6GJ#VA@>5u#?(XL9Y` zA)l)vIV21FS`2!ol;F;JQ4z1)?q}4*(0UQ07KVoaN6h#ZY!ck^(#3-YGX*C^3Tpak z11YH^8yLZFV;_9bUYBB6`nA3e$C(sEvfE*lHVW;LE}>t2a_zGR)<6Kq?N~-N^vfcX zCjh4>O(}ZZtSEyz`YzMFxw@1#1y4e5?6|O|dtG))$xm{zYLeyi>4^!@TP;WKz(5uS zd7{BbV|I-@d%(|sFsJ%r+Ccw7YO#D;wbK8bz)YjwI!0Z2Cmvmbo5Nc*KbCG*oo?>y z=^rAJ@i1CFxf~|zN%O0)%Iw<{WscZ#Xku{@q2x)R-50fOnxPAo<{jSVKj%8S+ zP1tH>!lkTm#dFAb)S0HS_l2!3LkIH5gM?VDoNWJF>!BLiFq7BnDn`GIj9XvY%cYrd zOB`D3^HDAui9w#OCf3sp9irMMVQlnUp1w}PudCA4+X#4renQEDku|%XY#Iw5jboh*s683QI+bq&``7*h@pUDSfjs2EM4r+FAHS`SF%c587PCjI7b-& zQ?qw!kdnE2L$bNEbA@(^zFNapA)YI}VU>FS$q%L7snnG#^N!$*uu#h+;ThkrgsW;~ z1n3CiRDe^=I3I3|&A|=R;I_%c3I4^`JJ&uompPZx%y%kacAtX;Cr#AhGTQzOKh;M3 z;M%x;$;33cO1W?S$YFb7EcL6uAtuY`ud4w=8V^!!?4cZibH{o3(y5R1L!v64D_4@8 zf>chBy1ws-X#;u3^YUb)r5~8axEvThVm}5i#>e)0iiBFs;9}!if>gu*JSfy}G)A`HxXHHzE)B04iVi zBPlw!b#%*&aV@UQ;ao%^DoCR?VaH6t zdhWh(?o%3IL$f3Ut)13I!VRL{rlqR)X9-eFcZbn3yY6B(uJYiEy;{6@CDCw7OUC!- za}af`?vnRgeZyHSwi^ATaMMDl+!yTH=D#YUGyToq32%-i!4@~CaGPL=-ZVk5)$8Mn zQ&WnolT2^H593lTleYvCJsdyV%Bo9(_obS2tKI}-W`<>cJop-LUZ}_*x7WfC5>Au- znI`i_f7D!F;GZoh3Si=VbIhFjEY_okV~UD7BZ*rt;3-R}*6|QaFmaXxaR#c>W9FJ% zJ;%JS8q^P1{yW;iY%JW2nQYU5W0u(~zdJG*wdMQH(Ao<)EFk-PUCDVjUL=!$P{a>n z>q)yc44?JLoWdhcSngbj;J0TNf8wp@aaG^nbswRWIaq{~v%OcIf!LRI?X%Ni4N`?) zHau3GSsG~h1?xesSsti)ZOdAIuFNW>XfC0?5xhp^vY?VwfJRUij1h&I6|)D z$z8Sk#>L$$g9wKl7i$xCa=QOc2|{X7WcJ>wEU8~J2kEk{QSZ6MQDUCOK41!M-(Uc-dx%)3TR4s*edR>{a?g42c{3f}iTIosYgRwWzTSH> zwC1u#t{SNP-`UJL-R1Y8ek=@`>3%_|5$t7YO%-;Dr3Q(kCN-nDVw6@IjG@a?4BS8W zW^WkB1ddrlel$s^bL!ggU5*s0;(@gnNCyxVfd>y%5*U;db}Ymh<_f!~sEVs2To{i? z*cf(;y0^C$v}h?^$UPAMuZ;RFI2j7e*K{aVl%sIQg7~<@^hJCjeYR60J(1u|rcyU|8Guonp2(fsIBE$bPKQ#`g4gu}9jTw8bBk z)A*#L-0?j|&6WEzX|Z`h2<&L4Zdle_LE*(s*H1?`BhiYfCXhz3@fMR)Dl+yx;q5~K0W90MNyhNvVT@ArOl=4o7nE86QcH@p4+iX@BJRhGZT;m{lXDD~<7 zcuC>E=Zh4dg){gmqrZVJLuJ~7ZPdsna|R2RD7o=B91Kt2P~5n+9DB3)*pz!y1ztb` z>I?3x|J4H6Rd(om_rWA|^M1fJNhemh=;KU+$F`L#`u{dT(d+76)oy4z<=m_+EEPre zHNBHEeSl9T%AEq=uF@yn(XAuAjKh3~Q8&@o&^2=0Z7JT>`|Lu|)v@b6D82VZ-wwDs zAX5DADI}2ZIF75kdjl7H7!vrX21=+1{ZRG#(7TQe0m%MPlX3uO)5wOamzhcn9BA%S z4Y%D7=oI~^T>@hz*dHL0N&spDAfoY;j@%YnEshtLn(*Vgc|r2=SHMsg3Aq(#i%q291 zE0dYm7NnY2yw&6E-8LCUnl?gB2vfYtBBMObidxX%5s2lCe0{zIBV#27)W@;U$A=Qt z7=_0M(1h-B%U~hOl9gw$f~41L^h2HBlCvQt>=EakxG=YalQh)wX%TlZG1rIF9#wOg z*Bi0VUnaRRKEhyIAuQa&WJzzV{}|Oq;?6g0sb25!z&_`f>pWCl&J9~dO=6C3D2R!@ zmSS(N$%Zay--Lm_nU`Dz_B5zK==Gx-@Ae1E^dmAyN+($djSfAE+h>TY=)Yo0CmmOm zYM)u+j?meE4`{n*PXR^q#;MGf%voN6Ft&u~Hzu{ANsdh`9BAd}1%%mCsKu-U#37L* zNbFVBD8?KSP>blyZ{*`UEy`m$u&kAR#edrX+UaerlRBV>PgVgCP{4^Lf*XzZ4>5g7 z>2f4AE*GXFEPr#}c@z2WtxBOOR|Wm19oC!`6RySS(QDpRHO9(Yb?T(g^qkobdd3?DS~|L+e2i5>hb;C2|uoQBDn`Tf(_#jfJ@`MasisTv2yf*E*L(z#%@uQ zXtFi>S;(&V*N??Fz7dGc2vJWE4Bf{>ydN^JrW(H zI_*FkT8JSPhHZ92^j)FXyv3S)0Y4iGYs8_n80V=9c=SxLOxJpFvu(#NgB7pms2g8@!(rUKD4UZd_1SansT(X}6PRcLC^f=n)j{bA&9Q3b$WDFRd^5av zZ^@P_9)!>3cgqZdY;Jbt@Zxy=iR*aB8!vQbNHapcD_3qq-H*b4F6ZNdFWxcFqz@8*Cn_ z@ESK`Y`OW{Y@CoV^)`O~cQ|8{!Kilr{h@?(-E(cyGrOU8A&GOoW@V@?*^OdY=hJ$o z5Ku6<`!U??U>!CU91e2M5Nh0$gqP6RGu_l;%LuzqwrOvDhRAZV+{xsIdJrikkb9l) z&6`#up>OZwJr~oy+RT6FO`s&$o_K!PDhd_`DFO5n=KB)wii*z-_$Y7W ziH~gtx-~%@bV@t|w0C}Ye;B)>+V zsXdeY!wA6n(uZ0|UQ$QBrLIdM%6) z6P5bg(oJ@iXVS^MVOD!>of}!COU~>S#DhF#J7r3`FR}7AgMIA)BzZ_H4pOS?M+}PtCe+Y1mlIUxQ58I zQ&|lY?RrGjV&4~yFDUJ2c3pchW9F&2fEw#26|xb)pd2+|^-#7m+jhdSp9&`{kz5Qt z`+bpAjz%%v{_@X^`=%%(Ptd%uG@yNUMhlaqMn8bNBoGUY3VJ=m6`@-DbH4GK)i)ALEqVLq3L>k- z2pTUN+@u|~cQmqJIj_hy>?(z9-^R zlu!xc=Hs5ItE&T5Zn+1pS(W<1r4&xleE4B@mG5Mr6J_q4ob>5`Sg0(l1K;AL#&O;H zxR&@{rT(YnY@b7_l5rlj`?0)S7>;2$88`2{hgvi^Gt{u0mgTj&v;;n6mlBuw(1C)w zhB5dLeLTh%KNT^z6&&(!%GT>=XmX}5djqd^@@Sn3MKYjRBo#|Ffgxg~I3J4p29@bm z1y%eY|I+-AHTt?d_!f;J+jq_SGqrf4$FpT_TKQ{{rbU@$S~Kv>xpmgodWj7eY$=!^ z!UF1Pv&!r0u33mS?{CtLvdv`n^`<-O@xGi4?d_?15KJWhGvi+yREQ0!=vhl@jH?yO zWLC&wtW}!$=852g2OZjtBKwVZk#M3meh^Jh8Y}eH`ZXr3U*Js&^0J0%-OeC2CuaLe z9FRnAXL zg|U1tYiN9*rD^WhQ@vDHFx#*uzIP{cptG{$7o*j9aEXuzjhyy+^l;~kFUkgRp17dR z9rPBSgispQ1XMJz$G|_5{U; zFYU>lD{bCtCnEXY-7Ry9QgPy@ET0zV-z>jF-ZO@p#cvh)dyC!W>8G%HyQPQRjGR@Y zLIxKAa%)+Ds`dDSH!a#iO)$$jp4`5c=^Z-hD6=_*W!Zs823@gjWl=ZxwtZsNbvCp( zk#~sKkbSDDjqCM^jJ*cx?SH!6+`X-jYbElEZMAOC{Y|G^{``xtbD2Xl7u;WNbviGc z88EF5lC^o~8h-3C=b_Mdse~P;xkmqP;V%5!*MZ%~jB5-WQ|~D-|79ep@5<$?3*J8m zoLsYtL^)04^)n+m%y8yH3WXsOrWV`faw7+Iio)(6-`ySs`g_oWT#K|*sZd+9~jkAO<`;?8E35MJ; zAnAOMmq2j`UW#uaelzOvrgO|JuKfY(K4XC-_4*ud8Lyf5MMb-~w`H^=G@`nHXLZ(% zxSMq;VIsen7|tK3yps(aeHAH(r8qMBjm&X+e_>~2RlS& zH1$PUT@e;AWqsb$rrg-a6)>>aPPurM?TTA#VMjSgw5gN(X$TtLvND(~@rhd-yQMw3X5M*ItVX-CSGDZ6FHgsqdD9t1aIzMsJ zuKFfF)YOeydpl0?*mw2oh$_Qt7qs6`pw`*RAjNP%d&q`#71;Q%*rKrMcWHuBt}goS zWoZ3dDd^L=XAXm>g+??**5$BsSzAG+11Dk3I;S-SRTYU#=#4b4hWei`gb7|M!&_wK z>J6@iRgIs)e8#dC_wRFJ%G|9wrdt;>wUHY~dvCh0m(N{3UdAM{`bO%oEArXE+)TthR@b<%Kg@U_4-PDWgy*(fI}sISTN&l@dG* zt%cfs7kkvyaOn?k&P`0nMVh_tpjFeE++w`X_JrbjRNU(`EbBS+ASCwdy%t9EjCx3T zrl7`@jYBVLtLXR(pInqXqAaxD;Z~<+VfPUlNi^S_8yvhX8Xi0a%Yq9EUcFv*l@BjB z4uTT>_r5SFJ4-ktckjm}MV6sA6pt-=y}|WFyW}TuAhV0W67;>25ni%GbzT(bb>F_3 z<6E>wGlZnO!Lh1nAZK@ixsH#f&G51xd~c_vH~sGN|Vp6)z;bFr=5hC%@? zcA`mB#bY~wUEm@rtBDymkoGwC?DB6QEqFwIpU=}-7CTe~$fjEWuQ)^DI_S?w0G*{%n7bciW7*uv7-r&McD;5D|G{)4d;Wf-CbR zJFo4Y5nWQ9k`rUKQT~&6Yrc^a%&sszG&RlDXZI1)ppwLIha#wI3)9ux@^H~xqK;08 z&x|!4MqnW>MI_M3rie6a5PkG1n4~deOrOrlbO#!z=No(Dc9jHf;Pkl#QX3z zW59yIHG@>x-OT)5;)w*{%%F9$oOqNu6GX}Qs+fCp&xci^*v|lCc~kk zeWpg#R?qCX7AAD}%Sbo9^F@BVWNxoCf5-Z|aJO$SOE{6W|LELbWGb5MvdNQl0#B4U z^~6eDkV4lbcdMvYj3gteyCe#xDAx4#xf9%wJe@iKl@u8GYce;$C>|9nd*=!dv;btA zX6O0ZC35*%67{4To?=?+0Z(hsN6=Q)@Q`yt*3rA~z}wS4bB?W}VuIuA>dINrzLCX0Un#R3df-F7;L|$#)1eigZ zSE&p*>5k-6+3gR2ka39p=&_1@S?EkN(*;wZINy;%*gW7^)STbuXSH8pRHijCS5N~ccO^5uDK<@2Dn+D4A)L(gi?A21 z9!d!<4}LM`tnS|(3NZ4ZC&g~RNuZI|XPFRIiMUlhZoakZwq(~vwu;}m+(vSfb@W)- zz2*Z`;7mk?GEW4Vm_CFgK*!3K9*Vj=`~F{y7!|kJpj7Zkgn34-fL^!%<2iG88R$1c z17k(96NVZIR`vR>48#e(r6 z$s2HFxC}S>f4RYf9}LqTG|ii04VunUp5O*pE0%q6yJG7GSBXA*6Fl~TAKE(A1jFlJ zNe~7v0O&}7cd+Y<7nT*G+w9qBOfaXHjJL$9msAn!pI7~#1XAn0a>cvu$TQ|qHoHo_ z*TtNB2Tt8Nqi$CX}(4uTf)_x zLIIYMATvYjFw_=uXz4p|y<4|sD%Ty2B43B2AM+@mW#-Hc@fv;J>aXz4NJFvJx$yXc zZu8i?+W4Ixz4aaKDxJYx4c&@6?lG`roIa{7?Lzgm7Tp)EabooERc4_5Gc3pgJ4nO1 zlTv^7v3jI1#}puuIHC_&dRZUPh!hla1U@6}RYP@BTUymz-XI5ICSB8-k0XbN%PlaO7yhq|{Q9eRU-=X+{#@P8vB zsl(Xy6q%dOue08hc89bdBao^*lLO&)BysA30E-b9>R#1Pa*!8s z63L8-zXXa_c=9{GjWp z>AddwrwKyOs3Ps{!90=q@~@v?l>)wP51=Q`Lc3~9EE$fux8E5CCo!;OX{aWk58YWG z*kMjafCO(~!S{FEnUKv|TL55(u2+7^{#ASjV55=rdYNMwDfTv)JB0B(!*A!E<<%nS zGkpxOAtv(Trpj$i4dx<6F5#yLFR!2~2;Qz0!xpo4$Geulm+MA}j|oB6$&F&`OOfPJ zvFl!(T!>g%t)`Nah?o$B5yq$bx?hwtM&~3&bEn#C)A~`A!Eg3^WmP6$Kie`Pazb}n5gF+OJA3%oVv%fM}9dKM6C*RA`XkB7njjzRgB{Mi0) zKotFe=K8qP<1o88S_4~m@pm!2RN(#o<#KPf+0*Gk#&aj9{jIGT@aabaOTfpm zm|XSViv`G|RiC}wxbfTm?|*xqtwHrIf3`p9ngFu%8_E1E{@nw@^RN5S2H1NyUG6p8 ze>1kgZCjSr0&D_lb-P;p@ zGFSi2e)|?kBm&CGB$h|lK&V5u8Q8r#07Q!nWqV#f?*UkG`#>+7vFnW6rd?ABYU~5o z_h@e>RrDhi^zE8hmQhv*am&P>%{-W|Nm10M{?4$byCJkRFy?V#0<9b)v-r zfDRb1K)a;D6lo6OT1Td=$$`R;LUDH5+S-`FPkMGXiU^{q(+gOsDRKg{I8G-mWuds>Uu$_OA)zkkjC z2ApZ-RtJDpK>U`2p)sdX-7dUjy4NazO~DI>{9~G{C9KkY%KxFaue=#PDffT2*=U46(x4><_N8O=uyZFW zE4hAb5GBCJzdEaLZ2{{>Bn=GG(q$&e3uW+B?usCRaE|g*HYw|Xe;_~Ig7}Ns$w>@F zR1wk2|3#;%%c~K)@J89%ZuSH8zMgK61oI%ianyt`haeW+nNPxf8^-B@F;&Bz2DOIs z05^gU1@n&j97HxqnmYOFGvdkMcTPKCxlvl zG^&S-S+-tvini#v?A%aN(zkP}?U2Lq&HQ-?)MjQn!I%y?7}6-SHd(*3nq^P~+MPRe zbgF&pY$ZQ1xO$aNmTv8zyS>xQuT(_G0=KF0_}M=htcPEb{058<^E9Qf<@2>PTTQ>Y zA^E&>2!U9sRA;9C3zz5D{=UhVR~Lh6e0?Q&{6B;x{H}y_q+!w2$i0Vw8X!mSvOU2< zi-9n_y=K<#+ByX@H`-Yh4OOFKe#zwD-oYwt+Bz8557QbjEw5k zjlJeQIX#6K43vM1Cjb6?Z`*(V?AbS0gO$Vc-rn#R+5%JrIW9evRiVTY5S0RngxBTG zd+FjY>l_#`^yC@Ykg=cVhpWr2%J&pn7Z>N}eGitT&zUYAEnx-9vutwd7JAe{c^zV^ zhu_K3lZKdX7wS|O+`&C54cLI!9$FRKfgdcGLu_VYf@D^&pZ~s5S$GtwKKj=dq>fv3 zQk(Ahhid|yziTHd7r>}NyTlEENC->K|MZEF&JB3hU$ga{tsd^~e}64r|AI&TV8`s; zHF#&NLY zC;yl5+!%g9Z8N#SCbxIM<1`s_! z^|6xKvt-*h8LnqV9bX*3bMm7j_4!rHap)x$uk>Dn0BH#U6bS^a6n(B9*)W1<0WJmn z@@L>9;k4WlQgw(I4U1Tn;m4=2d;KItM(KdV!nc1ln#{q9^x#E$aNrC;J%|$(1NE#4 zkLlkrgS|hnmV^@LQfGDGvbZ^x4^Pp_!$sJJA@C6p=)cibtWxfzmP=%~AxM{VRg*ur z{yv(P(PSP4hlb6rfPWY;swo%&3nyCZlj!k7?1tZeuc><~Sj(_X$!%ZiYy$rJi`ek}1e@F2tl*~~qG_yOnxPsI=u+noELoC)i0LDj0E1Z{QCLb;~x^4h857{&l z-ba>KoxpXU!>TzAF+9Q=d2+1DKWgo7KllahLcpTC>6N_7n6fk^&!#P4^9PMR{I&sD z2w(`rfG{SO0b`y=u%5%e`Cewg=YM?#Fgkb=NM0=y3PKnp0JQc`o8S}64XW3Hw$>}h z{kQn6R>XdaJ?>;4c0dMY)DrIVAYwS7BWtDT zDy9KD4_L2>QD2B{2}3JNL_^Vg!lz~-T;hJ%Z%ql7i9x$c1?XQ>tnG>7N7d*vBE6t8 zJ;BEwhNFcPq3#%e9-I79RFarEXdIyq|H|YGk#G>^vj^dVxqs@g2&ho(a4B>1{lPB= zpO8?IJ|0dJu+Nqr2@}iwe*?)lQ$IR>Qzv-?%%1JDU4mR?0VIaYwRkb?xx@wc{?*TT zMTGCk7~GU=Fns}_32ci$!_$RKknG<`Xnvnheb@4C0Cit`^)V;FNK zGvEZ$0wiQxIc1Fa{6r)RJc557oMRW~{D&Ct#?8IP!lCn8c=q5CIAF`uG&-aBpb~cA@V=A_HxqK^|MaOX2oK||8k_FB)!tGD8G0TZ zrgEqWAG`;{6*Wdc|E1ceIBF3bOKq?{J1RR$3AGDR%Y;)ECo-@kt3YuvNxjdt$nVWx zJhEnXjZu$!uda$@nYEqMVnQ9_Yr5J3ZA9R&qUeoAym>1w)6BBEnr~`+y!w^* zaklt4OJg_0XAN;A(^*DWYcikwOnk^{^1!SE{c2I;kD@GE4Fc_cdZ@*J^mQ~E_EDNk#`v$E{&0rU!LtUj6~kw ziH}pr3JH)|9Sp;Lc0C4osUpJ{m8qVBL48fh46{!}hKU(2~m(va8O?w1%t$2UW^gWk(>$Qb~n+<^E6 z@gH9sjM4k(D}-Sd-kKw+7}-K9I{=;O>#>qGR1kckv8|-S1fs{*q)iZS5QpC<;y~M8 z?6uW=GmL+r&|?NTmkf;b1(rEE^z^R2D!R_KK(j=wxm#qNz3hiuJ`M4?XH@OI1Gzw# zpbSR{5JA@V;Ow;&#Hf)OPV}SFB24YqT?;Uy2$5L^Yl>`MtA40{VwfIS4`()d9h#rJ zdm@SI&-)atqu(J36Q^n`^y^lxz2l=pla=X1EISxH{m27}jvnp2d1GNWobA8@)*Y*1 z%W~DNiQxCoX;mRA%*S*TFI^g7dPP{H|AOBVB9{Eaq#K%|_<7lvFA|jA+Mxi1`-71hAlFET2B0n=Rm4k)1*Hr%)}hG^RJbrh z{CY^^r7#mwn%07Y@h5$0yoe&7kwZ@vl2PcDsSB-^Vn3Fe<<+~OnAe+XV9Vnjg)cQ( zB^^Ih#O=LH(d2pP^;w@?ruI$!EPmq#JNq_K=lk3B`j;Lz{um>~t~RHk$pBG`2HQ*M z4rqi(gDIoTfqQ5p^F!*uo)Oxdft8P5bViexNaRGC59q_n+Q60(mc2U@0KsG!E$Ig| zVc@?Na?+MJKnUR-%SLtA3bZcfs1$;$>Gz$Gc((T_jV*1GolgwQG*$Z%Mux|60<6mv zhgc@ewrRk}5(Xt@UN2)E{Inhxdh{RbPGymq7IfM+o;Z^si=0~$O?)oQJ$HJfT*Qg` zQ20mmz9@_n0X9&>{faBBw5A4VLiS9=ftwuiY>_kam^C@~eBbhaXm>-9Sq!zOJl=<{ z9+>_~K$K#7YqxSipDEiRmd)8Z>E=U=dkVK6M3}w*W_|%Y(Fbt8tVj)xkQBqp)?1LZ zK5Ke`$KgSLp#TUh3W_^}4qX^md*d^+u9k2RxQ!G5~2Xb84 zE?D6Uv$S6I$Pq3yCvEPt6=$S{v{$|+#N0q*0xX9a69Xa7$!^K~Fma%Hcs}fHK-hix zDn9b4M$WB|!C>2}gTMOeEBM5-@!(%tz9>UG<|^YJ?j;d0;=xA_)HKJ#wL*V<8Z}tD zh4atWHmUcO!*kR|WtXFgcFi8#xi)8_;&VmOIx%DuJu2|8wyUdats%7aApncf z@MxZ$neiUrsMmbze0{NT>|sIyNer^wD>Os@cXGBYbMf~YY%*mX;?IU3!9^-j5<(z==rpOa#CiN110A{)0b|xo zXb-&?1AK0HFD|zQ#h^S+B_Iv}kmlEp5H>g&(_%YI_pk9{-_qU_oz^CjA1GegbWL(IZ|AD8-`u#@L$c9|_%@lwhLJib?t!lG=+01|y4Z_vOIw21xOW-P=K z5M%*AO8|!9+|Fx|J*Mw+>nCH#%S_%Z6?XQ`gCEXIz*K0U?nmz6CF>(ZN5rlZ#gbL@Vf}8ihwwqJOhvL;@a8__fmH zypNYDP9X4_=x}S_BKE5B#$(%+(?gboLdBY3Bl-iJxN@H|Py7^Q`=OA=S1AS_IHtmN zuP%NVk)2cfspJ(2x8~OX1R8{&p*cq1g4^C6gUE@8L zVGcx>Zo47mdhI$^#Q7moj+Y>lpdn{<)P-@sxdE?8R;+U0ve4*=^N{*(E9q^mi3p2d zZl&Me1g<#Xf3d2Z8^RB&nuwpd(|)*!czLD~Fi` z6NE2Lt;*+YOM>9_fDJ`1ndT7+Q7QuG-UHS&z<*g*`qKXX^=p0l2_i5RwMe1SL*0P! z?ZR)h-+XQGDf9f#!R(|JHF*kyvRX<^my-M}b`Sn~slkGu_X0&Ee>mo7?kQN;8H1_e zr{nz8^7;KVGxThl7TT2vO4S$Egg0%qNChn?Bibn6Hle>+E*{4@dxBD!LHBOJd@{c= zbB07G69=6Uk+UU5^Teentd?Cb)krF?qiHE2cD?0d%#{znN*Z^GIqFngQp6|jTPL?t zi8aLys?7i|?w};b!wK58d)4(%5mu`Adnj~db5lqwH;V#e*gZz^(N>ga>%uw%mIUvv ziP4^AE#RZclkX66R!uL@y*qtSphz{u^uhBE5ux^8mHyZ5EWhV+ffKar{pJ_p$t_G+ ze@tPXotxUd&3B2y!sxPsGtYwUU&Vf$G$YCxtF+JkZz|y0P2@%vL741pU4f%4TrT-g zo+*v`@N#KUgIy^{V$&*TUfq`QtJy+y7SD41$NT(5I*hjpEcHnqN~A}95i@QTQ*c0J z<0c-z2Dkuu&3;L+b7@g+TDqUAfXU+9vtF7s#M(kK>afjC6*a!z|Yz?Ap z!S2tIl8byuD<0?Jid$usU{bfjjT8|riS&CSqg)|g)z6fb-(QS%O-9^V!QgBvvHc5) zje)jSC^UB}?nG+4-8Di8tn{hW!`HwXYW>H94LR~Y?k=jobM%o)Wsh6N-SXv=s)+?~&a! zvxdP~#=8Y5F`vd1_gUaZTd7tpPc*YuT)JJ*-05x zKmBAD+r`>cA{F25muRHvTo)gUU1jOmJn!+uv*qbHf1$SrK6_M1v88Qjqk#j?f-mzA5i{?2?aMQJ02uLn7pKaec{>J^qm z#2TdN>Yg4Q$#iiMN=cJ{&>J#C>wM%RhOoRp5fH)jpkgi+OJ49aTxxNqCr3nUn45i& z8fuS07dPjHM_wxvb?1hQwipX@Xaa??&zBg9foO>1h8o@}&C{ z&u2R(*F2i7q8m@v@`92?v;;bZaz>n4e8mtYaOobBv(XUu403Pc|DuwkSUHVZU-$J6 zm+r@M4v}miI5~;1;fPA+d-;&ygtQ4$fSUm!_NL7u#|4Y0NDWuJYj$aQFP(?H!AG!B z?Af+lWxzOp%hONJc{e{D988MdeD{H*8xz6b`Fg>u?tpfUAnMC!d9^5cCPWHl5B)+6 zX9vxNZH#?r<%x8(_<1%*Atzxa*UHFTV3m^En#$647UtMu%pI80VCAkJHq1Dbmg1VU zj0a&KCggqj{Eqn>e+ZvQh|Q#|m%%PO)*tUk`kDLD$7l}6;bkD7i>kdV4T48T@%`N8R@F3!htVSU? z+1vku18OaUEMK(gKt3Z}$>_;v?v2J27O>WTJ~kI;CL6P= z^x?zTQl+O@K^rhXg*(2CK5Eh2mdH*%fFJauMxT;7lFy&uq?CP%`UBE7(EUlLr5Qcl z(_Fjsfwgb$^UKboRL|RO$$yqa^Sr$=&VTcY!k#C+SuO zTBMpFwnfu_)$2s_4kcRXor0K<_o8Dxzk2H>lp*dT8zdeAv@1uOL=P7{Gmk;@l47N+ z3w=#dBke`@m#t*jYGPiR75lF>oOSVUs63S&=ifL^ zX&&`QU@l+~`MBtMAP`G6$Ls6r*kSq&1ZnS9A_MWI#%SM4CtU%z&PrjN^4RC5%t8$+ zQF^bcs#rgd2@5syyJbGb?esO5reBRnyM0>76XlWB1Md^0WAB z`;*;C-=OfS>8E2i4Rw7~1hpgi$jriRwB5$px#+~gi64`*-JYi!;ua%zI)$9eaq+~X z8_jAe67=Vy=~1K-Bd&qJo!VA6j&c5e+;-yNH4&oM(Zx_a{^`3Ew{Q6E)JI-<*%xIv zCazfK&J9I$7yeH>cb=&&?{izO#7E`@YDM$B7n*P%qIeX$PD~e$^*eX@@qL=1YW5F1 zcPKFAp3R=UXhb>eeQFp?m;H+0EDF0_G>SKYJf8gSr`@2|3 z)eaN3Tzr*v&2`1`RLzlu?*6+%ZC9Ix?@4r5+8;#98E^vloOI+Pg2(yB6$Z|Ag$eSlw` zD^=AXMJrV2O+X;K-Y~Z%U-XdmxI48#cbb%`!S0-g$fqYkTd;H86UwH~b8hh`4V@s;$51Q`#WD(oYoR!@l$Jt(Zu2~W%6cf2NK%nr zrD#ytInAcb0_ZVmHt4--!0=Qzo#N7bU7j7Aeb+-=rB&fj1iMwQLPI=l2uvC2K*=jp_*`@9)M zVRaI-4j#m(rvd|2fBgut z<}I_T6zjdR{nM3!uT9^HEh*V_Uou=Pr5hEMu;dP!fdTuj=mGZ3FAwi**$3+j8e!PD zRr(il9QEVR>u1Y%OdA4<=M~xNv5V$!D;^u@oekh>@8v|;hDtwBxN8(7fC}ic7k&#Uj{jNvv&-eG&d;H;?b3f00U$5&muG>1K zTy0U6UN=h2Y4i0``2FCD%b5{jMT#5^_LF#zW8xjQ#A#*zIRC6cpWWZT=S0d160fyc z9C+fQpKc@Dnye^=;sby%%4lb%K|f_jWNYd}AcSpqscB2j$j~b4c$8-%-0T{Y6OZ?> zbpOfA<p8aPoO}ZdAW&jR-}X?e$LC@HZz%$W356LgdpF;<XLz`Qpi7`8acwcIB?1;jtI$$v1I z_X(dZ4z)X<&bz^SQ2-K8<9_~G_#cm!XF8iX>$O-}q%S*=K8zg`EV%blY(8gZulN%T zNx!7pcBrh1o%?laDca-(*KAXJsarZt0~dX5_i3H#v&%+&Y8~9Q7SgiMGv9qnWXr<- z8CfZdw~1X;`K{6ZenFEgC0}KA`ASdHSB%#HHFgt79kZ|}jzap!f^`%7K2v$}Y`y9~(Un#LUd zEzhLO>13)^`3b~d;RkfF$90Tb`bM|-XzY!QjoIqTtz+M~2Uc5rsfdu=ZOW}Ks6D}< zPR}xeGnbO+kZT><4vG}xn7W{gn{9a*M>p5U8_vlZw=jI1aixVXahPp8~!-Ol0TlSi7cobc2iZ4 zKhtE|r#I$DTEt@bg)WIu3-ViB_Ae4`!UN0m1GeF8Bj3%+Yu5*Vc3pdbK>rUeX&YZK z8b`CXPt6;7@5oo)`YXuLX7Tini_o5hRlVI)ImTBS?QE=h{Bg8}I@!bT2m$x#S_C7) zEgkPPPBez>eo^QGdX|J5yP{5L71pWF^;5VPJgAa>_Ao|#Be(qIJb9_9PRAdpST<0~ z7x^CY&?wvt6Hl<#37dN65@YNcFVEoB-=*zrv-G z7KPAqwD8J6FdRmP7lrqH8P8&Zk%j5watin*GCrC!B1m_EkJ^ya3BXKXZ+EUpjA zGCrIhIbOHy^m_JW>hNX5%=<#2=+AY~sRH5gl-(d?Vnc36bLDmKCz0w?l7RweRoMEv zGarke`b_h>)avdtc7b$MhK}l^OVipNEoc8)j*_?F>`s*nGb{JL|NZeflrj~=8W<`o zW7VLJXJP!1x3(Jz`<5~*u{A8jqlF>X*BOdH)r#BmrYX_8&mL606Yw~B?Z&V1oGlGs z0R5_2chmLw*j_7YTh+3#&k@e4&Cl_DUY)IMqFk{~X?Nrng4d$$#uoL1+yS-!c>!{> zJ7~?b)AK!lbeyH7CV4)82@J&Ks(`fA^tn1&I`p5(FTOQR<{ypn9#pklURnrT^v(|+ zs22&x^R&_?8_NH^y1D*lr&lsN#c=Irm+ZUFIA&R?&(ye@<@iM@%#a6mPbK@-r~@9d)5_Gk5u+g(03 z?nAM&lONhuf0hrOFJBUQ$&^dB<>;VRfg!mJo5m-~2a3bEc>%W&?@+<1@XcWJg^msa-?xkC?{Lze*-L3>sGTf1DEBezp~HXbIj(k34=fi;Iur(7CfH$t9J&m!!yi z5+iFC_O@+r#mn6t3Bp`QZcy1VM=@c2)K5OPY#A0_{~6|XR|!45@uZ@eDKIiQ89|!2 z{wTMs(j9Yk)WbiDqqE_iy>{>EV{+Lcn|3bRfYI$fR7dW4jN-zgZ|$NlL*S9CeR|Xq zByuNv+TQ>iKl?BdPXAuLQtsGq>)9kq8&FUt^Tfr0<$j=MRn9xB!$GeCoSYm4A|xZ< zrlZkvjPUpJ!jocr_jFvboHob#=*f67a7``qtJ<6Das^@O}=;aH9-o4TT^WRm=vt%1LGwHHE#*w<4IG{DVoVS zjipp_lDghw1s-w24XNc+-%*@1AYfC(8hNxW6H%tPZqdU;jSP*EUG z+qpEv-+v`(cm$9t>ZM~(L8ol?SG|Xl=%l;p!|67LU)#AcE?XQ&fR^|T|$XC zy|LVTZX9RUQ4@*|+pr+qTJFOmKYXOvCfm9H{E!LII^}GfrlNL8>({pfrgNGIt6~)8 zG#+-uX)|Yb`p4>>J}7vBm%JJBtJq@XNi*a&w8ER$zakh%yv?@Wnd!R-1#cHvLFQ|w zGn;>j3aAZG6O?s{Y!Ex|O)2oOCTX-+qCXQE&lYlgK-IDJ%MCN;qC0kWJuDFhD?;!D z9RR&s z^YimJ-h}&4T_2*#DuK;B&!e6c;^@9vq{Q53FS=IEj5m+xNvyAG4DEHjKuz;tI<L%Sf>T=ipPRB z^^V+p{UlK@KqrfffJT5n{6YQL+{im2HAvI2OP1SCKv3tYbqVFJa6U1Ig9WIBy^+34 zC`qSH_ykX+iIHCU>Y`iGFC-l|pi9|UO|*`uhjkJBJ#qfXmNTx!dW!BdEu7;IaAYL|K1}R$V;%>Qg*oA4yXtN=z8mrv+If&fp|IP^xRVj? zss+#nSaUWBik@ZdaWy|G(B+h7W9U5ok?b)RmsWpWg)f+7)?tgZ6S^IkY1BiD1BUhe z))MSDWRs>cX-km)BOQI!%^FMowjXZg8Clw5gQ5ep`i3L-^}$QfT%BPzu;-3sUcZ=z z!j~w*263eg;4pL3tdi{hqtyHUF-nCdt~x@<8=V`ym54 zWTpqzp7e})_$l&!?d5p^{Hk!vJV1L#mZgPKw9OXV0V9e_%DRC&`rp(Iwm+UpCjRcV zQdI7}XOh`Pk%Rtxk8(E^eNsAT3UzvM@B0Mp>g3fW1;J;ZEFb%ooZHD){ z$C?Vqf23&tS#*BKy2(an?UhUNYyzv{~JQ+XZpX>l#K9v2U=JI{w0jLDx#5wSDVn}7BsOTdr3;&g5hg;FRF#kSId!)kf^vcVNIR==v3zIJo>`3=`VdYQr zvtR={*KkG}$yicYIk>%~j@Z9m`lVv~YNe;r&Vtm0yU_1lZz;*cA2WX18HGHmV!UA$CVZnzSp3dadR7n1}ZD}H624=@7DLb3O(V-I)kO)6IO z((#)GA~}kOH3xm#s#h;9yT6}CAZjG?A5{h*KmYrpmlhp*J;u5K0F*l^Z z6yh=IQ@U)wp}%%^rX9xWrbq&^++8iCmLPXU)Q~MW5v^*~hG)e#um9Tavn(JgpE1sC znrbZa_Uc?%RKJR1*m-a~IseDxEuDT&{wt%yMyCzqi4W3j+$yiN4(*2YtYuoCpc3YH z^kCDil@WkDd}Nck@(fiG&ebiRLXrLsD;|0$yqwf$s^?aOt(z3pkHYTTG&UBmmU(^d0MA# z`v(Vil`<-K)|B8kvaisQo=%|%ic!3M;!=ztm=;y%|KD>1)ikE+$elT{Z#_= z{0typE%k&oFsUNihq5j$pyPcS<{17F-|W&>U<;u~iU1Tq;t}$MwG}KnCa1(T$yZr3 zDk~s!RQRX;Mc2_yD?r^DXbuX<$m?uq0=XMRfyU-hk7T4{VdQkp4^SK=8IsNN&O<^39IjyW*1U z*EsQm)w;-AaU+01+-z?v?jNs;FjGzfyE8q*|q`gwOo!z?FRYbc6b7Onsx2?W=_26#sECw>L`W zmD}0ptScSn`hrSjZnE{;_C>!>;GKQ0hhJ>E?g0={_3p8k)REI zN~2O!GdbFcA5e%_rx@xkd@FHhB@9F#b>zO7iNZ5)JFC8sxhdhKjV3WUZu$oM3%If7 zG6wfCi8f~&U^o!!?kKDX_mxZ~80&XQ>>$fewU&kr45_X2@;_JA)_`)d))n-l)Gvk$h+YhiXqZ9%%>Hwiah zI+~(w_IOA^576SM#jC+dkirdN8)~mXf)3_ahU~U06MFMb%XIpWPilKw z{Ts^D)OYfaXSdnzD)qXy@FrIig=-RqIZgf`KVinlyqJLomlUMgn`%R)elf6AcBT&9 zvFIC!;qUNk@hM}23T(W^R|rA83`d=4VLaE4$45q76v>U1+@4V1@z;!beUi(mEOf|b zt~o437uB(Fn*jmeUJe9R04Zpb+KG|3r{!bG8as^2cXoDFd)6>LA}F9RwC6Cr^395m z^?|`qnfY@|G1NN~0V7+>zcQ8ZSF~(@$x^~zVV37`+LD$05>H?Q^7+toUabNgPUxrQFD=Fca zG7YMDVhqO{EtRpnRFTt9ddi9yqG=^KOWJs7a(K>-x)2_wO5%-Q_*uSOIQUeb_Xxc8 zB4ixNIb#Rk9I2L(`TfWFM$7!h)MI_IyLV@=l>C2xH5`0nr9 z)#dJifUYdJ74#_UXUhg8Grbg3a3`?F#_=%+jBGz8MsX~KG@2N{nOJ&%NaH&#^`?-6 z88qt`c#COIRMb(kvG8*a-Bp)nmLX%yK2ySE&hv9?C!qeQ76yAEmXPnlZMR(9UdpMN zvn<6x;)~P%>W^*Pkv`8JrEhF3dY+;KhklYSErD72s?oFqr{NGNUGI_?`VO$;Yf4z) zih~y?cOgjB`Eud;v*pv-9ky)1nqDlt4kA5L@n>DP^!}CKce?mDKz^JI+V?t0J^IS` zdf4sjM`7DO_gn7XaHRL~e)$1-V!kGFNZ#cyK8sK=GdO1JBo^Qf!1R^zwTHD z#J9O1YfHa{`MRJ0TT25@grZs1v%;NWe32@e)^GZj?-jekx}{PyF8JW8GxN(X`)Jxk z>DltopQoIKON!_>$BN0O?EF(YA~;vROA=~Ipxe&K|4@qVkURH0nd&(Cz`|rSK$&C* zpvK8WOWNPw%Xc}n`@(l)d(CMZ*D}@kOK?Tzz-=vcs^{nqjA#Ifcz~z1n5iyFFd{KI znyJq66_4HP^Na{&*@=gCpD_ubi;5)~e<8R5gZtk4cgA16VULU`2 z4EQ4r`#Kf0&x0>~uAi+nNCJ*hFzMjyIliUF{x*=st?kz%-$OuS7ixl;}Hl(+8g(?iVb`#3>w ze6lAj#Hd!_U}Ve1yavqiO{LJTu{`DD!ZZ#9L~8712ToDsJUcc3M*5Zw!)VXrv;l8V zZK33#wH)*T8w)Ic$op_`Kt+}4e>eZYt5XOFWtJ{qpZCT10b!;+XB9IyZmh87(oaL) zu9=F1scp6*?;Zm{YOs7IChFy-k%^r?y|LQRU)gU%9(C%)8>J(yaD3OEH7lB{eW@Ke zdfDJ1Hr@wR!7j(<@V6G)Egl14TWQH`II#X&#$DdL95Lk^R{V8}lLo@lZj1n6BZK`HKLST_ zR=TE34l(Xn`^g?5d0wIY&2lp863y0R8m*`8I_pvODll2x z;$x<5yA_yNUG$2ZWcVsbCDLsh?HbYqnW^=^hhC%2)bm^eI#C{wb85 z#R0SW42vv;1^9Zw*f^uhv1}Mb`nS*o>zpr4vyJrJmqXeSClMj#BpJ3=>@uLsryz{T zGa8*$F$T|p4V;bs0wDm_=Al0RZt=R5x$T=R_VI)w%~{e4@Z{&b@aIdY3ddd47Z-9!r_B zO4ovGEhBweuu^oYLv8TQ{Cq|N2Ym4gyW>TfujUVtpr| zbJRM|5M*rn8>OauFDZASr}Fo=iPexgkRE0CYDGr#irD5RpZzBI*Pa4NZL{ytn*vJfc4%jAtUd106$-@9}29Bj>_$ z(Y}I+YgJLW>dKM=nHf6Qd`N!a=gtdwPF81Lvd2j>uUn_i-qiWT9p$=JTY|f|0Hlg} zEwqbGZ`g#!vw2!~L+<4tPoHp0S_}18;bS8X3u2gTlZ50L@xIFKM%vT5b(5nM>j$MR z&PNvc{Y?3bD>YS4?cfqXYOZ*JL%N_S;y)`Olxk~(ra_@mH|9Z0!*aV%s-}w%SUZ_A zz^Fh%MNGYM?-<{Wk`OFG`<6atjujB!8n_2$7bi7%i_vkV1Mp6a^hkXdocc9V^hsr_ z0dqs6h*qUU5a_O7WM6_|zx-Z>ezafA(g2l_3QQ51nUC;-N-KA!zv4{%eI^j4D5%Yf zGl#dn{D|5bpgo#ZRd&iV+b@auJF<4!Nw6|hFoMVh8~C3Smh14)!$-Tqnzlz`b(qp^?q&+;J;7!x;>=>dUURdJ zWOMT$KpJ~65<(~gj6bpokJw&_Y9$f%EBXmr6|R6D`QZb0N8h;~owAJt=HxK9u+G!( zr&rmYIz`&}Dg2FhNgD-MVOWU9mtBImERYN!zj=NMhJG5HjM-?sULw`e^G(b6q9RL6 zOIxln8VxWJdS>6F;LR(`YcPux!|#IoL*%Jrg(J6b>5%oZ5S~k$N*fM`ep_QjUy&W( z?0p2BLIm2K0w@6!u?KHXc4)7Gl%(+$2Kz-6vX>0bk9oZ3#+w3I*tg95@IKaY8?Jp~ zroGXZI{Cr&Z>QAVJ>emH8|V>T91LP4kJYu?j_ninoIbWa?fk%}sIE-=rUo3VVLFrN zpztX=fE0nQd2Sl7%$7M#3tF55Hs&$U;=_ z!%m<9P9r)ekQYao=LJOyr;Qbl5Z<*&B%QhB`1?qetIvfki&@tY? zxq4y$Cr3e?9-Q|vHtut}xFHxYHx`{DM}fS2*%zj{?SO?Bpi546De_B~@$iI>8jg)w zbps`x}+9T(|3Y;E$xW@}^-dBeiYt=-vq z>qKI;f*z6gXu8D#2?YEf`oIf(#;fKlFzpw+1+s9 z!iwL#-Tjm&tNx(jf1E{v4ddq2@m7Qdp-W2^UE`ZQcrb4?%po!i?6i~8_!BT?Ii(Wp zB1ePbp^`GMgWq+6JMdFYdHj>eLK5AAn(tCJEMHFeK<~G&^>ND#=+-|e2HaNA(|~Y|NIc9x?Da~ zoq8k-*nU(Hqphw47-ubO{^1E0#Ey}!e92<(?8Z+BCDql*wu0Au;Te-)GGf%kb@z5_ z(CtPHYKvKBdco^xfY(YP#strJL=rejX5UYPN&cWGf9jRYt{Vl-;bO<%&;8wP{7W15 zNbnJGxBUv%?`zFpepI;rYA}`nn;45p&kq|37uUU!K7G4d7QxYv@m4FUI{oY3%06JU zemqp8*6`77TAiU7lzeFLT9%<{`mK1`p^ zcl$3*z8zT!sCP6wXZU754*W)?Xvr9g5c(vMNd~!BqVNUsUEVGv3a;>q@MA;^MW4CQ z2Z!t%D>q)Dqf%y+^hg>X4^#*|Y|N)KjK?-zE-Jdvdm!a-R#XbRiCB9YbpKH*YBfZJ zK)_ygF#G9S+dOHQ?2rVUttJR{iAqB~l_U49AH{32@5O>53D)=+c_!G!)fxZT5?tV| zY=MVF_OQo@i_;r4i$pE0CuO!lgL!nxjEuCoz=Xc2 z;w{g3r{$tc1K-Af3eTLT;?;N5=52Fh-&g78WJ2Ya)9;QftZhRFl*GuHRJp?fUDkpIy_CX9t3BqJD+MlO!W=^Y~Jw6nn6^l z!Z%G@n`7HSdqBO7J}-C2k9$l*(Y2cQMPaL$BEJC@t=?7wrxfp&$c z3F{ATyNoUxk35gcYsSX^IW2u<;cmt1LQytOMJn!=p=Pu^a+GN~s_zB4V@g(&Qv#5TJS&M$BG-Hy-k_jyhuG2eg@=*V?@KH$UmtCu%U8=HWT zu!GZQYj4sf41*!N6wPC&*B|fmi&!=wgP4Q4c~D)Z~{)h#=2RJ3zS}yaY34NpaWVxR@A|GZVUK@D0uqV z4*JBsr*u3d+bRr%+2FZ*STJ{tjDS@ta}7LKY=0c>5GzhSWiU69=h5i9CwC7K8Jk~R zea{Eci4@qq2xNO5Pi!}_ZGhmCsMKLbBhMM_z)wGG&$ik6)$$D_GIq&^OCdt2YC7&c zpH9#k=H3Sy%?8qJ>5%4#{(EkQ{>uj9l}K9xf^x!xkoYr6z;SxPqTEL(Qxr#^^3_y_ z%C;Yx`0}-xbF4L4d-Eej%!$)x#ZLjj|GFwxBY2+uRtLx({#mE=zCj~ENp0rV6UJ}p z8$Vp&5>Pv4orgCuN@$Y`f73LQX4A`JG>bGzU**)&!E4El{ClWJq=t~reVb(T)yv*% z13Y6iX(-qGglQ?IvN**}Gjv=rdUHDnv}gp09QMwRdxHVK`V%AN<`@h;;E!v#6YvL7 zS?(l1s?SA#K2_(N8k1XHbuB|h?TZEKFh3%)Gp{2r6@D$W0I@yP-PpP=`&_aA;8>6o z_jjYh?nip8nh7D%1Wl;kwb^pGgDRy9 zNhnInQ&MOBvaP6A&JKR1wCt9OUxkR#+CTk{R_4?oHQid)K8zp!#2)i$n zq6B;5BAM;6r{bqB=zz5Wk%>@;@(jOvM1~3fS)tPOAz3>vK`Munly)k-Q0vq~WGi)u zW?6XSD)?WOJquVC^U(|g$NzQicjBp$-cvXY&Ke*tVhI9w{v7}Lj)v_b z$`y>o7#(VOA5;F8{a%G75emyn7Og-Q;!-L~h3+7c2YUI5)>M=$^V$C-gQ|{Pa5Nvy zQoJ+=N|<76VfXgCrcE+jq}bTLXoVA=TW&>S?2YoeBu|4Tt$faI27;A z!^E0-HoBv<=veCz!-p`{=s88I5RTDVq48UyZ4Xw%$ToAPc+NfIC9D`S@aJ_}yBU^f*lb1}^C( zgRU#BF6lJgTBeO6!K5bDaLJM;^TetjJ+rMy<)+@I88%d99|)AnE5%TF zW?5{#g|e#tq>TJv-LW}%{KgF<&qzz}YrFGSf8j@!%$q^GpzCPlDP;8P+e&Ia+=~Ob zL(s#Ej(L(88?kK(Ja#*~S^7i`UN*?$Agv&G^J!s43JRE@epltY_?sgus)liN*9Q-f z*cexYqQ&@=6w~p+r?3k5*51M*u4R7pBnqs2&sDP1RZW-wMn>(=;y?luTN_JpwAh1b ztxHkC@5gC-Y6cRiR_Ka+XV_o)iPA(jt!?v;_#G+^*`7K$8+f6AU<&6LHa)^7+s5Tj z>D4WCylDPV0K=IEmE1sYd5u8BxAy-M^;v_SB$6l zS9tfiR&2{*bWN8Y%2=9_(fo-gngoXJ|J zK+D(B!b$vTudh~IRUSI0^!dwcvKE{xg)8Z~7AW3& z6a!Y(#35QU;47Bjb8)765if7(vv8JR-&w~WdNJPA&tP$}X3QhIQBVg{DC*gL=l2h> zC~xRt!-kOh*5Y_0?4hVG3t~{MS0_zkp=?mBd4%iBU$t!Vei> z9IJWGT?_%U>q?@MBlm6~1JCUuVHm5P@|QJFHSezZ&wCM9+&+(eIg%{>)KMhZ-{(4Q z)t`b_W3(wJo!$9i^TC4;bzaBWQt^?ZFNT=fWX&uWL$$xs7E@+QJfu31LYwmUmR^Ta z!RTdySnTTK-S2&w4Fi1-m$faT_0ny0lcE%}9__wQHuUNNe=8F$(>-5ERNT3`DQ6Ms zOQWtLr$U&y>@t)xW9OeIEfp|Z!`v|ZZDq)3ER{02_|2P3w8z_7cl1+j>KYn=-jH(l`Ce5?VDGI1ha!H#q^H8H!}3#j=L{vZl4? z`z~i*I%=8h5PQ5&AM6V@6I$ifBC}JXmHlBc_AqYG<5X2l{u)=Md;w76s+W2SqYJ;9 z%g1Wo2uzTvG7O>BZ2J;yFmVs80Ix(79-}&rEUM-JGHka*$5r_XcwC*Kp;$+_-Y>x% zlLn#4@A{*M_qki72nSWz{R|)jQR4G5*~wRur}oj)ayF?uc`mF?_P!cr(hlu?KDL$@ z${t6bY0ECER?0PJt4X1*pXuU~XPk6V>|o3lJt| z0*(uTqZd$km#}Uw`PlvG4z(r7P(s!_tSH;_h1F?1=XO++W9dxn@Am!d(DwRNV6^6y z!k+iKv=zc*yj?GMR?WYxEf>GpN$=#xw=UYkcDdC96o>W`>}`Ykpq#Ar=j$=aLxSfb zGp><+Z-DztS6~`vbcy6dTOw&{6s7(8qZKBzjF7T z62MV0`oR!^(>0X!EJPmE^s(;sw9^4fR;+PY;H$Q_E1B1Srqyq(7P9pW&YpC33!^jU zq#ixnes!~N#4;+XMKiLmtW+k3;?VBwEiJ(q^yfcONZ_C8mHRRb5HR+dm6ZjWSytx& zXfptbQUw+Zs}i`J2Kf2P?qhE|%EWkxmpvPpk_yM-GTKTWGc#B|d+>Q#5rry|T*c5H z@c9Lu1hS3U$g$g-o1ZHdd({5&`q|*3L=n?2q`CtrSYMU6zG{&N(hk}U$h;zy8$$7M z^k|_N{ajwXW@^5fRJU3o0O6(zhdj*ttqrfUWK-Vy=Zl{M4_F06xTW zbHdEcK+X?_%oSYjSJ>HYn6D|B*Kx0in$uJ&6HsV2g`Z%-y>EKJNHvRsli^=5F&g;a zoY_dMlR+BxS9Qes_zM`&_)KGz**;#Ie9K)N;8a?7Hx+$#;p36>*?5>;_SuJTtm_;N zHIR;$Cz|0fhu{`I{+$5iNSF-wb0iOmw;5>zgimkSsSAN|#U{;$vEB9unOddnzB-VG zmeGw*$2SRbRl(N=gOA<93buIAwd&;}UsvwZY2}v7qF!4&e3mTs?-PNBgSKYK4>%G$ zR^@lURh#x&wdx~*K=2|RsNgx*HZ8>1 zHvg>-m}2OkN$cE_KpP1PuYklm(Xqf|w_ZS$`|Ho*R#Ls_z20BCv->~?17!UTqxbw@ zDxd{gkdtUZ{-~gimYPbD>|{)k=JF*Vkn-=Et3M*%CWfiTSY??adK72^L%y{U17Pj*?`f)yVMBT7+5tz8$-=QMcZbF( z4|~VAT!I1tNbVcDj8F~4^1TMhG{$hO0*g~D;r+LSne7!U75h0JqZ3H)hAvFoh3uPM zu}k`xhxWd$U}(HAsQB@N|6DTC%ckCq#w6i5y{2PsN(Ry+tPKkweIfEAffM zqDAk5{^upGsd0M9BJt%5H}|Ua_Fu|;9#7;|td1DeZGVk;j#mVyy15MgaXbu&xV{V~ zzeXlY0D*@fG(c{dnUznpeflWU_snz2tBBQOpWFdoYUM--&egoIrEfbX7tx!632=%< z9K3>lOgU31OL>X^BBJGtV~jZK_(YqR`blxqS-i8kv=XNo9SFeWAoka@Er%7FqvSSu zJOS9XnD}G(%h0ar;I;zsQP@)MqdskJekoeynIh$S-UGU7E!o9}$8b1>bJ`+4bjVZ! zOagt}PMZpN5@Xaw`sq2%38+^#`9yP&ieYR(X3xy>Nx#+Mgrh6g`Q+tAjG$qeAC^FP z5zD$I#TlfjeAn6=_=)r#1tUEW{~l*vufXFr+2StQP6xuJP*EgbW6Hdam}+f=@Q9`1 z8vpRx+dO0%M{`_og<)pI-C{*t!NC`^`BwaVppV(W0jv5%X$A@8qx>M10P_M)R63_0 zjq#o6ZRdyKE(dJv36MV@gee9Z^e>W=zAE){Fd9+7$QNMpAv|?-hCH~h%PP{QK#>6l zuRbD_T09q#R-6&k{<5@HJ63|e=p`R3Ye?kPucp2wn&WGwn*JhD>6e`I?T83tv7;iN zRJzz~5CO#()&t(6W>zGu?oIn!%Xy25%SpIw>yr0n0_Z|F{b$qaJ66RxSR4gwd=c(WHcBaA?@_JHhLnUnqu>+}YyAq)23{VGU=mJ7 zR}7}mYE!)dL#9Uii@kSW_SeM0ABJau@RN!C93#Ks`~ZlO7m8=^39Wq3Ib6PR*E75F zY9{sDY~&f7fLB%a=rr6n5#{U?z^V!HVCPS?x+b3+saKl zU=oI}z!%H8nsES8ixnmF;E7_2DsPJ0nBvm%RDDv0Mm!rb2)7M0B>c>ZO@1X4l{crT zuOcY}QtbaR_Xq_T=#wp`U-u#F_mQul11Vg`C0A1iFYZAIE=yu|mhnH@UNt?%!N1x< zAE|YBc19+k*44leWCu-z8oUeE6$I7aO(2?Q*ueRAmSPuUeck)=$_mZ#_IUX~0Y~l* zz83LZCq%8Ze5F|SL2C8|hgLkhyUgccf7N8|$&Wd-p3J5%uP(1kr{E2hS*z1?)S&^y zF|WrGSXlE6W#Rpl03X8|zfxJ{$!5Ceie+RmBwc$Xwjy8+bnz?1H{lZHly=SV^UoKh zrZ8HAOt3`XJ{AmkU5;LgyhTND0>&c?)n@zth?jpBxI=Tqk*I1|H4hC);oL#+&S;$q z4*flZ;|^4ZAv%a$io`@LwaBnz=kRqhJ(5!`(ylK3bPJwa8yEfxQ}3 zCxTd-%99=#Fa3P}x7yz|waN2Frg`d`h%a3rsgtlJlM+QLzGn^n^%}%_2%Jul>Ie5s z4~|_ie5kznwNUX}4CUz(@QbE4AFZsie03>&;D|1QJSf1QAE>g~%V1=9ksa{gjCTJO zMBRhY^BDf~iK9z?vmy@=+m;5BU^q?tyJUXPpq_6-XFAnJ(@ja-7D66U|X4)Y27KYD#e9lVBfMR7IYU zy7uFt5kCjBhPpbKyAp}(6G(~yz8NM>fnt69*U>Dr2ru7?jemXZ6hLikJxiY(~7t9`(s-`V~B#wC3t0BDENWTNF) ztEfW#%!LlhXQ%myW-Lu`Oc9k{AVsf}N;L}DRM4!?5{F|)!;w>649~aFfn%iHT76P= z*PQA(;pHsEwaIr+{<4g}&Ax)8lL%KCWymevMuE|a4eV> z`U+$vI?wYUdN1fjiY0k2zkl?LwWePD7nm@XXOw;FGO~F~(Qt$lJ?@`fx%Ax^Fs;6( z=gBCtQqzO-j9$mJZw&P|aK3!NV1hwRBB&Sl0EWUul8VxTJ1&s_0g22llI<(8nsZV0 z*QNHI?tgGW3!_v9aN+OXRwGzGRNOa1`0v-BYo%+mB~JadR^hXAV>`Xkkm3aP0cjsy zfw=eI*=#^6uUw=C4IWuJDLl9%o;(U(7kqXvdf5qV$e|=hww8-Wh-nC$Z+KNK#tB@Y z`-x^x;0KZY^X11!1?uM_UDGe$wj6dXfTH{4nc_t~@XOLlfH^rx!S8r;Hg6txB>LnE z=>JD_?l)Qd%9W|vZj;?NY(hE~Mx(%+Y$En9jg?+_zYqL3TxUbbZsDFl$Uja3?5;Cw zW?c(T(Mli3JPImH;p|+^)4zLN8TOn^95()T3b6*rrF~q*YQQzM zdwZYIEtyx)8{Sd4)CzI`ln(0j$k`w!3cY^y$SH&RcN1cXp~i5h!`*r&H4mdR!L?&= zrxV-sIYO^S78Pm6-`-ZhUU=Jq?wG5gId?$VY#=cd2HhmctxR870Kcn*(0O)o8WUsD z+3$N9;X05C#u1O{kv{oA!*b&OB%<|Rdo?I{1TcvE2nA2w9MGJC+=&#dEDyqME!h`p zF+DmQ2$5i>P2(I_%qwO+r(<~mx9e$)hAx3 zWTym)<8fm?*9DkqYs+=V-=-sK0B86Bw>?QOq-nS~ zU;;qqC^Q8QNJCPrUm>OA*}QUi3vRv;F4~&eh9r?I7xqFG_!_cZtJ1Af2_z!kB`C zV)on(R+;hJj~ou1p}f(8D`$+Nea314UrKP&F;)Zb@%4!+O#*kG3w;MAPUT_eUPiO` z>dQx`lk(?k_JFLd-=px2`eajdwTo8N3ywkqm~#Usko@uHw_JPKlw(HE2S0yvhKVF^ zb}T%zwND|Dh=)<{fsz32;WG?{f>^}HCSQ5K7SGPY_~j**oaFK`kGS`CX33F@-n;k! ze@aLaPK4|*mE;RO#eWjW6RtrgyZ$f2scz2=1G(@FM&j!Kb{fq}Kw7<9 zI}2hO%*VTxZAHH_5XxxK{P3gu=T-K1rKZRGv&HG5II6h{pC(%@DhjtG?^d#KWwG4T zCf5whcOjkpjo4#bho)|anNUAwkPfx*V?>>OALD`eGpk{K$LlNfuQ6V9q-|$+-=z** zhH=y*`{^(UJt=iq%-<%ZpmAYM z+|- z)Ema1tR2K}Nk$UA3Ru5@m>yex@;!_@lJ?E`%en6dgYYiL8<>mudxjipt+nGskCbke zoUbOGwF8Sg3g2r**HQNtu`XP?EVk+=8Q85fnjFS>8bT`6)IK?2&*Potv1_4yMfH%l z8ROAgl#|e@gz3sX&I3l)Ol7yFFVPrproa&{~qD)~|1fWthT3pV7I+|erJ zfZ2LLU1&(&FVEkc<+i&%_{hPGlL(X)V}s>?VnM<%e!I`XEZ>Emc0GXp?V%YK5EjqfoT$#QakeLLS`S{9S*pExn?dQJ++<7~JeZ3KhA%o8 zE+b~w-4E~PHxBXcf5v~-27N6wipPHtynS+p-cEuvul1O&1y^YYE|Q^D zI(CC2#gKiAN&R}QZrClze$zKw9aGX7he|s=lj*jGVkm(TXWSM2`-=y|`3{a1kNgn# zW0E^U+I@6Q@isN$*@l^=HizrR*vK9HVOXGgUDlp3&#{^alTlVN{#Z8X%pn7&y>kxa&^3?{`c7U*jOJP|LM{0`g+@m}B#L`*oY4hJe9 z4!nOyxCDK5Nxg>~I%!xi^ymLr4A|sXf^s^!w2yV{chHCkut9AS4lzdcHro`*r~LBG zblFv5B}V7oZ#?p&WKc3TJzV>sr#mi+3nu-H0M6ngrqLr_KMWYbJMJ(@ z2L4V#{j~4|(M0{24nmRsmaJitzRBh?EQrMSp@+GpU={BxP}-^iX(t{Uc)>vRVS!qE ziPJwz{=%C)utX@8nefdj%?xll$=z}wNH8RxH|vSZlEgDHTKEXp@X$aMzS$fFqZ%We7n7N-B%^7@&R14B-FJXxxQ)kYY(5Fd?k^ zNPue@y|Sa=P3nC~aR1}?%ArKa?+ zlZh}=Yb;E)z1-A``{Yf?Dw-undvF;v4hsLjn$A2P%KeMu$}+kmjLK3IrD$k`tYvT$ zKU^WMvZlxq_llOwSh9_)2{mZ5MTU^=TFRCYjVX*=N=agnWr~SGvXuFqXLP;%Jzn!X zp68tNIiJt_dqB+#y48?#eKuF?bgQbIx(-c3?HWYJ>RrCz{n!yF;XM6$CW>e9=-JoRP^i&^Wj}0_0!ks>FSKAu%P|SAI5DM;rA)8B_4UzbhJ|h~t)p;}q4--8w=*$Vr zM_PUuPa6=uZ)kQzga+vYM#TwsM9Qrs)a`;#;0{b`Vc#k68~%1N;3{Z(-%rRv@FNYKK|hsye< zcrq<2WMfsljnQENp< z``bNV0tR(hwX561qOm2o0d%LbNd#)>kag{fOeBa_YvjAB)T%a(D*P%4exDnfSTo(4 zV+Qj&=J22UkGjNA1{H#+QlABw)f#lT-y+e*$mHeaL0D;Y`;JlXQc9A~nlJFv&3p-`gmr6QKMT_vBCDi(Y&E+;bLUYk zimV~%wE$|yW_w(8p~n^N93^-bO&RIoJ=P4+pGpjVn>JvHFT^~!QGLb)Skw@8hpz+6 z6_v;8B-$n6hN|8F@;M_J_!{5%3-xYr8kPNtH&DtxAb!yvN8BDNqRLP#C!{g?5mV~Z zof4{uJQWqNz8dYa?0o}Az=I$a zD2Q{?s5=^`0erD4-j0^?NSr@#tX|ig9C(Zp*mtLkdLLdV_i(&hRZv^#L@q#f&~9oC*M)JAlhRm9W_TXzR)?6-$B1e79&0bzFOT=K z{J`<|EN=fOSu!$G8>(kSu+XNz;EzOx3ITYFQ9r~ylQLo0 zjy;>S)~_kc1JZI|C6u zgl`_UZ&xeTdZ;m<2U*qLAhXwZCkHlkmc+f^Y3Pi<>l4t)Vb0eQV7g;82N>T%LxK^4 zz7EHe8UCFbCrh^duu42r<-{G;_T$&#WSP8>L5Ddo)p*nPOA0%tFo70h!Zwrn>!+V-d{O{&@)}g zt*^9|N^DA}wlB&|0{C%{fGF~k7=Y9}i}!<7;1^e%Xyd%M0bj#cZQ3d@h!Os+M@Jc! z&GD)XCg->g``9ChGqP1t8E9FUmYeHpN-n;}@iKY>*<>{RgdI6QvE_7Ov$tueTBgr-p*>EAXuS~ zj!P~v5DQX)u7W~z5$Usg@ln$<6~QRLBSPzxPfe-`fSC0v)L~ORQnVMaY&);Y z338Sz@x(q@@_{!QN~M`}Cb6Q5Vx)Fb0zC>gpRvtJZOLYm>T;Mh@)a4Ng3EH6biADL zW3CWe1a?Xp-JxrGp1}LscRD80FQY1?im`6P1E1=01oW$C@fD+ZAa%Ij6CH!s@j5@@ zKTe0-+t!b3JK1^15_PWTB07y(k6QCMPm#Q@R+pK6r47E8KOoSe<#u63-cIR!;Jtu= zNUX)xPfqJQF{`%nmb7{^3rZC$K&WZ)v0}8V2|2=NzbUSFqmj^h9Y<81@^aoaPn=v#OoPLzk>>sn5KNu_#MgzS z6^k#BePoM)F$z8YIMr1EwE}%weRmcrY_%%{*{`wf&GF>y6)J9y05*dM_qv2GTtq8= zZq?LEqT+B*chXC?05N|Eaqe_%;Q~szBF@+-+JXQX7QRKpiV3r732>&!g(-|@uVJ&+ zU((yN^B{NllTvQNdCWz$zm$JvOl}w|1zIIIGKF%g?{B|wO>&x70be05ItEczY<9?+;TfS27w>Z~gaLk?Na0)K-kf-?21LQ~-o z7Qhl9!8kdAbTYaIo&EV9wSWxeG1r8;;2cd6WgaVvXV}Q_-oMJ8-mNILLyO)vpEWmp z8gZ}p=+Pd61myOCm`h{VtgNzl z;svM9*m&|v(sBb_lUuOjs9%IrYKTNxw~R_|7Y&pSrIiKKtOaW{_1um22TTE38u zML*kSE9sU(a+o=HK)2$ISH&P(9f+nMO4nxT5~11P*1VT(dQj{zlAo`f8IIa*VK*Q% z8?4TJMF8aN^bXn=nx?rG_pLm-gJIg?rNpEN#tR`^-s{qHPZ9)6*j}ad?bI#w# zh*;rH^l?vhSf(jQgI@vh3ZU6{=K3G@0pbNLO5-&FIR83Q=Nmf-iWLKOCs*}6QIINhkWFNTDMy5$&)WJoozn!Wes8r)sp%|t`b zh8_R@6Qp8%xgbu<(ZS(?93C7-^OhzZb+QAjCr%W;G` z4GPzaK%!^10xKNMv>ze_d_Mg=3KPk|x+$?>T9C zB51aD>|;Jid3k)7qfi|IQYWNg7!ObsK8un_6vU>pT{_O{*qM|10}f-;gygcT(_Hu zdhctiR8S?z&q;QDU%kUM^c z^xDEHpBzFt)8?l426OOU*+bcD2`c#h*h~ zhT~D^8Lz9aWcdh>h5Dz6TGZTluHHqz*VEJX`AfMuIkOymyDZ($ zqQ&Z{72L7P8XrP4J3Pcew2dyrrwL&UH{yvnQ+WHVnwGT%eNfO7XT@iRou%3mvIrDy z4t&5*Tbn|WTcDdPrn=l+Ak6_rC2)~k?Cg%Y#glew8cC>56u(5B<%gx+ju8&G%=DYd zWJ)oN2j@%^nO}aUKAD)D><|@C93CBgCruy5JGi?Yc5CC{GX>MpJ6kDm{cqh8+`~8^ zF@7)~@5dLcUC(SFAl>sj4$`G;960z{BO!SLnS&8MCAl=m&625D(^U+Sob7^tRRF66 zBQ}XWf&JZ2nk(J4n$&gzkI}Zt;6BbVu_(nEFJj|9 z6=vv`EhVDrGy~fm58wXY@LmfO81uq$#g~K&L}iA+3Ggulxma8P*sL_GyJoytELk0C z&Q!{E-zbA8#%Xm(B2~{@Q^s_pPgE6+R#l7^VdGMG{laR&F<-x!xjypaWWTY^t`Z|$ zuUs(Lf}dKHO_Tlip}VcMjTL2y2%JMDX2F!UtD7#4{Kz^cl+B1q|RbG)$S58+3*(`^;WJ`vt^nnmu{L)m zPT}^Ris^ROW}#>5zu4OhiGl^66d&F7K$(_*;oxLP=11F0>!3c-&WFBN)KGUOp1&Bq zhmE^nOi#H=?DsWD9R&LF1O5R7P8J(&Vm$aI6!<+y+?M z-7v|k%k`+$nxp9lfoEehE#nol2Rh{Su%;0IGr9$1yBVWcqejO#-tkK=AFUy38u*!0@HdoysJV6r?KO2i)1!QM9Asut8kHzB7 z@jvX#Dy%+1g=7o7xUqvyq;2xUH)>Vl z;A&+JhZsHjhv=rV9?64Lw>GuL!AjX26!dcM-U#4 z1)7ax)Y@C3F#k?oq4g`v?8gog$X*K>@%Uig^~Qy@0k^B`$>`jG$rqTcE)gR_D_4jx zq1J@83O}lijSJ6l<;VAWx?*E2GQS7#B!JcN>tuGm6~@+;%(QRwC)|gYE>24-dp1cwhho;~m5@r3$Z5Qe4*eGJX7?Zk0)JixWn8Sd7l*E3Sy7EJ!uTZ_=L8{(Jer|>N z_7r{bP2B#8YYrnNxtww`s~RflSWNYt^|EORQvjNEEh;0&GLSQ80d)y#Z|BogN` z@}pH`jO>PN9TjqF_?>$cgVN)b+5j`Cb2|DiaqhcNmdSaB5Q|jyCfGTt{LOuFb~E?v zMl#m{tdRa8lqyE^@V9V1IH0Ts17nw@Q6;;Vn^Qe$M|prHerkLrjx=vick==HWSE^r-)SmO6)k?WNFnqE3c3kzWc>m$t+&8zaB5dNB2_65p$s?2yT z>|Oaa%o^+96;YCiku4a!Lf^I9Dmq4(4bF$hY(8%!TPEbrH#u#H{QGqiS*e6tT~2{~ zlgcR}SKJxA#6WZaD-I*9S*UqQUu%+_h28|8S%bwioWZ?*8aOa zW?Yz)KC;6%38xpNL5qzO(nr+48mC^`Jp#&9KRup-lj>LQ; zXs@VtCrY1Q8?tS?AtI4%@$u5ppFa^+S!QL>xe22}6NFBt%jIX_Tg%L8&d)faCqItH zl!Cn?FMwAq4EDpS!_t_@hnuKmkaT!oHz_$x;Sngo9)t3GDRFRcz{#`|KsP~*=9;gd z&_NL!OyQ$MB3PgBL?Eas(dBfK5H}0`hd3C7k_8hBRN&HXPZ#ABd0ls(k@p#5mNp9d z>Z`;;Xd;feV(RuWg1&NU$QA0eq=l81mTs4(kIF1B7uRQT15ac-4B`X|G?cUS>I)R| zt}BYZN#UP_=iTHHeuQQ+x;3Zp2^9YQh-Q_iTu2%wN$JA8TFvvxT|_kUVHe+P*y}Ax zGM{)M{X~SooR4Leb5+2Itda5Oq3So9FUR;|e;&2YYL>3;z4V zJmwKyKN*2;n?n8#@X!;>XoRU}*yem-pQ>B3i^fD*-pXBcb&`g~p8Etu~X8eur?W%|2<(vcIb)x(~!QaO8Wccp?@ZwJ2%~_ z!CJ=u*pv~&omLT3Cz7r$)^Bz~C@sarn$m9Uxq=OMO$}zk#2@K#J%eh>jyIV>=^NtR zPpc=IfkV_8HS{E!5U;dao{t{ zayE%&ulLkme&M!@knx+bF|WW>XLe1<4h7HJ8J7#vEpAD$5I?GCWhz1`mrIN!!2i-LCR38C4H4xyLnyIxA%?G=V- zVNKibMM(&8S1anLJJrGZOIV|ALEp;3QErFq)Z=By#$mNgEmZaHJ+pgYKV#Dw z(n8Kr1m`owX$fsDnBD_rvnunu6ZDa;zRe{Ax`Q`5syU^~apmRT&GeY6o?ch~mF0?~ zs70`H7mkV>5jd~A;ieXeT1Esmc6VG==GZM_UM#r$YREcGZ&z2oQ_W7WT&eN3xJaeE zcqhcqnAqIuPH`iN8;m+A!V9jDZc*z}OoI~rU>rL*aCj!EHrujp6z&DlN&445M;}s3 zFAJqoz8oyHS(8>VPyWW`jfz~#Vm}npDH`%o_&{pUEeB@(%I0h|Sh4q#7Bp%a(%5@S z_1qZPzFZWPG`En1(n=3^b_Q4*mnG<0kB`oKN#c1p+#F|4H~ca|OYfs#%Z6{tv$q=Q zEEZxjqt&)~fQCul&PRK*34UpfGsiE>1J3c{|f9|YQ)teuca-O7LxNbyv zjBb$|Vo@l>WSP1rXf{%Og6$m}`lv zvCBpc#Y}k;&~;JYt-g3efl1Q8bcR~F7<0c<#;9_{ar_RkDlX%njvxtwJQ<+c5|hpu zjZDLJtHa)0$$4nvD8xb&rzNLA?|p>09r@Ts6fdfkc(y z%w`-$uAR+D;?4v;X)|%G-OUIu9iuLQxuMQkpjKadLhNLgMR+AutdgJq4Y?tE?h|iV zSv6OD*PMg2<#1pN4)eJaNQo>oxVN7?oGidIg+1Hr>*a>iC#jrv1Vy~#H;!{oJ=B7+ zt!&m!9uLHGLfec1fJ=hjAiYj4}EWQVD%AYD3)~)dKi(mnG5kqjyR0mTF87SQ!zs8I(p6)Wh^4ioBpv zw#-^oNO6Sy)TSHn%VmzSPag(GgH@KWzPqdIgInLcT--Ed(9rH*xpqlTq}MmMwCJGj zdU;)k;uV}kDVv=Z2#tKbqFE!Gwo=rrDX+d(HiqHW8WYao>AQd_@n?^D{0Q@$uTcnC zGQhKfC#LHOnOod%dE^|WO?SDT(=6wsOG|SzZCvN}T-RK_z;2ePD1oSX%}qX6HDnJS z8yU^lev{^fxlQuJ@2ug|j-b!ePGIDZtz{MeirJD%OiC-7j@BpucbcCv@a*93Yg?&| zen|8K=0x2hud75zY%IG{=IG|;rk8%YZVJ~5&4$R*F{P_moap+ePsTc^=nlSbJuhe& zj>B9vXQPv!4x7TE5Xzu@YUfNLn$hR7$a?Hq{+=7uh2|(|yd&7NI9C7sP)N~0-tT^> zp>|Ho2a||{(Z&IR6VFVpD7pDAkh{CP59O`-(u7jU!O=Qpo{EZ!S?lZFEF3(`xnD63 z_9Bj5X?9W$N_+G>B7X^}KLul`4*W5Y01mF$e}0CXtn6W!TICe5R$RchOvNGO124#n}v+VP0K7JA(nKZ#oge`V#4wS;u(*?%gCFwE`2H zWUim=`MaR^oP*LvlPxhB^V_CL{b2@@d3EVLYRvtrDR_$hHMd$N7~!~{`$&>p040AB zet&@aR*e1R=7W?ooO|E-ClR0pc6zs~B7Vg`n=1k3HC~`S-Pxjc?$5x>yFkyJe!ZW7 zGG^6}xz1W|_QOB3*=vGcHVqDQ#a(x`Eg7|B?T^|qU3NlFUG@Ze<9%MBR#?>P^e{+5wY0@|r$f5jBVN`oCu7HMV# zqTxPrXOH=3ql=syaq3q`uLh`HNtql_LC`wL!$kLO1z2+|ja{{{%n_M{6Kc@RfSz8+ zwXsnvDKTeRilnT3w(jUqEC`UkY;q46_De#4R-QDuK3TD>g74|>t0^z9g7Su{p+aeO zP#+fkYEN6PFcyg$`Jc0JbCSWQJZo^58d zXIaI41i_8B9&usCuM)>9L5Jr~MEpN~13IKKo?S^{-*xV_llg9z4ddZ#Vt3yRFh*ZW z@bq{~&iA&O_>VhsWdPk`j9$g2u>%!yD|4&{!`RLevq?t!yb?0Y0qWLVyWOR*hU3}9 zr1_(8HDHU?Hm5Bz&MWL-CR%_QH*4l9iSG??#cC^VG&~-@rA8ojZ2Bz&5fg!W`2@`r z$Ii~-4@^uVxxpBRzcM_I?Kc+oWYTY-Ku@TU%X(8fiJcCSq%_{}w_M2dSQAIEUat*K5Ke;jFnjvQ-j*P1^#>2QRQnqE)F`B)Rg*fV)h z?e;xCy3^pndY?I>ksTVdGhPjZrMNle`FsF+UC7wscZ(Ms#Ro=zF>TQ5Mr1cmvdOe``fiEo?*Lh`r9{^@Z&uj zuh)wU875;qy9aH5!*{-SXWxuC8pazisJU-Vze!d3+1KDXsgS}c0CtLqpQ=0)1g~Wlq0~gyjyqT_kRsYwX8=` z^E`;;G@bbR_2`jz*saC)Pp-Ip(12iYhv3cM?hP9hL>#;EF)QqTxKh*fJ@Hcbiotl{ zCHvN=^G$LdW9@BBylSm3A9D}_QEh>LPLCudCDo7mO=cgYS9N~7Og{oX%l*|+kLuSh z!!Pu^+dk2ERm1i;s)vDahkeeG->}u056H65HY=Ip>it|iWnUJ@c=#@702^GeEX4%@ z(&-VpN`yrX`6<|_G79Y>w*#E`TfHf7mhwYyTTQn`{+&-%kJzaHQf<>H^#s{*Mmb=; zY4?+|VA{i-rEQhS-H%tV6%8qGHVLNb`3D391P0PYZ%=JS+#ppld9QDW-;*v?Ti4U3 zczb&4K;CS3(&6Cyzka>>`TkJg{u!Uo$1AXQqi zM*mX^u^>$$iiU!>+WPwH4!8zM=P!L)^vjj^BE~B#e>O~t+zJmFFt+x1m0<-d9AwT_x@ztuPSf18IN>FlBaWH1>r|YL*^hGk9>NufH@C*c99U9!vEj35<{Wv? ziP17M(p^(OonyK>9VDn#T2r%?0xk!*eEH$~R$*aM*|}G}r}tJy=E}Hh?+oSxm4G+M zw)g2c>)Lm?zrWwm&>)hbUoZ%k*Vd41`2N>B^NsMtpX0Yu?IM5wqGsf>eC5IrltjZS z(tfkiYpPtChxw82HYqK%h7v;X$xXCKNfDnQ=%=WssNHOIYyxjYQ&klM zWJirb7MwRZsJ z-Y6n}^{OC9|H!rUEI$AJ-S+pO`}|FnJD;8>dUL6U$aYCzdV47FZQez82RvNMD=V*m z8`MP$?MlgY1eqZ);e}I-(OpGvj`0hmF4K>b;@`2qi^09f7CnQTmhvOQbHw{N2Q?K zW>UqT2#Mu>L_?j6Vb7*2=#_JTNz^BeKtXlV9G~h5kzZzbrjIoRu@|e_Iqw@ANX`$W z*dbcb!DGw`*%5dPSwBbHAC`WL_()6#GKcaT%MZ+7V&YM0a{nyeHlyw_CJ{@VsP|}T zX}M8piB-PLf;rtujU0XR)KDaH|5d?M;7s_VHM>a7@m-!!3t zU#Pb*J9!+-Yn3k4N`;^ND8ax23PR%UY?vA-njrlVe-fQAZQWyIeyYaIkupzMWWLO* zB|8LLI7Cn+oEgp-SX7Zwja>?E1fkx06bFP7h+)GYz(O?8;-1KP!Uehk^!TJn)}p0^ z%Uq!6*OzCRq*Z?JjohfRzCYns!obL=_OnlLAp6p#tDS|gS1uow!2SIDGrO%7>?JpD zhW}~}-rWQbx8H5Bwzu;Fr-OeyyE7YE^JafS*dS}~;6OA(f8P6jUaHy({Cbu=9JgQHp7~RSDcGgk*J3mJG4Q`Z-!r%b*5o;@ogglcN8m!A zak^H(<`Ebe*jQdJG0++3-ftHPKH7BT-{0ls)8PkG<>kiD70&VRm8ky~%QKz{S*!9f z#F_?ey*UV2A4l0cmUV6kTiHWr@^MKX*_n79;(V`DC-{!RWX z!xh!dtLNJebv&zBC>;+rr_S34@n8N}D^K7vTbL8a!$B&lBXDk+Vqb59u!jRP0r0m6X@CmX1gAB+L zvhT|)b1|m!s`Ky@!j66(?JixiX;|-7|FffW-tNT)w&`TE#YAc41=y{NzL#j$dk}8sZ^I6J(NZUS7rU)t>{GB7gRa&Vak& zJKqBg6-x<`#P8ZnszQn18hm7q7QUhpGWdF6fnRU=M;{cy-FlS3UT4Z>RhG;8Pjx$=%MEcO}?4!7eL)#pL#vYC98^op-@Yd0$3H{SUY2 zK*{W1+ReN7Nm00MnY8(?t+gDlR@BqEeaKS|94R)kX}J4L-ZMv2@XE6fKR)Kx*BIu? zS1mC81&>kQN0&jRwMs*g-HMyP-S?J<=6fD#LnQ56n~I9&i*vo2LGgEd+mi$~_x(xl zOO~|`-*Y%1w(DG**mw@sqG4K`T ztRjZQD&qak*GnQ9{rA_#E`%nfsvYZPd)}RFV900i*TbW0-X5=-nW!!voPdHJu^q$v z;5<`8m&aJ;(S~>A$@_~nq}o**FxJJSt^(?XVVRJ=&zbDgg{`n9Ww60ncVB1VRXYWh z!j9m2&R%&fu@F_S(C^-#xuyEcVIUtDkF|l*QGhzTeCAnt3~wzQwNN z3n&$5t3Jco6G}}D2q^463hM_acG(6HN$}(=aETl#au(D0aGigo3s@OAZFy4`n&mwf z2M0F8(h*Pr04Q4Dz`~JAdF(ppexT9XN$rny0TdJy6O%3^Coi9_GGU&z2qF?4R8CHg zw4H{SRKi0tq_frL#s)avq5s=6E@Ur${rUxf9)tkaQxugMy#qFj1eGVZ)vz$lf;{7e ze(b1rHyk*Beu{vh*^szaCigmSO!PbczEhp05KG4z5f~gn?|o4%svE5LGfBR?ff%Et z!x5{~(L2rR0FUeL5GADDUWdZJyUS(WZWKF-5$kp=Ysu{F#`NWWDvaqcb)`V&T}mJs z`T2p8_|5oi9&ugi(KF`-LB(>9YJ``8Sbx&)UxTi#Xe9IqeYqq7+NHq?mdJQnxKe3O z9)X@Pc^{bZ4^2sxkjMfvQ9r5Jlu?sNap=QyV(WxtE9D1wH!* z4_wQH-4l`KhrqSJzad|PxgJ&Q_Ef`z${}p`s5LldlNKN38wfl)*p4+w67tb--Io%f zI9SKl^0=Fv)w@$sw@DVz5u=E-=H8tE8{OrFVZy`<(=osT)Lg;OMF8Ro$qe`AbL zdX1I)il>s1kZsG_5aJG8wE0NC8c=WOtpeU$rakqOEHxgmgXfWDZ~h|onH3fkEE>mO6rWtYd4=)ZlM$HG1@OG<6xxV#yX?eN3d@c}k>`7_slFtDPLqsJq1Fb@p$&=7s>h3*PE0Wc6F(|*pZNjqU ze|dmz*~J`73frM~9Pl`4jV$K`xjJjt`bmAG$_6ea7vfq}3)Pego{@7e2g~T$wvwfJ zRexp2D!F`<)z{hANg)z~1wbQhTu}btqTkoM^4SA%U5?{%wt<~K%Kc2+JELl%HHwAi z78ZrjgY?W7P}#J3g*(yTa^}1qduX@`u?*=RuM(w1>AN1^mEh}pBGkXh*ZAtytH#Ew z%n$(Tm6?@%|6+trjIsUc9q{FR-WBb#)ih5C&5DJ@HL*?K>u+8?c@o!M+WZZ;_j)6Q znXLrx1X$uya3XG&pX|g71qB;`Xc&$U;&t}5&kgezV1v;iA$0xSe2q`h&mA3*zD<4t zghL=9?IVsB--I3j0#4fS-o4#wyB1@`MW}_vj*JpgEOFQl&#X|yij8xFs{43=zQ_6U zg>h)6<&OUw*ovtXN#Ugn(M0>SvK6iR(P;tWINuch?xn=wE{Pgcl*VkgZ(vag)v@f| z(uL3yFB3zix#0ZALw6S80Q>~qE;%~=QJfNi0UM6(&h>uV@B6&GJW8WoJyy)DSepKo z=VSvYOlgAG+*7HMzCkpZF0ZT#21WJLr$#7OW0BLid&0Z3K*Csbx_-95So(}db3>@i-E7?$Mt1qW^4Coc*S3W{o>d8 zbtOXgL&G%co(HP=OW0H%^cixo&uoGQhm|;!*bjU+IJ1t!-fCh0*VzC7R7#lyfYu2n z+)sPU2LW^eKtCfM9Kr%Q%P3rpL^AmA?OiC&bhe5m&d%~_LGm4ZV33+SI7&mq;`8@Sl-7e# zwjHMwuf|eJzMZ%^Bm)D355r@9v1eeg61cnj?s}ccvw3%n zxXW@O9&{W)I>;IUK;l{vs=iy4UqwvGT zlKhbOhxYoqt zNX(b%2P@0g$Q_GSuOOP;M#)RM#eQ@k?X=43Z~_-hlevk%{i4vJS8fDXg# zo8hIWs?TKk37-dqW_U!0sr0gGt9}t7pNK0e5@dz|cy_r#J?zj2xRt*~&6YkqyEh%Y z1cIcn?G9=T;`@^u_GO1CU=W%@@vmN)UB$6#|2~zEC7P3qStMNF6ICH0QECNDoP5uz zGqgd*vf%v@U_4#}lEd)&8rZY$=6~`!k*yCIl{`aUu>R zW8Zp|fVtB0PvqUvO0VbCq6=#6SzC2L)-!#RLMLh`fdsYd`&ZD$ZLO_^{{4Lb3>Ivo zTJ`|9lK=8xvIRIc;q*rnyO2OPEer~E+2bIE}-F!2l!+Up4%arXHvEBJ|f-hU+DUDyf zOlE*34hK)^HeHOi9exmFJ3n-%*`B4Z@%anr=A(2?DU>wtLRJx|V$@V&b8!j=RIl0U zK;vCqEj51&SQ5TNI_K*N1XJj*)X3AW**?(u{L1mDKBrT6TiFp#coxvCS5|#vD5a*9 zS8h+dS*t7o@O!T0&2y{zirUFo9FroMMV74XkP3qMyoOmjFhZh3GZ2 zfjz4&mU#J|w=1tRI3uv5%hVf zQ;O4*=l6^~JvS0&;+UiByFqMx)O{(U?pwJP342j%5|7zrU%k5g>2UT|6Cf(jAi3eX zY^~jO8?4*`u&hhtUaYG_h+g<-H-Wk@cYfzA)8n&TzkhkVFL!0yY4Ls4;mp?Cr0MXh z!7KPjpb6h?C;ico$~F+mc=P5>L|a=~nf`&jC85~Z$iJNc7FX%++-EOj>XDNjh zjHI`7HW}WTXVq+p>2OftN$s7lO*OxH^Crp3wL(D+ExmaN;IZ7bQ4Ge_zFm;e0Z(u8L&o~JnCS*OG7BkR(Fq9OQ7+h6L!h( zf3WxFn!GbfsP)s9%8p^U1FRouhbLQV@j&{o7mWhsl_tvrC*MVeoI@2GBPE9rL0>Fx z*wqJaO-DsVrS}T4V5X->&aF!R8Cm~&u4&cb@-(Ge}JV(FohFkx#_9Q%0|~}Of=vr#OqU>Z6{MQQ5qq@B5M9J6Ex2d3t)%iA6MIG z4A$Piq(5K6eUK)c*{QGY1@6;o$zG5F@gmL`x%MrdsDfNz2ep(e7ZLC1plm9QNf(+! zJEB|ib};wJWrrq!P3o6zK>Ni9Q5ghLl&frxo{(i$ z-2H0usPMRvrTc zPAzXNJh9f3bK18Y7ZB>oZ+u%Dq`orv*_Mx{At21ggsCnRHUl^Im|RO25i*5rBA=J zBi$}sl^UN`i*_6b*;;V7$@JUsmR-3PYh=CI$-}C+e^7FFVeUZ(IETmY6wCl{KTNG` zo4gk;Q0{V7CEY~y@RB9sYuyNq4D{P$iGlBOX1??m=oa5u!=}deqJ`)5ex%_W&09!r zMn8}E7R~AubnUFlp!Q9kVbgL(kOg0ROlPH|!1ZUO;LqsSuVVTvhGdFihucvU)PD|p z)s3G@JAQiJe{yhO>!W%1U{n3ZX*(!4ImISwN4J+e8uXD^X4+_G#)J#;{QEA{F^o2^ znuUM%WPV>8x!4VE~%{hFAmWSe{ny25%+7ux6B7^-#=8TO>8?x<_efNm2xS0KL) z79s!>O`+XYe2rWn+KTOR1TMuG!6)3~`o;)01yk`NR?{YH}tT?|yhHBb6UGMFEDVl#nK|?4LUykVt;!_e+44o&`X7s-M0MA4z99 zgyjLo5jdqZ3`d)+YC|fu`mr+pwX*C_5kdI@)1qVa|KgNxt*0cA;F)23?9d&mOegPi z^i&(AaUc=`$_A)+S%qSten7P+*{Hy~cQ}p0!R^C#x~spe`(<{TfoCR7?;U8@!nGSJ zbo7ymDIm`)FAs6$)M9BpTlf^6Pl_aj^#LU55>GS6^tZj8EMiveoV7WfJH$Ezk_krU z^zeN*AC7?0uY~lsp$-mY_L>dF$tZfEB5PY0`CH2;`#At$|$HX=|Ytc|- zWM(Gy;4E0@3uGZ|SlGF76+1xmWw77=@ zV3kj+H2>E_t_Q&4Ey+^?4kSAzwxr~mvPJsiVs}k*U@c{#;~jpYE!gC5YvOh3 zKF3JfG=!b*^s+Int*!#l%hS`d!@=k8pWnh8X9r%6l$aE#h6bo6`AnWM6oZIJKrarw zlo(KG6ck7F#X@nMeTPTj0q? zoZe07JmK;U+;w zG}ccC?5->a@PL9?+x1xD_2^zOPwoymbXlHJxK z?jShufLAB;d`vevGJobKJ37Zu#40Ns^cjzsazVDLFPgU(rlOWN|MN(t*OzFdJ>oB) z`*G0^gTTFh{dMYsK+gkdvIFsGC65GQ7QZJU&e^QX7O{551S<}3Go28BvTT^GT^qlN z>x=Px_hiG9KxZPt^0@-=$pjsIO%>u|7Y08`;B3`rryt}MXb8`dvI{_t{V=P*G`B=E zC!i`ZpBg*p6XohGT_v!x&kVsPIR_vPHBfzC-}(f=006D{`(w9FwY&6ChNPj?|4gZj zevJ@ypwls|qF^5KIHJ+69WMo?b&T<1#oupW6?yg8J8Z{R$5UCq$lQbY2A%9L;N9r9 zI~6eLX>By>c2z0?2XD4ve6z-SzvMAE4>T(cLf=T}@vvWx9>;qUXaqoqoMI4lP->39 zx|6yIR$&8yK&Y>O1d{>d#oF|{Vmd|xE^Im%U@)NU3-$icT{^w}WbVO(h4k zSTcg2a&f(AWw%~t~suayaBM%;;Npts4wA8iW>m~ zgt-;|1(RE1C13&%Wb~butdjn)Nav?lqBS*{jX|jZh8`0yV*zgxn3Em{63|3(G4scd zAFi^rz215vON?N6F$&lNLq*q8xs?Ne89#3o5NbA(<+`_S`^j2dBI-hn*K4v4+!9(rUy#@yW;>+@1#SA{pV7jWx<`^?f4a?JsA8aqL)b#--1bT?s1%v!Gg*jlw` zTo&+TDJXGCZs&Pegn?}H;_ZK=XHj_@q*I= z^M`L$f-XjUM7JyC5?hU;yKCd2p3w04IKWyRRtR8zN0a%sRlF+G#seh+r#&AtUqCp^ zi?2Cs?5!2!#*JW#G}HOQk35zEQCFPx$Fc0E!>+QcF#M{_8#xbsfY;*X^>&VXCptqPJfn@%+8hd72wh) zch7IDDuQO6zYH2RFaoM`_)Y04BUN|lL;!~=t@Rx?e6PEVxbE*8W!@dpXMY5Cb>*Ms z2?tavQz9Q^UzM9@Qf(+0*>C&s6-iP78buYZsfLw6mqsBBcyDZTlB zEI^jtf@lJxTbmLvjaV>$RW=Cc1g}MePY}k&#(Wf1noXMyUSzaKlQkd&1a*9a)NWqx zQ&rAZR?fEZUo8VynRx%Z!1gdukEIRWuMFa|g~h5~;LXUhv4)wtdn#7mWX;q3zs=*n(S(b~vroDb{<rf2olOY4X>y zY1vi=Kl4<6qbK~tg;&8eefXN0*zzbb;-OTdDO6ZSdV5@~#P*|ak6h?xd;5FyRpzdy zbo=qq3fCixfBi(j@XHrzSriHE1csL9u-NR+XfTryHOcHAm6T>waU*vyrV7d--3Oec zvCTN)P>?1|!`_L$6z3XL1MR1;GMIbhi>-WfwGs~{H6?(O-9Gu9#mQ&55-4hXx}PRU zm%_6M70&(W$90}Dp#lfhEimGO1^rFB`GSe3a{SZhlYP}3r0Q{S;CC_~*Qgsk6^B{8 zzn8CpZs@_c($}wpkM~x9dK%~#EufFI1Hi*p#6e3(M~B-;v7}wI|7a;nxCULg2VAqi zJG_7EJSXK-=5>={pg|r5Je!nl(>5R^FaDVZ#2N6lQXHSwge_i_7$BjMpSl4+VEOa# zKWftR|ED8M53d}of4$;8-TLI>jZaTyDEI+GkpUF^u)(Ep;2uP2IDCJH`C=D1b@sgN zgVdX?K{CC*ZD533F{K6Q$>!!};Hi&{k3VF3sva2uGI*1SpR5W^w=1L@>B0YOe_-@APi-P(G+#AB6pur>1+V0<@2H|p2MstoXG62t`vfUsWfz~>RiD@>|;#Hz;WX0Iuw zzSYE<;pypVkc5CZD|{<-2G~2-zyQL^3f3*;{~;^i0t~a_dR25lXNYs;VHfBLTft`w#3Ff8ncY1-<06xJ)g!>KHT|ybH@5&`&DAOYk$lzAAA<#LK|9~W)cyh{ zA?%=K#?#xo4a`&%u6=jA4orc5fL$E{^pZ+mRZ6%2`k6-1_%sUX=jG*Hdb2ltAKW^i zawFGlW}>_N=^Gmx|Nc2>10#dO&*bMhXOg=6qTD1F2~ha2$(<)YU=Ee}ZY;!Xa_;0q zb{AmlipB|_HQX3-mT9+x8}Glj%~Lx>xJK=$UoloUDI0SPQ*ffJT&fbJ{gIcSH{d2$ z_j`HhCJ+?iGiq06826oq%1Wx`1H-?fuUd{|K^d+w_4&5Z7DkRIdtWg2rH;ENbKe_-(_#Go!-S+30f|?ODH&m@UJ3Gx|x`Ui?2yol7PCxin zC3ZU<{(60HNo|j)&7^+n1K8wDF*Cq^yBq?^-$uIAA%tyH8NWYKCC|HRyKTr;ePN0Hr;C?n00q=j&;We0YdpaS z&c3UEdm~j81HJaWBW={Cm zy|-&q*0pDdkj$)XS&>~_4>U@jWh?Fn)kzs1j=9|mdWXiCAX4|HC38)**gi~v=D zkmioiYo$Y#t-zeXW%$A`WW7W?AFfuQSxZ1Xhy#8H*19w-C_L8ZzNvS$;FJaBU#B{A zP90s7x4y?)(>2xA1U|4*VyH z$>S@a@}T#{v{oJ5A_pP6ep62xCSS3-qiV#60A(wgs$~B4-N|N<#&`}16zX1T!5wcU z_2<7zSV@AIvcq9qxE0IJ2BL4MIi)2fLU9o4f?v}cA4=Q!&If)uz%O5x=ua-)Y%nF% z)I2&|Sg2HqYS@1MdK98-&9B`FG#2znlFO2au?)sT9e=cdiUaZ%jy?APsz6j z1Cc_FYfzk_6)yY9AKZ~m!yiFM75V~{m(q_ugT~ZT_6DpkJaH9O4|wJ9Ou*!WXC@ze zBAA((*S5FiK*LfrlT=}{GW{LK8)F3c-Ju>gh7*@Y`bHIw; zgl9Jf8_zdTxAAwoz)YZmCvT!dDniy2G)|22r+6pfPS7Gew=^q z_4&N#He`G>S@b3VTx%g}b0mn@r0r^c|II=`!q(UeN?hOL%?E$~K!N+a{^G>u>r9$3!kJG=?6~Zxp{e{ zvQ`&9P@yE?6iztw!f&FxJU{<*l9*V{y1-X!1nnSb46i-eu&Q0YP*V3yujAqFqS)m<7lawCG`={Zt`sQ&<=-Cm_CniE`L}d!_%S#A(UEYq6{b zVjf#pFGyf1R?dfbog93*(^CC^2)t43*(D{Mq{5Smx*uGBY5kYjn~>sS(4g#(_rR@! z!nY38)Qu3r#HX&L_$9Zn@XXsc@W*{Ig7=5|)tIhW-@QDm=UxvFe|}x)6~-O-+)v1N zmO7e!G*nz71H92~pb^u>)?fTRcmeVNX_8t9oO&?aYVjhB=Qb%0DhM#23vqJ$?a?xCj+eCwZfw7BgMO70`PTlZRalk7n7UmFt>QFHYoqmhY0Zh*+f<_@UEaJ90 z{7Enk*Ds`kHrdVH{pNxhi76Sp?Aqnjdb<07zae{E$68s1&)SJukB_E>0?;yFTMEnam6JU9AKxZwdCDqi(H z=zjqIZLc4nbJCe7q$zZD4-XF^9U@1QU?B92X)b{U9Xd*wb%P#BVyq$14{-E9cKkUa zd#>{a12^|ADoDdf>{U&Ge4O52b9G;jEG|bN0n>p*sYU0hZMY^1b~BTpSNGy`ivToS zI29rlSGj1;75DbZ$5+%)7N-qrQo()#|Mju?K)Hp+;Y*s;^RyQzhxuyr*)ONPk}VgN z-o8+aIao=Q=cEZ&@*9;oeSl;QrIG0r7Jq+NoOGo-mgDc6?Wb3mDHMwI9!a*PikK%0 z7`P5y|78q-F6=J)wbmmbZJ3&wk%6Wg|8f;(y7r~KJ+jm(FtYvM1irJgnd*hlAUo^O zr)7PtaJiIW!aU?#2sA~Y#WaD=2h_rCZ6ClD0(y^Hs6{XWoEH;K?6v>YY;f3DYcqzr z|K;t;er3({mVs4PQm=*Ye(3|)QCKkHU6JP=ySWJ%)~ zb#HH~b-tgLwj^)F<_omDCf_Tyf|*?mk%OpO1gdSC(wR z4i|m;U}h&HQGMgua#ZwKVI`PKz?+kG<7uPEZZOl(jY9#VUF#DcS(l}QFE2u;{eD+` zA90gneaH-i^AA~Svy-5+^oR&UX&)(tHS*6#{z7qbKe8SVFLfC{Ipu!@hM{OVtMyUb zJrnmNp}2dxNtV-B2c=G{7&2$tk*+{dR|v#AiZ0{LQ^0!cfttJl3=UE;Pe2kqy1oa$ z;P3u}ze#4Pu1?-e9AQ7MrwsU42BF^Z!&MKQH;wG=HpqO}CIgq|R^SbcY`c3yW#> z9Re+?z$whi%X$LS#I;|r5D)!1haHPz2;c-kB?w2}RP~=t zH8Dtra>_%u=!H)K_;H|#P{MW$hcF3ma{WIQ{w|a+nx*O5?wH=>d<)u&gii_y`TLub zS71tg*)P;_rQ-e@@3}_z#@%cLN`MJazXYC}R`f>~S*bMoE%aiT&0ab*%AdeDZU#%v z(Wde_vnM%}E0hHONB_k9!Np`7H1F;2l~b;W=#}YvjK~Zv zS&e)gZuN`YU$57ddJL9;8$nNbX?7VpW4}UiMCfn++>CE0@7*!ZC2Nlt)=-l4Go#gblAIDhCAANWXfeR8RW)u;Y|)5vd|HxAvdNKP@V zG|bZT&P;{d25YNGo;9&Xc;Na%Qz-61ty#xi?SiB7oSPSVQjUVa+m;8;wvE!tlg0OG z_3rL%6&#K7>|_$a8%%9Ow2x)YG=3x}TUB99>XYP7hpYpR2&?E;7u66w@RyqZI zF5I5i-5?)kJ4tCO#lr9|hhdW4)d^Qb_@RchDol%PFdbywI75UU>@^K8+pZTaJ0vM4)=Ky; z*=Osoz?Q>ElWFAOz~aufq2a{Ve5Dw!eQKR36X&g$N7)P%mp215Tvd$RSAJAm?iVs|i5Pjd~nj#-S(vfGKx#52a`cX|Pq&5hHs0b}; z+{CNI_ZTIVqt+;2yWGG0uIxEkD305?9*JA@b%^?(BsSO$6*iflZ$7kh_FM`bjmS}U zC>qdbCK#IkDK3WPxpn9!C7w9_XMGaiJO>wm5P-3zD|;2%1B6m;vA6Lj1^BLDum z@^|-&E1IG4p!=g7xGc93kh5c~tgM`zoH%KE9@gZk zM6*b{=BEt>RRonxe0t|csoq~lFBbpvfZvriTE$G}ved)W!U!7hFeO?TR78vdybX^} zP-b682N|-ywIvH$y+@C(M$KJ5X9(up7Qnnfd<+%yf}{{jw5_E7BNB0Dw|f=nt0Y-O zMdA?7(`<9`Ek5z7WHkWf&h$*(%IXWgTYq}! zk!%V2oOd9OSK|e0IvEqav|qqPx^e*ua-zVO}qclnwh zh4==wQJ)nuyFihZmCwMwd6T}*s0Mo1z5RXADc_{I0ZL40NP+7aQ@{2#;&KIPXa{iV zB)T+>)Fo_3Vbz!DUY8enP8Q&Kj!14=3pQNb{k1LSTerFaqkxOu{BaP&DxCmaJBSSd zS!;h~``y#{!p~+fJOMOx-nf-C2Vd<2$c4b02Ur?NcRtoT)tf)7#2_Nj2nGnfk>Kt; z|1az4InRevCcQi5rvyVhf8Mp%-t3Zr4K-ONvR;!%E0#U55AA;KukA{kBppp z&1L)2XFP1hX){71q^_lOWCF~2LY-i!GXX>3Ed~Xhy`+G2i*RjjO@B`!+5U}Z6 zo+W4W2IgNK^6#U--T^5R5JQCfr}H;C@^1Ldb30)Fk?jvi@9g+MLqTH3MLTtt=2`Zo z8L)_%vGFH7)(k|3MdSixkZ$K84a9s18;9l`mj6F400&4xnl;KFv%xDr^{;C;3*&@K zSpNi;&1a(QBnA9;T}!FrG0I<@`3`cy{E%9k0n?IaOuWCFzxw zvzxx`$M*F*$$2NBr(?tIEZpM*B^J1?6C&d?m_`*|kn++N9PrYG%X4x@V4%3mWF_+y z4-RZLFVSyOfMdEqhbE|{rRC3m#UhrJ=uwIjqt1s^CyyrND8q69>fYnhh|rV5!6sSpD)52mM#$lwnrIY4sieOWSc>!jl^OBCU$ihs_!{u#& znZVq5)nS70f?I=>;M>?T@7h+9ED)J^3`xFP8EL1#C`h9ak=rmKb?5`uT9}oqr0kwO zlauY!f#9nZdal|kb4l5(VN;Zyu!DzPu5OhPefe(Ns#rA+X9-acjPos!c+jKCtofyr zCMuyuV{=kztzB3uTLJ~f&1tLrju^IUJhx>$ce2wW7mEv@^H0CQa8=!`cR%qszA?oc z=qPAU2}uf}|8lS1xlTw#Ps#3RrbDyxD+i=uv<|>4$fX_Jr#?{X2i2QE&N!r0a12^= z7u4qD+`NS}c6x_pvBUJbr$?UZXUuEl3o&IFx*AeJu|;-OHQyWIoid4v$jKXDiQb0j z8JEm!p9sZhJ$P$A@FiahhD1J-+8{x&lNB^P!FLZs+ESo05CSMMn@%r33q0kN9@e)x7yuFTWD(lDBDvhAFMzD4KN`N8``ZG5f zN?e68g=G@&4CR+|d?8J_hA0y--7G}#>fHgALoEAs$k7Nt{0$_Bfny?Ca8PDb;C2Eq z)CH@qdeQUa%H$iv5hwe0nAfyftA5DnE=&XPfXM!rG|J$2uU;wy6%6?1f6ls%6-3-I z{|b2$0NnFlFc56bzLT&Lflsbf`h!mb0Hfo|{MLS@jTW`@mGBtor?(iyQm|6a)sgPZEu(a3u`b6zm|S>XYx`U;3c8UCWS-&K?W?@5$zuD|yA( zyE(+IK5qV6qRm85C<_oMEv%Ez@&%CF3oqCLL3Z!{0jFBibj_K8=UL3dYGEH1$F(); zoNUzTC>l@Ue)Hpm(@ESkmSr>Pj?4J%_X`tqn3&kMQId7S1!hb9QNt& z#n4O7#QR@lV^Yr;B-gObw zt_yb!h7C?gum~{=dow);Im3us$hfmzVL=|rfh7i{eKt;~Zouv`CdL-6GB@MelHWl? zlmmG_yY}GFBv3J2ah3kf_3e-2X~*f8dh(|z;UeaLNS^lAsGls%a!MZf~#r~lRh^uKTLi!fq1DB)s>(sIHU z-4l+0q?(F%YXjmV#&WM!Y6y0nQp^-TL7cd^2kLiUs3gFG zmRD3fm-Tu%Z4dndVldsK=}koifJZKl^%uCtCnq1{sR7Vg?d?Hi#4iXDSoQMl!y+M0 z5>vf$?KAP+yBg$b!I^~DA2^Jps#W~-zQvQcG0z=@9feRv6O$}eo{h~-2Z@j1-3~&( zcdTzRuDEzh<=OAW`W7Kp6%OB2+3n^;^&78gU%hHD778(&nVA{9akxcDQb0$84WnB_ zk1)k{gQS;}hTt$d&gCI7L-%DT(F6r08=0(_?qyB=u2t8?vqrB&ks*llNjHt+^5}8? zylH9*GWiIyceg#c2A$TG2sgZYwIFZA%+T1t;C8#zs(gSAmx5j}bkKmci@Gdz0vY25 zt8nzOySw}T-W~{&+}*ogc;=SAef#cVX|zhI!Nl0S(*@Sr7tVy@Km@|uy#;I`16%4W zka&>cF$9=0nBqR`8l!b$t`^$()5zG@{nRh_VpETwSbKj?0bUizSI1O?R<3gLnq?oB zUR|nfbH8!!8w9nc7f`cW|NbIZLtU8{Xwu;9?ch+}Yk?{pg9;CH!hH zF2-UkkZuA<MA+TUIRwM$*jPPfx5k_fpDUICrzydn#=A(m>ACrMo~*k zOJ!x^6<$JQ5^r7t9hnR*?SZ{!NASFfP|D=yfQ9I~n@kdlIIzuA_P+C74U?O{U98lQ z9l{<>bGCJc(D@>i1aQqNSugfZYm(9WfIb(%%6q;~t*rR=h7)od8~;G9*7JRG>sDyT zqNbdznY4N{Bg@ZLyM8OX3K_!#{OJwM6riNU6UrVQF9oi-uJk~rkC9^<(DSVh^#ix8 zYu5w;7w7K&%FfDaVc=nHT?FXVl?Axg>|P!g6HN-*sGo1W5j_9m%Kb-9CP9Um(z zE4#bb4Ll5#XQ3dW4=3WHzR}yOO6uD%aKTOb)U{}Nl-KS3jf6i;sk|6>?pcdC+S1a} zMJbwZgqGF%Jzgc%>O8hO2J!!5Jm43Ntp|B@K-S+&8!nN= zfotz2xxcUNer~lGIn6!$y(3v^ZxinI<^nZG4C(v{a0t*x|AH9`QhSi^wtFM!7`n%Z zMp*-o7oaVocPtz;_<82Vkwp*>A=|s~V^gUO)qqegH5Jd^Cbt|WbsT1*#x#1gG__K>0s$*{h1ix?dpPP5QGRj}0^is3Mv0FP{6|!qgN{^3^E8m^~ zM#|J;di)1a!b-e6;n`}^JxJ?Vc5FE?3HtfMn}7Gf`(nHR$M4CvCm$T^9{@~#hMFCF zI7(>BIYvk%Bdav-e8JZ9D45#Ui!r1+f%cC_T#C?Aib<{IT@~CIlN2wDGM~b0(&t|m ziy?;s0*gkuSh*4AGM(LFy-oxCQQ`WJ?=Y%(AqQCK&l1Ii1FI;Jre@px>n#1dN|TIB zLu^vc@&l5Nr6qcUMn-WZa2~6yS-WY_68vCcm`;hOu56 zX=F|gS7PtVcTemui2xg|){6H~Iy7gK$yw=LZZQ%!11OEx>ipRNGQedK05sgu5+sup zzkUr_Jhr)lQI>`<9!mQ^zsi>Uxh>S9^FoezDdPO?n}j&m-Xf)$c6k{%q6Foho7tXi z!*VVZIlbK;yc%t3*t7oaW#p?&k$9Rg%o1=`aHmQ&+32{$puAg*&P?eC(y&Mhnb#F4 zNygw+92z@2B9CZ~6V>3V9+bY(=^wgrWyBm*qiT^#X!Pn(=V9EwWq%A-j z?5xvXcy#3BD~G#uZwrO7ExFIkG?BwK>Vhbn`%4x|o_2b#EqHce%sB-d#{m5o zsAt*MOpp8cLiOQINchjyDm6+crbfWGNp78pPD>4C3$<1W#Nu>wf@4*fc9<3EqWk(j0kp7;imoVfvF=-Wn%5VX(PSr5&;49 zmlAtNVQw^$VU2ltZb^rrvr~zitL%rifq}HmXg>6gAOT&B{0gdCZEYAJ5Dc*Y{l~)z z>id~$^IK4ZA(^>yTO@-TuQ+0l<+EEa2kjT=X=Y{&lcs4?mWfVbQMiR3 z<7jo`FJFS_)r(3>W?nzC($LTtte^V&Q(Ijcuhtp0o#)2iAJ*>I#uYwicnK@851>J4 zc!kJb%iL@0uGVR|e&?eLi_rO1adj@7M-$un?P6C^(QXU?2nF+7!_G2Vt1FBZko>4# zNbd%wj(F9eWvRlkVh$XMsNnA0(-0&vb)FcO>>59)Baa;wCq*!{z}@e2$^;I{>KtXc{3KaNyw&yy%T)V}1Pq!1vaTU!JlQ9nhLV zE7crfl)V^nVrqz0DPnwRLKw+EfI%5rB?C<0q6IB^fUz@`L3zoOF8 zemm1q7nb%~zknQ5Lk3~>k-#frSL&Yb#D1{gQMNfUUA3@i+$br%(O?YPTLW$Ft6*jxrq8CxRR03}SV?K=2MX$xKZBFC@}%OtiNCU(723Yekw0mw zl5&B&(Z5o+&B=r;(J7A}~f$0qMIb(nsBOFe)idG-_b~TkV=KNJ&tMDqx0ySw+yR@k`EXP~} z^knuj5DJvnIU`=*iKQP?AbNdfe*4$CgM|)dxzF$oWVHAN6Lf9kkvR+x9xL)(OfRToT$@2hB*sHv!Xw08yhZ2k*u$Fh+yYxU zK)u1~Ll?Anz7o!7edxJFM$G)W&o44x8aLIGWOwf_>7KCbem1r>xzOO0ijV?nBYBC> zB=`_>3&is09G@+0)o8%z^6K;FV&7*^Wn;c#l7dBe0`|ddiMIExJJd1}OFE4hiyMZvuTFkauA7p}` z-DCjD`08Ef4h!P5#;wSF0i`ZqqL zG8v3aO%?4XiQOGi<9)HU(P;K(?^eCN&G$p{>{`Ezk60AmxW~KeexH2!L;Cw~n`(VA zmMke3INHG+%=pd=AR2&#t_&%cB|rls<+zwB0utZ9w3lM7sH_al1i>P}1XDnF(cITt zF|m~zQ@B?{#(eo5^^HNeUV~%PeQb%E%DlInKm!zLm`PGVQNhsv5+op?_b-Y1)A5bC zyOU0FI~9@7(2^^RihYsAB+Zq}z{Qg*R_T)P_P3$F+AMfF(o^c{>wm~o z?o3_^3-8i|8p`oLv!Sj*e-!e zn^T(FdD<-WH}&OI~-`A=x?~fe$%CkFfpYSO1?se=uYbI&(2R!@EJxjlAref`|px(7KU!(1RvhjdaS zI%@jfNTDPmQd&i$FvX=zUo_rEGGn9V;a?Gg{zTgKJ*c;+IoJd1E*wUY)Y}8PmfXzw zGZm!9$~DszxZRu^vD5bND*gYS7NI5ES3IsvmbsWq-+Yot7R+|6Ck-v1U@sLf9c!gN zrxmiy)xWWs6im#LjBMA~G)-;ah$QEF4EpN#53g?3ni?b@Epx>Y?n$vHTwy@qY*-R# z8S;42hD3QUB872Op^6N5%&VDs@-mm-=iNJR%$zasu+9)BKP(^>c~qgN?~!IBHsIc~ za|0}$gvP(L&1a1oj8v71$JUid&yUn1JTraYK7-8>6jz|LoUeC!F~3bsL@{DQ&z_4i4LXlbsZ$`8H?P97}w*SCxzwS5`Wz z$U+o&#@O!Dw-)n85PPDkg=1BZ8-J%3N%plgTp}u9;FlyvLi!AV85l|FUzrT~Z05bF z^@Rmf{Hc!}A^JWal(IVp1)Am@>kM5?oS%Pz*|tzU%UeQ0)rIYpP8Zz^>f4B5{)-E6 zb5&FAdj%s&?=ilxx8HJp?Mg;Ia8Bu^LPPQytO|ELK$6a>rYCv7n8^snKXGbjM<(AI zTNmqg0S6*W^ygV#eIYmx(={QUAr%c5`~dqs(&h1%hyk;JxjTbyz+D}FU zp2dFR)Z{}sa7ocZCf@A|-`J4Y_wlBNVVrfr+<5OmMOoQ<3(?v^Hal_>DR~w$x5QpC zXbt-M`s!ef`nSersqb%_j8F2pT?xY8kbL-l#(k5!nz`&zV3~kvYdL?6XOYr`!a4n- z>vE2>O!bHB0p2e+&!`no+XHq$L6Lab`|JLMGc(1+I>|L>uZ$A&Y%0YI5MDnrc}PkC{!jKd^@k_%pI1*#W!rcs ze!PB>4sIWSgqVThVa;h|5>sPqLDz$KQPXIx9JD@z0e>yQ0=bE{Hb{X%gf&df`$ed$ zfOyeFeL)yx0FWiIxv$_2r~PF{k&|@_ zjv7)tZ4D3R5^GOBLk1I5Wblgq>D-K5$OJr7oe}4T2>4tOOjDjPo&gs~K=+mKruk4b z#Z^HX$S~O4*VEPYcf9D2j&Jr0J)SNou!6EE7KDu5Dcek|kho>xD23#Srb5PWd zz5JnASyQiCXix?dGY!3Rbp{;UAS%d^{jO1-RB&U{&eA~OiVX8Z^aBoME$890gp~Om zyJ-F+X&U+$*4CaC#p}HlLPA4Nse)#av-JFk^>(VTtr?4=zD1XD{ zz2hPPzgtC2urq^y1^Ge6C=A1Wa-(Rr^`!u-sL&G`u{ib4NN3M}E+RM+61HA0!CZl; zxu7tK*QrI9gLdC# zyN@u7HP}pGj5|xc|E!w=A{d7LlGSA6xFJ-r#05sP&gKf1%Jb3i6MIi1yL!WRxNTga{KdBq%NrrOm9(-K zr7EJT52uD9$}7s_PBvb;vqi7->)WOD+;g#`qhYViduwTO1Y{bvl6G~4B6no} zrp?_Dr>Xk*wSn&tdZATYnm(D-Aq%nDytnaY&<1LFxF1)S*gkRJ-&*|o&{i|uHIuoE zbPugA7=aZa+~M!#_44x5Mq|}V!w>*Q;?j!K7Eb(VR%F!I~tW^ zOWyC|e2Y@ghS{GAtMHmP z3o<1WX;rl3%2HDiKK#x1ujp9*JUoUI(H;f+yL0;oO9dm)d=Z3{1v-yLLd?0L)ODm--2DutmP z8FSy7zYU?2m&Y};H%kKljfnuOMEbtxyQq`@3UPzC{SrXoj`c?Q!V4-RJaCky46WY_ zvMfZGNc_=6nPHcbiM#OG0%G~sVNAw*Q1ZTqD2C<-zQ1#y^Y)esDdTMzTmVE~IyYT||`4KS_XHov6`o&azA4%CV;h^ekG^9Za zIZI=U-}C5$d-!L>a`u=%+2l}2vl;W?f>N0f_l})><^~=$a!i60?e0EZoEk6y-(aj-W)A< z1r^***xRW@IY3h0YPETk&p|fP1^^Sv*sVes<<*wfx6E~(ad$AGtFw#@d>f!&7hm7; z91FMAq`+N@2`A3$6wy*Y^CiD_Peh9{zn}YespEfI43XOwFrm%j#d%AN znUK?GX?+`hXonMfw`h~YU=q%y7OBIp>+SzMg(}1XP8taFp298sxYCIXRS;DQH_sIt z&POBv^Klzm1i00R@Pk}q5s?R|gT+COl~g8zk=r_EYS5OAKOUpmx41$;Ci9ys!aTz8JT00mhtr8Y`c?fuyt8?ATxiHW0dIyC%9COdMD%uPuP@z(|8 zmD4h)q(_Q0kv3}&IDMw#>jvGsZy4T6Hr%eDXPfkom*j55!H`-FpgC`%NS{&wjsNqc zQF+dVUxy&)l*xKEZO;>>O*O-tcl1dkO#OX{Fr;B{(zI3^SsT(kk?yyK_fZYGw| zQ;C?yJxVzSt;tN&)bBf2LwLxf;NMbKQBlQq*07g6h(=jUGK%fvyO)utPpR3MGRqG2 zm-F*qiNrp88QF=e9C=5?|TX26( z(DMDBg2+Nd&p{UJs2ND5mAGLvv=}3_DW{LTrH6vQo*4!4?9TZ8IAyMu<|cmrVg{U( zg(cS}{XHU(#@2j#h%H8m`xK$I_KjJ;|~@AZRw)P#=iHW79mn#H?a4V7VNip1`lQu+d4SaOSXhjc=0lmoAkNCl3d;D0HEH_sVQ$0WhHz2fvRC2;p7ct0@{=foR{GghB_&AS z^q-18O=*J_+S)WSycbFy0&u1BH&ttM152-X`gzhwGd9kMxvB?7*7qY2Ni{;_SHg;H zzVM#0H7WKg)Q^Ln%A#hC7UtJ8eJ4*16bcTB~gbzm^5SA(Nw_1kXi$rv zmC_xx`!5nDmO+Pj;hq$ygvUiTx7=5)SedNgDoT<6)&i)9cAWZU%tIF+Ik!qIA*kv& zK<0lfu~)x*qOnmX#0=yS3Niv09|m=te?E`aIz|2DKA+Nx->UMdq&lT`LhuqQaYN?Q z-Mf^Xj#@IE*gj-nf?yak=CT;6TY3TP-8=l@tT@Xct(aEH1aK(`C~{1|58jqQ#Yn?%GrVn zBT2*}id^+-@p5)9og^=fFuY_MckbX7TRc6JY8*Km)tQ(%S1f8(*@j7p!pxAfP0yNU zjerXml&=WbU=U4Qc*_$SI4^Vju*QFilWV@ndNzGQ!aCUZEM`;Jy0@QgSUdhKdAI zMv{WP&lhj?jW(+3v&K$^vRTrvxnh$g*_P6oLR3}OKB~V}HH(ukR;Ow#p%%?m58Avn zCf4bg6N=B5XK$jkFw9=mKdnJrpo5s{yHgBY7f-`C3p81E!6h2u5jdj?*L^Q+CD_=~ zk=V@N0+TDu7J+bJXIb((c@%p3nZnsVmw+7@-$R52m%caXWZyvw=HV-;8ZTvh)abgH zgkaF$bnP=KQD{{aZ=Fg##<1v8dY3H^nJj%X(5TR1>bca&ZQvZVj+fdcH1y$wa6Z^!nMpV*@ggcm!fiNjg|WOiU+vsytDB9DYGO>&QdSF$FK(cp6l&NOknG zglLrSS^XTork7Rn8vr+tA{7;(iSN zE@^RNiaCOejPa@#22YRo4}WcXrzB{zYMehH+jCtv#kau3$$bjN6}>%~Kbm4ivcmo{ z#^}Anh$LI$#RY@wZZls!8s7bhrpIs}42AqjO-%xYL;K6JPgwS=a3kcAE3mwv#!1Iq zK7}8Hd_)E^p&He9e(A`y3Lh#I5YW6SLaXh4=h@%Uv-I|7sA_*7%OiDqhBMKKWNN}U z77nE3@@<4Bi$Wg)(Rn-<|0xGI9Aj+ZI@JYNoN#oW#GDD1c2&~rVEp8F2~d}-WbXtLVDFf1-{bgBDn?3sgd z11?^s`BkMDbU8+{Q^`j8*{aF|mfR%EM?Wnld3Xb`Hs%J$N*`S?hN^hxNCQMW)N;%Z z9v>&fZ-v@2)4K;NsJ%beUf90s3P-rQu;zED@oML{BmC9JiDup@5u+j*9J)8uBQxXB zCE-+q7Nu*{g&K@3Pi=^s83Hm2`re=uQ%N6$K8ydXO)fBO1BYMQ zlgn-vtx@pzM1Jl~#M!EprcSj9qpzxT6bJ7dN#F;A2ibPGz}Sz#+VaXYi1i{4zigbN zuM75-q3w38oHX)*Dc2_Ff!cW7=<)4{w;q%~*M6%8c-+g#y$f$0*SM znXxIjTVFLs98halGR5n7o)JY)5BJJdL`y$LyU>!BhDr10omz99nM9M))Ue}Jy@Tr| zu23QSuq-{b2ur((fNgxgdFhjxcB=Jd1JS5AyE5*xH7O4A9X`q}ur>cicOT%Np95aZ*jJiQe zyJA~5A?}^1U^b;065mEfZYOA|uUwizrA56QAN-@v_mDW~?0-X>-7b*V$Pz8XO64<) z*c5KLD!8KgKV77j66iJ+PzP!%Sc-#1iJ;t6#GE~#+~l=C1Oeqc!f{^leE!z=ukT&V z68eSH6DvNS-{>IMR}lUE)uRcvi?jiTyXGh};r1r1q&nB2D4818U^C))uvYVQ z{8$Jw`WLIW*tk5mEiS>-Pb z@9$&`Foi2E=xa`Q7yIqPUhxv$=aLE1BepxkV)EN&sV>?6S0EYCGB#ZLW*EF9kRjiN z@pprB&>$pDJ(M)(-v!3RljmzOBqgDP;<(6M=%7Pow$AjprmU5UZkl7K3dJ>DTvdO; z^i}}vS0&%^dS^t?$oJEymiHKg@%aZk1UmtwB&i}^br$_4fzmNGhUK7`qX~v_Dse1b zH=Rm423*zAlv+FNy6Yw&(mL%= zK=jQgwm5}C2`YV9@-46|0_EFIpnXB=8-$U8a?i1z{=ly|TP1?x&rX(|FAr z68BH1a4C!>Y|fLOQaVMGG{HSr)eJJvcYf(S*ApcFy(q*7AOJV1c|E)r)Aq$WeE`#R zm=PiY`|T|^C|(qbqzcU|C?M%}%-(wxs+jI4Le}3IEX|~9YUo!*aDzPgoNM~+#O1nP zi&1-t$w^QUu}5~H1Ex#}qFyqxyt5HaYrbDOxyg6{htb+%%>_82@_W2C_GmJ(t=_y@ zd>$jItOcc#X)&QUe}21u+TQsuZJkytjSTHoU~;dsQ!7ARJS=WPWbkbY?vrVGQgCFz zCzzN}o@waK#}Ld)MZ830@|NG3+ffijVnD)N=*xxj#6z`)P)u)fc2X#t#& z&ghXemr9IS?Jq;Br^zP-`ub4!qBQ)lG29vXv)xHK>8QlyH+3-3&4B4DL& zt0yob&AgtAV0@>9xeI!$RBJf&@8MO6E1(eJtv}3v&Q6T_k-mE6`vJgf!Dk~uoIwK7 z00qUl@rc@6$cG%^s@WZ6^>Jv0Z)gx{VmLgoceBlZBp^^%Ns8|eSlP)TYo~%sd05b> zJSUE{z#Yo-w1pj-2vF3hhRubHg>yExjh;gr{*(_S5{Ym9aEVJvM2o??Cudy0fS9gR zi3|6@^G&%xseJJt%lPJ?;9AtfiByErU_r5hMfsF}4qDh!a5c?cH@?=5^yUqgXpo?U zbuXj+;k1Dx=YBy&o;={*bh9^6>yk4H%=C+g0!-xUq5qJ+6caP%dweM5rJJ>jsj8lE zJd%i319cz~iV?%DwLqH?3Gv(4M~16*)(_Ms?;K`MAJ`KI!GUCTe z=ZM6~vFoLTP)1)E7_A?}UA zVxxoxOVfkRo_P$;sCs@cEWDvsM;&FfdPkk{iok9}xN6m}+|(h6@2m)H1itmF{m5Xkxpsl%Q-fx)a&i*nGVETRII1ENPhKYmT z18?cz97&vth#*;5IBpV;T#2rWz3A-$xC)1hqrjO{vu`oW-AGd-L(cUGr;|puW4ImD z8>=ccJSB7o0!TS4E#omn-bnYVKObZ91yT)EV_;6N*#>g3yJIuI+S#qHML4<|2LTBxQxl}HGGv$z(cCU3R##Ed-Al=325DS?pa%;h z1MDgRcRApd`G*_|rm2w-`a_ne-0hBm#_n)+1~gfcqkHZivjjAEr*MGDzBwf#l7&Kc z5{gSR5>je<_qVn_co##An$#ZG=#KINA_ll|`=Baze{)P53Gc9N5~aaY$U-s zlW(4?_i4~22miDiiMq;2oUqYl2!dWQ5jxlF!{lTb=7)4|noC7XU5B%nUZR{R4)XJk=cIHNy9d$3%?*9L5B=_8%WRKV|+en$829>i+-#B_tFgRI(Dsie!^b zvK@P86h~Ib9@#s|vB~NXDwUNjl9iQBLI~M=^M8Hr@Bi0zU-xz0_tmA-`Mf``_v`(7 zKcA1MqIJNB>}Ee7XkxSvzNVHHC4OIl0j8BG)jzpL_qGL?CFE<%-qh2ZhsOPiJuJos z{L}=cXJ3pB%;JX{&nj?s+03A#(q`AzAsyo`01ag|Kdsn-e`N2#?I*Hx~udDBklH=occDa z_Jh=#OQysVAudkq*hUg`0XFmw#p#Ed)V#k-+~{iorY!&U^Mk2nw8$|$2Pr@_BwcZlJ}$FsHAbr;o)v_P8QR$>vQgewXL78YkUu2Lo(dOQdSkso}7 z#gn`KP44wv+4!w6f1u0AI2f=11?etQUvr^d#Sbw&wR1eVjSh{_%c#a{ZjEV+#5hludtkY9gWYT#m{_i@D&kMWRt z&f?cg&XUBj21U8Pp3=BW)WojlPjr7u1xo4N91VsmxoM_Hcev#mu!-~K9YQw>qaz;O zzxpf(50_OPg5B7s(Y#I4ew|Y1H;(0-oWS$)0;9Q;l+Mn9HpmAz?_@&yljz!7tZH=W#y+n*Z0p{s(r!VzP;1n=^4nB z&>6Obdnm@Mz0{EWbS#0B(d?j3hUi_RHJe#stWA5 zHmlG7SsY-}+Bz{yZ|U~Z63&7-{f|pRnx=rdtip=(;}v0KyK0VI&VkwBe=|Eszyt1& zjZOIU7aRV<2L|fL&z~`in5ezM$}9osI!YnetvUlf>i3i@NV}+bMS&v$+8}7}XIdNt zOw{^UOZ9vDgS(9t<4RcZL$Sa}CnY$^BMizEqf6B&tm!FvTkL3VHW8S|`pA>%NHbYI zHE&x`Z-XGOf58=Q{J&D4MM)HW%zq!Cus3#Zf=pLM>4J*F z+vrie=R8brWb^w{)bg2-C-KTs=#yAj2z*fZHXn`Xb18gY`3vHxmx#(Edfc>_<{cbZ zl^$nFHGS-VHV%;DjWUTOkq4<{pg=^U_n2V(Vxj~vAXo?~cI?8>d=jINRkjquU<)TO z*?+e%YUY}}dsFQzk@yJg$RlfL@aeM%w%(H{Wmw5I*O;ClI45^>Zt#o@=B5mI4WAWfvZ)KvB0 zD*LT+F9GRMycW6t<_`9PX=?=!4F(rnWmI^)msqvfBA>??=l5L@fgb^*2qqj$xF8*6 z#3+U-1jQ*kf-`jfH$GO(idO(ZgL71am7M)VUF|Q+J;I0l z&C^d~TY?x1g4z2>g85J2x%%0g9&vtq|?k*-OT5dNI*vBN( zI=y}bal_ym{O-^GkR4w%`kjWki4^Wgit%B|2)tY$gC|}mptp1010WKnA^*ZIKA$+u zEv|#;r5->4)>^h)Vv+{8wUC!4@o(Yk6TShYTmy1p;CsyV62DQPF9-k=3xvI3Lv?Vk3u>cDQDLPb z)})$st1cmST_Tl!4pZYTcXs@JD3JNV;TK#~$y}9qL4|#)Eu7Ilo1C&OV%)%z zIpDc<^4xx8WaPVPmf)Y@d%+L4$oVDQp$05|fV=_y6AU2mZ^#f%9q^*}nxF5@b zEiIB-vaFTAF+L8KO424I4p+ueHW)C30F>|q)Tw}g00F7Oy?&CdjeifhdZ3G)BCt;& zs3LEkpJbNy;A6I^5Bc>ck%&A91c5LGvyX79x|~=StXzH&6F=O{jdb+zPkx_MF=w>a z3xGx`JzS7g30~Ix_9|jy$r*iH37CJ9hf7E#;FO(p#Tddpcsl)kU}&hx>;4q&0Et!Bhi3i;EZx`&KzJ3;sn^%q+nfcxz4e+Ep51#p1u z`k=K{z^wph$-jt~qX8lFcBWN86t_Xii6rN)L<0l@{gPP_JFyetoDyXc$K4cIO5}Ho( z1sN}r3=ISX+E5H&e*t3m=Ck@EzE{kkhVXcq8Lik${SWROc+AD=!xqmcX*hJsu2L@fA3B8d}`=H88LZ=n8spwC%O%bCtU-8^*S{0EZW% z!B_&9%*#>@x2BGJIKs9F323z!XL0X%-|QLdu3009H&<4$4G2g5SkgzmW@>m4Aa z5fo%c>vOqH#|Tv!e*koySVk8h24zkD0d?j)v0pKKt(~a?kUbwZ1VI^RnYIA$1%~{>L62m`s8pmsVl*}5S78% z$Dh$=^`UyD>RrVC>m)M;mpP8^ge$0ffb)eA?K`{8G`%xzMA?uDcC^`zWQLE|`%C^v zKmxJTT^>#C`u!TeXZejNtuvIIm@tU95OBfb0A1k)voy^1T|XTCiQ;Kwd(*doq+ksc z>Wq(%)5lr^2ONmVYru2QS4|5=kk%QnAb=iuPyui<$&+bGsX+*w*sC*{0$wq2UBIl+ z^zOz-`0}AN!81Aiakrc9+O^a>mI^vLS5*6e4H`@X*u#{(1pvn$C91H3nWx(r-YeMO zJ>Y!;W^oXCWAOyQ!BqS1T6YDXoB1T|x=Q`7N z0o=@O@OkDPAXjFwT5lXvfI7#@^c*hH?RaP0)x>_-v~Bcryq)J%n!}r()Vl+v>VZZ9X+) z#f%LPW+sdDXg;N`p5K*a{kwO35|RkutpEH6Xh&%zk;>(xzH&gpvBO0F&^ zbR{6B09VoDlFVaBe7Q@y2?bko4@} zu8_;^g;(tUtg%6Iw`gIJwF2|nskyeJM=epdr=LH3NJ@2aCLz6*fB$5NZe6~R_y}U* zS7Wc<{R_cpkh1>vZ5J9O*9i5LIT&KLC}jS#zssK0Ab=Wf_m zJJ!VM81)Yhp8V6RKWJ0;`}ynFQ4qrs<6;X0A{=Ep=fXSQ3s$PnwgVjGE;8f4hK7W| z3%Af>JKU~RUl79dN;9PlE)+FsZ6?h_mFJy=o#wikd7gt5<8m^!yjxSea+skeG0nJC zp8WcB{{wTo(b5NB(s{zPIgkbc;2Yqb{P*wQ?u0+2n_7d39X?_h4Sb@5V<0uef|1oB zeZ$y5`0##UB@et~_6~kl8#CoyYAh+7{=mVuG;jmt0}R&f1e@TuRS>pzD#?>;5}$ew z2Anh2>H}nKIk3u?O1fpx(nOsZVtTX2S5I-GaZ-W!f1N8$0mB;H1HVcC>*uJk<*VADpZxlG@9f557D@3pBlT}5&UPES@(>%fiG1)KU40JXn~S3~D5 zBD(?L&pH451z*G`=UFB8HfDe?K+@}#6})|*F<$UeyoDF=d}1{d;s6x0z(%Q=s}O+K z5efU{1OPIDuCD>ev^wQGg7%~Q&;Ds2WkOl-8@yN|m}U>Q%Ac~0@ZTp;^$~c)6hj#w zw_MNBR~puvKf5xBHUIACgo?)HVc?FZWCTQ_>B+EI;1ZpyL&iY2awPd0tqdFU`|7E) z;3qXvVlQ>zNk;pO$9lqh$d03Hxgv|v(u$XPPL1G*Sv$dV%EXc?b@-o5YnYl*uZgxh zLE4f1j5~R=%Phj9hj;N;DJDKOnW&dicE6ez=l{f3h6t|IKQAGj)}Vp4Y6_pc;(~p} zI8rD)reoe^Y8e?FhCwbFfF~OZusLtz30kB)Ui(8=b3KXMo1E#zG#KBDL((qZf$$JO z7wf(^6o9~`t9KvMiMkT~K7_9sb~ZMo@`5YZ#l-~(^kjZ(2s(|+tPY_l5M{&8SqoOp z>p~7k(3RkMQ|ULy+(dR@Bgfij5M)3~O8O&HlA&CwbqWw8h@tmre{)K*7`&&*F2;E4 z3JS&(;4J)!z_7WXh_=1D!l?_lV$@Dafp_8^*J=Qf+69O+_(#i&)*1jfJQpUny$1Z3 z;MuqXsr+=;|8y6DH62x#!^FSw`~<*4&Sy@4@nP_>AbA?8p*A5ymO-?Ouf3zfa+$70 z9?lm0|D1%^PJ(>uTT!B#obXUO;qEFVfcUAcm>LC6L-j4M()t6hM{p6Be33!a)dm=% z{Hi2mv39?{#LamQhfS?}Af9Tz6so`NG{#O}0lhNUok>RdgnbP|(QCX9EZ03q`HYKK zPbfiCkoB(x(~h1(YvtzTq%mJQo6ZTG)mh{W0C^8J00Ia2_l1b##syM`;pg|uNVDM` zKaF6^yIE?s#Z-SVQ2g)%1g+A*eHA>MBOrNK9q){R>($%)5D7H^ezMu|KRxkoq)8F) zR(~ySuq6v5djuMM1nGGz^Yg;cAvk;fKk`!nz0K6tiER=jwpwj?wzj z7P6;qbny5Cpi17~nwGU_pG5`^eg})i-IAw1M@a|?8IFCvu=oJlV==`E)(JV(Z?eAx zCTc|L3XcY<(#%YdLum2y=Lq#vNsQzWZzaDIh#k&^7I<$y=`O)lt$r94Q=<- zscgAZrSV!O^oIz#8$|_hmLWJ#F?h~Uga~Xy`L+gk6d)eVAmC&csI~|@-FJ8VXOi^s zYW*crWL-mK#bufBx&8gkIV8Z3u4SOV9~J8f3#?IZ z5)3h?5D4K1TnsYtwFUs?_(F4srXrwJQs=mmE^&Nvo_^T8;$|=)6`_JD^E}IZmbjF! zNk!1&0^UqBur)$5FSGbgxB`h0pP~_+B^Cj=|GG=y&ps zcnReKMXnSKTB%y*w5`%SY2^(@AGZ1;8i&CqwSEgclOG+mRgDn^UyR-;JQynV0u<}N zJBq3=^;DxQ81s3x(bum2#=MONhY}JCc5r6|{~d zx4%pRBT>KBtE|!36GnZtMS99}?eNJYCsiKmmVUBputJM)Ua^Si-fimY(xb@10LZ<4 zaeL)=?b!4>5$F+5pb>zKzBx|`P8`H903subhR5{d!?VB5iD@$whq)8)@vQpTKO&Qv z7YHUl*gYA6^SI-9l>j*rNJS@({t7`eA_3S>X!9?e0uhHz4e|;8e1iyq>kePcf$9LJ zOQLMpJjP+ysE4=uHk@*y>~Wjb)xZqz^XCm1R3SuVIBZ-iFxag_a4$gk1yZ+-tt~iR zVcUZDr*T6w7_9lE7cX{ZN}738Z3CYeb}JL8tWTa4;eDENq8*|q_&WaU7u}6e4Ra{4 z;+G?!hYfC;g2E^uCH*&^E^Gd#V91sixm1q_TZkP(auI(ppO0+Z*ga~axH}^{U zHWC8>I3|fl?NllY5K;n>X~;5yYvt|K4RMIPp$+w%3^?ylIZK_)IkxN;Y99DCODw~* zlJBs5B*|3iX!X>TjO1)iHVQAo(Ow&3u5!Knen6 z59sWfGWqS4WJW~dl78VbzyV0K{lHIQIWd<`7lj94D1C-RKsdsAfo;ins|H)DZs zl8=Qg@PAh@iH|z^ilP}--r~FDy}IH}IG&`!3Wj5;mv`Or(d_jTj%9|~9&VWLDWI-W zXSw|KVUI&alR`GyPe6m%`*9pIj#mVd4%G5XKK|s#oh1@@F%4qf1^2YVe5%M3de{_O zUff6%a!fpI=u8u0kccL)^4TL3qwu21?tpQ|p{(p8lA8c-TEM+R1-{C(HE84r!C>!P zW+4|;0#3|;GE@LpGm7fJcmR?dOzpWd z+dk4IDY244A+;o-K00cqQ3nWaa8Ih_F{U%~q}0db@`c)V1v4kIBI;a{@7lW&aJFsHKO@l{lOJ^&CaoMO;#<@%OlU$o9(RGs*c@U|E7dL>b={Y zReyYO+?}B@^XT-1AyTfV{sffX>37fL(9;thHxrcB#T`HCUjl}FTnj^S<-xx^w%5aZ z)he_3oAF^}u|6|#CT@q1{3NMZ9qsLpN4ovofB}f)k6-Refa)X;p((JD!#EZmw$H!* z0@5bvxaOgU0kUp`li%*Qia53WV?Te^c#1M6u3J(Qs0kck1)ZE;=3$D5l%3^IKf^1E zWtS(K0=`TbzM{hHI(IvwfPz`saMPr)nx`A;mxxN8n%u07)@vG^Okbdtgfi&+vR zW(J3bf^>hE+ox^yiPNi#uSlUd_QvFb-Iz-KMm}#Ys*CKw;SJJ|(>fuh6+-1D59V{n zZz}kRKN-v#Ig0Bj%PP1~A2a=hzGOc`^5l*HZ-bt04!c&*Xqn`}@7RjxjYlMS1s` z2N#PapY*?tp{p=I+%a+a%NVSCv-BDhi=>|4I;<-wi!cmyhQlHx@jz3gfTG&)5YLB< z7U;2c-+Uy0&ylz&(!Ov$$xi2!B2m6j66Jk+Rc(le%)Ehybs#O4l zg-P>aJHan@!fw}<*%E)3zI-7U0I9q6ceyu0@NkSiUzp$qsRg}aOYI; zy5ZmWG2kQo9qiLgZ$4M1rh`t98MCA3ayd5O z3_;pb*tmvEFVTO^1))+LvFjmanp)V`fwF}34M%q05yfw3jiIw{^6esBm8F&1>( zFQ!2NR9Y8|?*?eO?VX)QZ?Kh#(@)eMF22iz*L;ArkJF&q<2&Saauz3h($l==pncDE zn9_8|?V;3Aj*I6u8|5gZ(r7;dj27t0rkv!Mx zyaL2SifCkcr+TGu;v>_^i1q17gO<8{XXmdMH2!xoy}3EwN~VOvttwpoz{FKYOY0n)#LVwKXv%?eUMys%cthf7 z3i8dqIOEV|+er2UOdshXll6%mF{)ZBA9FAhubz0Qhwy;O!ho1^E%iCc@g(|%?p5Kd za7l3)%tTH}Xyl?I+XP}lfg{cwRr{;yh%m=Wo6uf5OO1*!6;Q8QSHp?miiL>D;u&)7 ziu>dZD$MvcpkTauCIm)@1bjGwF@fLX>vxa~G)k4ga2xt$AlEfjgYTJBQvOmM<&fEo28fkFm|BaNwr zx#mh!oUnZM)~8`+DBBx{Pa!HMRu924IOXNL))GiI$?Q=iOe#b&x*9{-@C3P?-rJ~3 z=o&RNz(e9Ln(?+}*6@@0mE_yrz8MX4$_Tft7+VHk7c_B9 zRHTGphH2s@r;E6DHvpEou-myXBBmZ#5eZU=L1^ZJ0MqkNz$|pGqzwfd6#RYR00-R% z@8b2VtWJXl*)Cm`lNs(WNH3jarmk}Eu{W|*H?N)o`jh{Ps`^ZpxL(&2X}L?iHpnE4 zw5fGKj0SJ?9lY5xA*e@EY0m^0(kN@P-rxMR%XY161QKzU-L$A~Z3VSG2n%MrNow4O zmzW;>#8Xa_1Bi7J#`o*g-k>7$7CgjPoGW1}bC#_VwjKa;;n4Csw|c2Tn27#{$S417 z?t?6|*@6k=isM$P)U;oOKMmME6V1@j8nO5!bz#aM&~n79;}9HEih(E*2}fv!ZRLsgE5{MO!*OpqF8mk-=E2%mwg5C{`yZM{hBs18 zg*9Fy{>p78D#5P{yx>&3cLRY3GP1&mF24^Wm=+QciUP|^oIYP~g=J2gRbc)F)RpaL zKOzy@TzR>!>b-AZe7N&emm>sao129S2}M1Y#(!>U!I1#6Putt~?Aq@DuFr-JptgY1 zj=ahw^(9qh@AyJdVofHwa+$H7Xnw22@HU(Yv*&Koa=_?Hl_9AES?ioful&m z=(-F>lxw;_a0q<3` zQ4!AAz-}`yC6FOW4x+xv8IQ}yg*q8RF#FiG}Pga(99$K0z$`!*@9@ z#CSI(_n<-mlWAJ`XtL=&I`F(1Hkv3?QBf6i&{$$i3i7&yG??#e`;EML5Kp|x{pJqE zj;u``7kVZzoS&8co)?mN+FoB(TU-0-(|T^K#J{B_D}x=FIbj!dz1rFH`}_M_xA8T} z^m7OT)b`|Lx=P82UfSX+ZUo3; zHi(2W(A#%D)o8qk0J4@wWmFCr3Fr1hLkS6t8mHHhTLywypbXLMY%f@uP?2I(jwCC* zRZ*^RU)6r)Vo^cCRRLRQA++*TE)WL6-r!U{4#m0>`(T*Rp=2bIr`=#Dm6{{2SgJ*f zB)hTa@T+7ZRAvMA5S_~zC>=$G$2J$F zSnHRMaw^zGmC@Fsh_#;GP-sO3;~Peok_Ls9yM@06564eoLc7U zSQ)D*&dITWvkY8q!G*$jt#`$MEsV$PL^C9|`?aEYTG}>iKB+|c1fg}6&FarjIJLcf zE~5edE2gU{;6-iXMI@m5Y=2zm-KwvKf<=Lpu=8<=AEMhuae>TGTd((V7NDyvbGqlM zVShkje8!|J`AGRmz z!(-A;mg4E`Z**#FwG*7P`+D;{6Hb(!G(GyW?pA&lQHI|FxdRqr#|a9;YtvyXrMuKJ zz24rglyJW{_<1$8;N>O7zOQxS5HGs`L3Q zfR*Q&Y)?*3BBBdu3*Mc2A&ni(zc|=Ci_X&0=gZ8o*<`=ite`_=%hL)}yy8>B=HnOD zq60tpz1My8QSA-pV0bX%Bc{QM52O{xD4oHGnKNnByb`PXhD_NN;msSHO>N%3st`Y62lLmyhu|KtdT`UU8M~Q z<(yxCsF_gGD*ZFJxm8uT=mC--!d|=I8V83$m}RR`W4T!Ih)wkalJ|h)(wD1nugtyJ zujnq26hIYRj>1X7?TR@cvnNUZZ;&H5x_)f8vVz;VxUIu8(1$B^-3mQRAQu-&{G6OJ zzK!J_9{Hh1!yjFa1Yn#Vm~_3vexH82jeX1qdOgT=0r#}xAT@873hOzAln)CZX_dY< z%z4u9^F6 z;mtS#%a+K*%Nu;$qxx=`-T`*vbKRQ^sh#fpLOc_-)!fvwk8lvo-ZqXS6s1RhxXs3_ zWd0p=Uj^R+0F{4~kaPo*VzMxZZ{vult-T7&@OH*yrjNz8j zN3@n-F1fuEe@+@q&SFUv!6Qn<5A59gD<3Kg7f<$vJ8*??bzP|Q#P`YS z=ELfIVXZxbgg*wm_-syfPe zHYc0FP-04-k9lkM*=-yRY+Ti=r~h>H;`l>y8|T}fN0NERpG=f>vx(Un)H_XutR5Z= zIRa6A&h^BFLV5Kkf8Q|Ga_58KP-+n?c5a2G)I5k02a_`;g_{NS68z^9AYWJneKBOJPO zKN*66sTI{k4CIildWz1viT9<=A7e)TdHKGb)w+}I+{A6MW50{bAAGrNW8Bj3+aGe% z7_Y@b65`+c4DxTs4V0#ZB|zfX)%79|vu4WV6}=f}aW7RsdExu_P)zEJX({jI(M~|K z6U%g=@3h|B zMnH4axIWEm_-Os|Dwk58{0Ea?)g|AFdveQtJND=5mlyK={|J1cddoM{ zjXzavnv0IV^o(UdjQVF~b|yKUfwD4izr?eE9{R%~^BXGjI~;hZZQlw0P9V*-+-b#J zz$p(p>+Xx|=sYtCJLDn??Cb=S$@w7Z{63~DZbcjCO?(M4WQ83mII8kHDP5@f-cPp7qb>vM z@%r+eqj)yo2_Az#8BIC0dAxdjL$bfh>Q2&8hHa)tW=X~yJCM-qxn*UQ2*0jjPDY`gDPb<;Ao^1xZo(Q(Jd8Zz8p2)@s9ZMcxXzy>^08Ijy_C5N@%| zlw3x*2O+B3)ziX+*(_go@JJd5js4e7-GvE4c zF>r&qBHjN))yT-v`v61Bwn6yL)jQmrR=c{ONy<4%#)bkv0F#=W#*7ZS#A&6Y6LOtV znVm4~Y&OA$h1}~}y|^8Md~BD1eRQnMy6?p2cO~5t-n_KrGIT8?zkY3rlXc6{Y_xF` z&r{%Wo*t{gUx5?cxIy@|;$o?yxeK9^W`Q})!+0eF8rl-QH z>x$3GW|3Bv?dhrxo1BBl>CuxLbIWXMt)HnxuSd%&UMLFD7{`t1gte}!C+oZ!8+69X ze4p9q3?Te?YWa&ZhS|^ z+rNujL7s~J(dI7KPq{tuCf+QF;N}=KC&J_ypSzVB9*IsLlZ`|T$%#woC-&gI&aF%( z3d^h8ZX|(%DPYg38l|01*>;VjKsJglxZbeg6%X%xs~HQ&wJrI)7&4r97VVWd9JbT2 zYh%EgKTwb~=tK57!|icGPr6nD)i)P^;`DD-*lb@VR5HhfELI6uglROm5f0ze`HZ_O zrpX+P?)k<`x-P^PI$#Zho0amYMY_2Hhuxqsvdrr(-7(Y7O5NP==EZ+6`*oJPJQ3%7 zt*#J5!2V7`wqwq2*Q1_8Iorh@LnZ$a4_oS=)IPvoe}JA}(!KA1UBOvsex#cEm z3j*;MiU>1AEI%d5MA7YqC4Kzh!xKU+;*_UdWucwBKo?YZz=>h@IY-w-8JayU+*9<6 zUv*TX{Ne{r+Z?C<9yum#`{$oIuUV4)t9ey7M;EqC`)azLDf>;GCu2DITT!R8@zkA8LM4U(lNTZ0`z?znhf1{hQ?0Td z-#ni5YPT0*1{toDwxEzh{!XF$jBcL@#om%m&@a0;WU76Ppwq_Y$=a<>;-qh1?qPHv z$TbkrszSfyBv%dmMTdJzC#ye7h$El5EHmjU%~~HXV)L>{S1fh=7Yff?x?3PadBMD8 zKZxNVJFUE&hzXGf?m~9a+;87qxVChIdmosiukQM<_GE8dh!eZSz#427L+~qeAolSU zJkcT|Yqi1a)^X^A$&$#e1<1m?Um}>PW6VGOBl71xv*a#vO!6 zWrZ)3n|@g^Vx|1O8==lpgTg^cp-ix`>7B9By@XYwG1O+5d-h^8ELOq`*(;`J9vB|F zU`_9(97#4!!44@TB!tN8U`pD=rv5s5I+@%V^flK*?|{mAhRx5S_x?;Yfx*-56e-?x zogbR17kSbqaChli$ZyNX!v?7Axpu;?5aUe3Dskt@WwDG)0ueB!$zI(NCrd@GnrABZ z3gY-npC{Rs`tTY1mZPXGWENhNa-+Ipa&EGz`>m}_DPZ#c%tN%EQf-S+0y)%?LIffvv} z)ViwX$80Z{(eWGiQLk~cX4IYi2HVMD{K{Qxzi}GPf5Kf#*p(s+ZGW{l7ge>X2wpfX zyNYU3Reo4Jl+b_dK~O*;E6^|JVDo@-N>5YiMhixNDYFTd~@0)6Su2JJhV}gK0E8o3l8`FbqQ;4H3&EH+I44HTR*U! zz&-4l7Jh8gGj@h5cf@Y6@GJ+KxP)bZU3GQ=(_2G3uuHv+VU`VJN>aB|r!>%Pm!?d4 z9>mI*jg4g$JkP0WY{f&`^aWt}i6_DteyLBm!nLyrGLOeD`mUw)|>FI1iE32*ZW7*y>PF&wg#kIGZ`kmc7h)nwpp zZZHU;w5LVlrDud+z$id{9Ku$jqMK8CEN=hXpLE@&ZM>T;Pa%LBcl__?=S6H?ZrtH$ zc^b$_6qpt1wknjVPf^C+Y0Lch{Zw(pHmVcD{8(bd_D@_qTgi@$Ce`qj&s3UJ$A9e- zhj#$&FPxD-jt-_CvF8%!IVhCS&sk>5Aw5vKWLbYDSM+yaJ?miK%JE-HJ(-J2xUtce z{iN zS?#vFW9#>(xBa)w7uKK3KvXWdlsmTEn9ZYIy_=@~E=E3&Tbcc`sxdK3s{Tq)i4XD6 zH|ICiH;z+m{(#uQ2X2YrhHk9X-U1}Z{4Of}9ANv*FS~jp zqwn%Cb#6i}!DUr0(P&=MsqBzHExMFOc-s+9#sZfoOx{qHUY9#xGT76D*T&8k<>Qp! zcXr`yCX-Kk)EOZ*^HRRab+{e}?KPAokhDD@3cz5RcpX5Vwp#P&O0mx@C!(B}Kjyv! zA*_&S1pmX#6E4qmbjrBE-2lIqbE0mK{{Foq&hx@jN9TOdSwp@HgIa2l62qG{@oega zg%B!2C{Mvwq!_75Arc0IeYFfx9Ey00LCcNFE4dqQ)1oxBqTw$kO@ge+5OiwO2_a;k z@rm>@J8@K9CxiU<{MXF?QqBn`GUMu2o7UUWHbRs^^!Q()mbx5DKQugmA+qsdBhBN} z4YRI5690poTu92Jagh|GyHs5HHx*GjSX!Rzy2^C*$;PXsni=pd9IUw4D_`%uEW`U& z>k@xygC{>{t@3}3H#F;!Id+bY*F)nvgXGCJ9i^fu?#Zicg>Rfuy1)e9Azs?Ih1Wu- zAfNHJ1rz5<<_TUvG5v04PEIlw(3{552`_^Wa zgSkfUc(xS{DCHa!d}V5{@cFC*b*iVhK0!Obmh7y=UPIXsSJTr^HY)fq7h3r&k*7_& zwDSU65X|e-tDrTp+v&Y}+)@8*JpOcbdhWFtBHNw3D6NSF=z3EanWLRua^C(xvEP*! z*8+TR^-vsv;H?S-XP^%}iix@Cu`?~Kpopm&lk*g&%Q3N6vR;HG=Q>Z$#1hFou}c9~ zor9Z_w<$hU=V6@4WVI}n*h!7{CdW_;oE{`=5z^$zc@ZDUbabnk8p3p{AG~_*`E67C z%+K+@wMqH21H)&e*qQKUNXE+JS=N0S*l=rhjFPDW`Qwve&(Xo7_vmA<$Oo{tsQT$t zjMaiz2Ne3*uU6N6PuD8?>W@02=zTs?;!D$>Xw*-nbT0AuE(8kIOzBu>;1G!p_a|1c zy7j_~3uot6DnX^baV<4)<3Z#9uxIKe9Ee~iB1bEQ1((ojK9>%=S>aVnPuJM9xb1u? zC_Jw;#-g+_UC}K&Y|Z)^#UwF+B4|k!aw&|5mY0`d@Oy{qRpB|qb3DbuF#x_k6n+6_ z-}s#QZ*0TZ z>&q2U3UFe=mTN~BdO?&V%Yz;@hqLql?)I!WR4CJ2%yOp1{KZoKFq9RbP+PtkY&B#) znXR3urRL1}NxGT>ZP@>fRoH9GCZRm-!{TK=4!>i#h=uE)q*=+s_l~}c;EXadGUEI| zeOV1t<-I}Kl|W6kuyQBZAn3J8SirZ^Hl655!W-9d7_s{fI@{EI2Xd)nTJf~_XNh7S z8GQV$)jGkfX`|UM=aC$RjT~6XVP*bitg7u6kWcsdA~z9}n3&iV$=%9s-&_!fEO;q-nGmmkF{aeC|o>n z5~fmPAL|JucZTw$yefO|XEKlCAq%n#2kK&@p>4Q1^H~dG@PSUuc<3Ieh>^PG zUb|=_x$7di8{_DcyI09@JbxdNkn)n6fuelZtG3(c@7=YTlcnwxySCj4uf_Ms|1UA~ zhYG#_d$-P=9d0XccyAY(9e)KuIS4HogM-cYTKtb%GLM(6K>)X|mhS>O*824r$D@MG z1NMVsh|?}wtYX-2y+2tW3+Y#Ju|8Jz_A!1l}SIz6cH zTdX)4^FMUtJ@%jP$$kQEtY^jECWADg<{oe>W#zwiePWMrPCU6zvDFB^bXI%un>)Hu zz0QtI8R}fUa-^f&y-Vl>Mm=HN=V0mq0ZlI44_OkKR#-8cg?hFBps;i8zXlme-NdTo zcK??>^;Ms_(_@feO7A&N?1GXX>`!Nn4(UB-;!f8gtV{j4Umd_5VD~%5LH!s5H7?jU zK!v^Bx3IPAx4Ju-ABlymOZS@T8|C)(n;EOWhKKi%fVzV%jE;_D^>X=ws}j*Z*m92- zz#WE&-lv3&K_L_TQr-6-NOc_?9Q;61@4w$+rbfhm>L=3*P7ILWQB;+!f=IUL^PJ~z zqltq-)ph3fY|#riJ6RvrlA&V*v0ZtveNV1=jw?eIw)!+zYKSmd!G z0yFR5o3AI1Iw#T(#bCvN^hoto=)tRX4;7-TAnOHVaP($gngb&CN4#K&UqJF8S5k>6 zfS?kQ$WHw@pn2_HAlFRv-JmJU#d{5NCs0D+M^x`CGqw!%cPsVSartvF60o#v;*A!D z#kCIckL38=s;&=c4Bz#q>wE5ql4emQmY2#c{3hXco>ak5s&ci!*XjaS&nM&chwf3Q z{j0m6RR0NeAX*`p^Mg~j=;EiQFb2r#L*Dt`j@#bz&x#jMX4Fl$+V$$3S8PdnDIqaY z9Q2)t;xX0lhUwS27x*aTx(~F)Np)MdhHH&Oe)7OVMUKRarn^<3qTPpf-~i&-O;6Z@ z+`1p%MO`TwfyCzxhLl5PaDLvNzn6M^z;pbQ2VNp|zm0eo|9@}}JUy8>J(N7%l*}hA z+ItdjB1M^ninP(moj?Sae)a19ZP1EcfapQ|$MfviMm#Ph_ND=uQraoW)UrzHIVnsz zh4po-+il!CHR^9-wdyA-xJdX&=UeQ?1jk5o?K4}6`Agrr^K%L*VQd0UvGJj)tzkcR zWJnsmfB*iF_X~Ks>tMO7=7SeQG_5lSb?s6ZP?jgnt~9v}{2fLT&2pdeWV4Y}!75F!Huh2zfa-I_Uw z_4I%xnshh>uO816U5V`i^^1h(Oonn%?S4kZL)GBI0?WmD&WAn|{#`DwH}6Vr-hE1E z=Dl22G43`dPh%oiU%4z!+jb{d`D;&B#eMcL3+=z$JF)7oHuYOL9*`ZH2!XQ_Tr~$b@4wJc%6{)r<=x)_=ly*>|7|^g_|3$UaHUdzQa9y}GHS(o13ih!Y-zXD)EI4(0gER#Gek?c6`dz55C*msM3 zOg~ZQQTt!m==PxAN~U$zZNGP{MGEC2RAkTx3%ZwED@*8CS$yj`!<|za(KM+M-Con| zw;55XJn#8>&T~z>$GUiD^?c@h)yi+^8Bi3dq1Z-Yrc$Z8ve@^mTTdgh`v_ zoe%m7e)smYCoq=x2Ya_~=M&Qs2)JvKJZeH-71&S~B=_Sb7q-11qAd!Ox@MnwdNc+&e-kIAan zkmOO=>6-7Vx7o^KxqXvN49f#ZV&TW#KJg&OhlhQXS-9=Jbwf!OEt5?Rjh#xhFuM}8uZjh>8&$K9AOIk6?e7mIPI#RBxo&@*t;27e) zAqFJzBf|(8lE3#$IJ!+o8-tO?PmurVZQS(w7g%-}3lXjkFF0@h_XjK2ZxZl$oVED_ znVYevd$F)WIuT<(qTh#C<9ywe*u;KUF_gIaMU&M+8?MhD>rrMZM4qslpm4n0cv#UF z=SIF~T&+2p<>4{tJTo?Eb5H-7I2d|qnP#ALMdsA}(LWpLeWCe$y`=Txf)pOl9IUc!>$a5&0zD@Qi$5@b`uG591Z z55wa#MZ%6-bGY~pMxo=NszGuF;rJxQUW-sNkZv8Ga5WfQTx4-MZ!n|4bc7Scju}{K zMk`rPTTb^FpyYyuSR_RS3ISOL{Hah}nV4q4Vo(d?DlmT}!am#yduZ95Czz&&09Tbz zQM+ufllsa8#!QZWn`x)>u_vg*<~A!3d25pI3%@u#mQZ1Wj|rK)tPeI|AmzK_l35J} z+kaNiFXT4o{_(`=@n*NAKmWCRpi)AX0~v$FQ6Npt!-X?{|9<5h$MI3;>b8uqSaeh0 z95x;L$vEd7<+?kQ@4G@db?{Z`MS9amuA-quoqh)iiBBSG^}y+b->xhxb#M3dWa9LPwZx|5NoW0jf4;cliDP+IkasDA)ggSh8hj(1eIAGonTfA%u(=MrN`vg;X>4Em_iH zZN^e+k|sNYkdlccl`WELkR@BBEJ>?Usm}j%JKyi``906`Kd+ZluXA#1=AQey-k4_i6_rMXeVO~QBv_JLz`QHt{GqMlI!;11}xYP9e!G9J`{h5ZWJ?LNv zh+M1u!z_wUHJteQ?Dzz%xi4?@Y#e~J;WwE1zd@@-QFGnj{!_p9xvd~kmEZFPf9Gdr zWgrIPpC3IEm|1RBvw_zD zrdH6sM(!{6c}3?7T6p&QoE|hgnh(oNN~cqUpzGlM#>Q(4zo_`vxM#7ZUNO{eDT23BJ{k?g*Tj)^nISfzvvy|R5v&$!EMnLB!aCXwmJT!D{Ds+=tgO}7KDsj{4+`T^x&KP+OwH(|_=3}#wGDP+InleoP*=4U3zNl zi68bBQ$A%)6=_OLjAbNwg4yTm$I?jgWv*q*D19=>1arVnJb~=ac6>EFT)1rv+*O%} zhFcOArH`EH*KGAV%+k=dOLV!Cm$wlLmxrx#AEMl4KHHT8>43u~OHWIi^e zN?|^MgJa9)2M6T>?>i8W8{Zb2+a7~t8K6(a2W>@;1kpba9+dpqmeRHQ+t+Z?92Gmu zXz%WRFv$G4(l}36v+kF99cB!g0dB?c>QfMZ0!LGB>Rc6^-Vmep`13_Z@k0|h#6vDc z#d!a9N@W1c>d6#@)4+-!)s#QmnuoxgtS@a?Phx+y`QBOg{g$~_{V@;b)1 zu5e^yiptXSWn+na6qp0@<){HFkEzR4{H>r}LcBR~w}L$GM9uzk)wEIVs8&@`6G}Mw zV7H=p2_~UIPmOt`bKS3(^N~2sQ{LQ*YG>kmy|*!TJZr`BZ=cEyb<|?6y=MY4;&ssr z_4U|+OhrSB9Vn_VsOyTI4k3|$1Ql#P-4r+z!78iZomKQM7CCYm7(&G%`gsahp4yNw z=y86091`I#@J|74!71}|mb^iZzy z@EVC-c}}JMVquCU#WG68YBnitDl3e$m#Ep4GpCYeI?lfTzP+f90f{}6 z=ZALL7H>2RPkLXY809rozt~GHU5NdD?P1AyA^rZx>D;km9#VQS1ZS?6(O4R})n_yM zay5h>+)q0{&<=NfAHM~$j&;^Eph6=dYR^gf>i^x)MtLI|Yv0tu@%hw};_OIC*>!DoTf9|;AUNZCKgBviP)4)J4#Cv>w8LdxD-#5NN=AuWWLah%vG!Kr2 z2A@X8Y>W_GChLRJQ(~eg+CPg@Tc_sDEy1)l_9gfRpSRV_|KuRd61w`W_kxYw&dxR~ zZ0n**1i7zQS(e>;^_i1pJzDlgTqY$))yLwR{S3|5xv32osVEqGDNQFeoD}>PuN${6 z%R!6W#%*MQ}k)mTIn*bVFiYS%xOUKsn*1 z5+$-RoQeyj!pkjjn>bzO)e>dX%m%b<%b?Gkt)7&P;h3$d)LORm)2+Qng4Lg=gc1+0 z)2R$7b|o%bQL_>;%J&cvKIw-yok5o5tCEj&Hg&yGL4R4U%2176C)UQ(J6kp$_egVV zU+}+TvGGqEN4O|(GZvWZ<@Dd==Y?O(ImPDdGe-pScCe*oH=~b=B~FY9E)>#buVUI} zuT=_PW1pd$6DqANV=cQcS#Aq|$w1Pf{396x= z;CiCwgC9C$L5nLS&E`;#+cG~iUy^%E9Toie6BKC^_EL*lbZwW-xUSYFBWOk^Ubh^rP)DLUj=GgVS_m~?@zn3`yHLg?xXhq(Sfh| z#f`(>n@0zkPDspTW9K?V!JK&;`s!DEHX{~4@!H^ z?&h4HAn0s}bQWLBAS-rKR(0I`HI)|ECQ?2gaTQ-~+~;R|&9jJL%j!DcSFe{PJ2GhL z%navRYD{t?GuN>}HB=Ohjp)}nr0ahy_bu~XkDl&|qnb^hL(|lqcYsVIK${FUA*%bg zzpMx8H?kmA&Je#Xm4felBEM9B?nscuwe%u>!K@pkYo#4$+YHu|M+Qyy?hY|fJhIWM zVU*qZ({U)+&EKbrb*GEtRLU&Z&|kBU6!<#PFu*q`bg+G-ECaOFwWksfoa=jU8o_~( zFeihZ(q^zfy_;Ohyco4VRVqcL>vLat^drq@gdieYMCT68*F;%+e9$z1s5wOpvRv~hQ!H1h&|sXDCQ)`(zc#8w9tNotxB^xpBcGX|3@8 zSFj}xS-MRgyYV_Pi~5>^sHh$COKfI>`4FDO;1>7K3$6bt4bG^935BDKUgG4e4$qRb zxIkic8*r1}(o?SyC*{LJdKxCVocF{|&_o5rbdmym*Rc!79Z^{!5^EAOr)D0=4F_gSF;ABd)qvJ## zj@jO6ZWa^Y1C5I;SKfE1`JZ*Xf^}WTj~i`#a;tFJ>cOkMwi+!Bhl8`)xOnKs1mq;2 zflH5&k_v7oB}H#kthWG|qXZ}F$%;zn_z(Kc<^qDU;>A~AoU&mi-7 zRFv^^+Y5f6a0w*hNm3L>C3;PJGn+^k|V~2xL5Tcqgu@FXufUgl0GdVA#Nr>R*Kz8${jf z>h`QZ;Zwq4YjlgQIRWl@+6Z*Te2XQ^^nQ|7V8d3*3nX8LRofKz>%Bmw^i;}7ud)v{ zi@#b3&aJA43?WJAG2y*6CfS#%wyx4wZNh`6B5JgLBSA`0O;_pMl=Qp1nNt9cxSBI& zw>_K&V1O|27RVRw!oharJ{_Y}&n*@rkY5`dICu~$8Sj4-GzW1m_6w?&t14o^;6JjU z{X{j%tS`l(DgM>vw^U~W8D7lrtJmo}1c;wg<#_zLD)yU`dh*)W+hd>kCUtl?m|b}} zJbW`qL9X}x`}cEqGrN5zm*|9tPuAQET8=fjRW&}0At<>!a5pTsc=I&*1&vx+V_PrD zMUXvxcA!Nu>}Wd$F`e_uP>?r=vu6oT7dh5&RgZG(q;^Rwr`Mxtpd3hsKOCyR?JA6Sfp*C+gdLzl+}<^1@zq67N5dzbVG0kIx@2ZRF#2@jTP2&r=Kgo%9KObFxEOENhCtPIoc8~A; zyB%uG*Ph9amY1w@(U?nz+58)i#uO9~nD6ceyU7U>54+LUY{|xxJXoa^q7O~PLAWY; zqzvEG$6HtHdJN0K5VPP%OcXOTwhjan$Rj8JB6d2*7nZfmno>Hixn6B|Z#6F#l*50R zC~i9g{M}dd@61sGCENxIMT#8n+W*|*b^Na>I!4!GO2@gceM~*6?Bpj$*X)7CD35^; zm+)j6=Ye+VBHAT{wS8wxN;VLM-(0HMb&5ED?6NAy!RZ(%J{S8giQYBCtfwK1=0VC??iDJ6Ok&ObL7MRq^%8~k~o=sURmOKOPvCCts{f~0{W>{T< zPxZ3SNlc8w`l5*Jv7ufKjW9(HSQK-42M=x6om>3rjb&s-hxFbv)b-pQ-PUC&`s5ZU z^II0P)gPvY>A!qEP)|ElLBE~=n>-QgK?;;Wc7EkCZehgQh{TBtv@QBJZFGGE$q~uv zdgIE8~tE zYS*_@KqXG-kL;KW#FCDmQ(>xdyea7nZ*YNiScjex8MYGrb2i7*K6YJ^#F=7D=vIBs z{j&Jbg^UROt14qwxrC^u9aRUUda!I09Y-g2#xE(bkS1T62m&(-(Z{(rEH}6z?Vg+! z`ArNwmOU%Kaiub%v66gCw=YZ?Y~U4*l_BVSo9H4Fh3Lf<0&HB9OlPf(i4Iz>L^a1j z%T`xUdey33o3 zCy#IyC?omiN7KNe6qigw5!JP<5}FBBETN%&PEKA!Q_=}GOYvaajh5fye|(hH#ixj` z{ozCfGg|NDg&1Vf^le9#&;3A;I;;tL92hTQA&L8nM{>gMsIOnQ%ij3%jr zCm)>4woAD2{JE;K-ED=(2J6@Oe9Rz>h!ykcrp)PX<)iM@PdrcxjXuF4Al^MjPdQc>i8Bb&rKLtE|*{I=lXi7N<+4d>k}_$zkK?iF*&+ z*}sj~gPOG#;Uqv)Zxw2&uP;fs% z3qQXJ?RD@*U=D#qTPnp_xI2tN)*MSTjNDhBzBaJ$kBpBW$3F!XHJVOp@^O~BPF`Eu zG^-#Xur@UCr7>E9-IXTA-=n3);9bekBRxUD&QsaVH4_$c1r-=!KUqkL17MoiYS*7M zv9`59*zB36FS^gOlm^dJF$NehnA|fDb%se&7{;7J9+IPzY^-}u*@hIKY>Jxr+9_gM zW1$a^=W%Xuq0A*($+YE!T84b2K~7@N)i61GSQ9I#T%QU8vBN0cSNHXzj^AfGH*t-# zfkFxl5k28WTB#PX?k{*2X>0DCne#3IgSu)ty>MJ|zN8rOX_nJ*jzp}qvbGD=JU_4W zNcr9@WC-Z>gby4H@mu6JpPaCu6y)S(KJ{<#LEk*%%#3QgGEzwIy1lE4PFS>}h|l_~ zT0n$>aQvDX@vX3qbxYwP79fc89BKpNFkZ#$DU%Z*q%|P{>V3sa4D{ zZ&259_F}`Zf+?JjURjxEzqhmKuS21ff&6hu6Xf zkT)KuV|BOYkm=Vvxe~^B72)S3p~HH%f530sPfNhUq>{JXV_s)QbZ9^kxFXADCAHYB zzrIp~!8@>u48U|B9|A*%5a0s7c=nM*nBt3lOZO=B*58i%__ICGFeyVKbX{Dt(%7x* z{>wxbM!3qq7?Y~vMJ0Qok{z_XsI`5^X7^#6KY;nRva-gX8$-FPnMRfxv-7ag?E`n` z=+@M}pY88h)&;^qd-^;ypS@1e8pmovG~j6epd~5>cKa`-_$qqW(cM?kk-5C-G{4sxp4a(vyXg;q71hVeT>dFE2BU6 z)NJNv;Q<`2H{U4l-(YE)1;}L^eAb;uo;sjL*Lp6&adnoc(GvJ@CLls?qPjb+Wegz_ z{)-Bp_}q;XodRn;0auD2H%SR4>-N{Jvt2TCgG!S$lH;Re%MYb>h2{suhTIs3I- zsJ$u3BL58mKA53)4ez#x%di^9Abl-SZi5;PCgl^lIj*0AIlzE*{OQ-%e`@-_PsC)zo0 zMU9S_#sKZZJhFVHBz~BF_AUL}iO@^F_y^lG|J-ZwK7S0IXD?_PGJgo0weL*{GCS`$ zdzqp|qqdNmT)wr0YH!HutoYp9KEfn&{I1rS-&ponfjypY!;({cXgykGcZvhk{2;xL z(NR%7yFB)MbYh|g1fd`8jTJ#7&ywWQ9L@oL70GAasgwY*Lc4>=w9v}j&5;b?R;_r? zGqNheoR?Z;(|W&rwPboZ9!DQN;cMuu7X;5-!EZGOluPlF9hJ8AV{cxKMFfQc<|wCt_J(6Q0_V_t$39IzFeKEqoFdrtP@`MdT) z=Ed~4g+BNspmw0hu-QqedYI5%Pv5 zRfyR9xW60jN8*_>TCw0A$;db%{lxqoK|nf(L2TJ~R6p0!Iii{Hcvco+)x`|}nSF-M z-{EJwG!c{v#iIaMd)o=HF~e26I1t}^I#xPIJJSt8P>3FWHSP=p_W6SOGyZ%4fw?p| zu;k+7_ORTO4<0x+@$))?aFY%617sv+JHuoM7m=g?Twb1is9XW=ZD!xe2g;qg)ltJQ zUlzsL%uw7rJ>HmZD<7|{Y^7nk8d5hc(Iak;f@pFKO3eH5vL+9cL5p~E9yyKNFFsg?f z$G!OeeRTfNV$?jZO_CoQPV2D0sJ1~ob=cS;yYIk_omcE8>uoP2ebBek^R^JW7Y=_y zKI4-yM}~<<>^H=IKqm=jH9fOtm z3+h2J23&ZM5dh>P^n08Be(30%KSIX82o`&%|Lbux&a?<7f(AHO9 zUq+KRCVg@?yxjMzks!CU(>?p`-K{-=Bn^-kbe=$CX2rV<8PhEP<526cymGTu`uW!jYHkM)DvJsvNrvKm z(0x2>*B%f5X(_Z;v<*i>Rc_V?w-yjY$sz~WflwT6>@0KGKr!JpI>j8DQ7p8U-N@x# z`7%sf-n0Sv*q0O(%&8SSNQmGhm^ zsnB?LSKRiyr{oZ|-`Vl}~cLgkVTMQzf^!MSOe!}B_~SULT{U*3x*YG~P+ z#twuL-=Wej;}&qx)#OFZ8fXtE{=8Z*O|ndeFTyoYF1tRfCT6BmV)mqs?cAgr#6h=I ztGb(3q2YlI9P~J?(f3&AS0MK}oQ{DfJ^u>$_TJE~>fnYdjPP^81AULLzc;`F|MlvY zb}ciZr-dl;=261;=U6G0Hrrv+a6c=^C^g%TQZ(`@p)}#*R;3j z%)mZE#IR?sHSfMCw;7u`#we~JF24h->^;K&dp>t-dh2h0&l{>$?Hon0+D)d4Y{T{3dm@ z{8djVjj;?1AD&u$nk~5h9XNPiLkR=S9@MEQ-ztKfk0Q&>VzJe%z*teDjdI`}pv>jAD6@ znF^bvff>*#Z36Cp^z#?&`L7pX$EzPaEOe*tU)e?|GSJVKzkE@F9r+gbcTpu$!L~VQ zeHunA;WW{%ZFfPi1ldXUd&th8L5pXCCC&`GirsE9AwmUeX8&ST)=(=>)a3aN(UPgZ zi_X|&3oz2>ZLBbP=GGjMJA8PoFv83ha-Zy#abyyd5A59H$xO)diOh&*ZhaDfw;KvD!x@3 zr7GK(-4UoCaOm3xoPezd9CzyCK6y}muL};hZFtjI5qPEU;LEj?-~9bdce;tlCyssGga=VBOD)EFwd)K5d>6wpy@RID1e%rDM=jE%tBEu~ zHV2(MO3?#La>ja6HIhBVgtkzx^;@U(#ovTz zZf(h_QCA}EyG9ar1RU3fl@HvlUDD+<>ma*#gKn5Q^`Y1jNsf8SR#CJ7-6WWiJn4P# zCuJR%TdMK3K|&#D_bf_xxlB=VF@{$x_L7ST#)?&DLr!NYH$vu3=tcV_PTYf^?Xea1 z*Fs6vLLj(gQT`XIk+yK9`BptV#C?KQPkLCb+I)zRQKuLhA+1>KC|wI|^w6;)<@JDU za3t0ADT1cn40g9DkWyWb{dLu{=L=ja5SJXB>oxkXh< z_aEkK6Jt68) z2{N@vdQi!h7t~l~d2sH}-I|sA1y2({J22|eZ+YrqamchPffJnx0aC*UE%ZbsqF`9StVxfwgH-qG;}!<*NXB^z2fQ9+8PIN4b%%Ri-X-`;vs7oC zS=*Gu&^HWY)4lex@nL6oZQk*hMmIwaOC7|CcjIUzCz@!u4g+X+<_ zJ_9fiJ2(v-^U8fI30@)o_!zH)2VG>;|D`OnTngX-;Z2_4o&BVh$msK=p85W_$6vKe zm3sX9qU2gcst4aXLlrkyS$U+*))85}VC$;1oBhsrlBa- zLgY&ziEk>!HxY3{6u;e3PXD9`N;;g+hL+9?ztC@)i_E(+%OO=wN5iH-eeo@YQl-aH zy_|KW97yO7cv!zd)$RD;R`mFOC_F=wn_--{rDpZReE-l7;#sY6?v9X&YPX`%HHt0Z zs~90gNd3|Xd$VfBzT1v*Xp+NW{f?IL{D zR(Jf0VtOk`4niX9xf2BgdgN+aiNMBxu$tc7D#bmR^L@?abcQ5>7Pl@|J;Z$X0pSAI z8oJ?Pl-K14Q4McUFJZ%G=%oH%ws`~@kxiV&U7 z)pO|Z2VCFy#ez?%a(1vd_doj2o_vWyk9EhrX#WmE!xo)k`6aLz)Sz{Z!_yiiW>#(G zrc02d78h81Uf>sKI{g20$7&xPlSS_Kv9RL8bMSS)|sR_ z&j{pQmzYR}`<%iI$5eGP@i(&j{)iMJZ#bb`V*8%eE82TeTzG0BgO+(CUYKk_)DnOMP%Xtn-(aF-Gr+kQO&oz~*t2w@K$To*hF&9`SC?Xnk{q zB88NMvh4kWwL}FtXc18^$rzI7fM5YAhra5{xgRcN{=Hs&SHg6Ex|ZtEkwRswOv$vHGO2 zZxv?f1K3cX5JilEgoxV0kmg%sBJZ(;n_X)I!+#d#`B#8xA6H9PV*z--&IlMf_bZH~?Py2cNb58x$8R~PP1e>MzaFv$>#F(tCDtF!|Jw}3|Is7owgS)~YuPXA7Z2Px{`*H5Rq1A3UE=i68qqP4Fo^A zrTv6>!4Kwl4OFy8n-w_DL{%!es)BIhKyHdp_4Vsg77q{jZH`jg-07A+!z-Nz>1^z*G zAT0T+gW@emL6&U*IK|tWEOVtSX7b?~upDkGoKn{!?zhQ20w2aTu(@*aVjKk(c7?M$ z^Yn)_AXdEfZpGk~`coRw znhLX~^I2M71Bau`U6=Knzw2ZET>UHO*tAn$Uxu(|G3D5S z`k+IS@<#U96TYmn`yb(rLTmOM1Crzvbxk3QiWaC21=|OnM8sYQ7kXk|f$ZQ_pf@;D z9mtHBC~R7;`E%M%C1q_gHEXo$>_rL~j?hc*4X4z_6XD!(I6QK4x(&AkMNir{IeGS- zc#?&+48aF2U~abCW|Fjy1v|%Wmtv3|?##Y6fVT@zg$j>b_U&XS(~mSFUGM`7aIlr> z)u-NDz^l^jzmqLvG%j2&!}|bFN^MgK4*}bWECKi(R#{d$_q2kB5Ga?WH}>P~lXwzM z^X#XBwf&E0_Tfa69fU{sKR+Rg?a+O&>4~0{M1?spHR>ZeV}k3& zj8Y}8pJ5AZq)VjuS_%Q(^&?HHr(Z|@s6rVD2(m6!QoFObEnZ5N2w&{Cth0jfl}c%} z5V_@w(sghrqmmQlBV9utbU6Td@S?D70qZ_(o0jE+ek%Ne?6@-UQ+5jme2nhgRY~>z zEZsW$^ZpHtaK;~(A5AVvmZrU5jpsf(*667-!$T2`!3j?;q>Uo^RSV4cA^7F@6Q9At z%>{p|k8%z76Vs%;zIMIaSx5K=Zo~-UZ6tv*peJQ&x2l3Gy#-%lFjVWA-LmdLs%i-2wN>%Ie-)*; zm%)X;niSw7ltnv)V%H;+oaUczc2~bc(DfR9pEfd<1W}h^I0Z*1I5O5CV<6Fu<~!;Q z#t2F`0y#VqCzROUCzemZ8r{pOEsp$0rbN;CTg%8*$PUbyW}VD0qrLEvw3sW1e56^E z1eI4e3lXscID^?lc+olObz+MXE`a2F5F_W=;tF_i*?sYlg|D!_7WMV%&blx3%1VEh z)kcA_ElHAIL-WfoemtdAUkG@tIb-J$fxL6*;46ndBqE(TUGSG1@w(dg2l5dQxq?Tp zPOC`hXk4_36UdWgQ)D*ZYBH{Mg8k5k8WJ};3ZUniPkWeMa@hexXux?{49LY6h&Avk z{4Y19X$~^mD8{U^2xZMe>wx!lB1M&`aRmYtFFxV`emlLF%Y( zYNsiQyM5lXKgRxK!u}QaSp6I@9M7%XuUY8IAY4%MLAS0Q>od5zUwG8@wSmBt)e}wX z>p=YyP1)N^=3-cWlG^rMC5wHAw5EsnBK)N{Xp7kHbp;FJj!^cAwvyz_oOUe5>*R(f zpn}$ysLoI-zxIB>iO#FzNju`?JsyVudJ$_Oq1LY>5r{GVz9^spG??Mw#+* zFDPj4@#juaolS?D1=uIRBkGX&4DupQSd;zf*{XYK3)^mdih&^!nC7|n8;JsvktBIuqMU)i3ZFpZ7P8jIumNN8p8?)S zF&Z3KhL&lLq=lH#jC!uM=r4-09aJS)ilv?p+sdy5>v!d|pI^I-7a`-_^KBK!2m^9? zE19U_ye54=I4Fvkk_rD;7y~tkHJ9YjHF0!_58q;;YninFcU?jB5!uFA5wq{oV3$Es z1zZKDk?7oBHBZ!KUU(iTtQ|o&Gm;C32-jn$|9QflHr{ z?2h>6CI%&w5!hrU2$~#kf_!^NC58e;P*BaH*a^};jm1!!w0QXRN*Pv~EIp~i6S~gH z2_zaqYF3_hyIH?#;TJ@+`4)>n6`01PNx34Tu)0LVgMfcilZz95^Ju4No0^RSBF)q+ zfKLGqtoR?4vgW$^bF$GDJN=zI+i=E^VF}c~+T^}EsEeJsDYnfPP!y6ddw&7Sa?rJ# z*XUSTe^(IsD#!CblJKi#pMUHWJL`i`2L$g7@#q%_!3&6)SdCLd>dmTV+#qpmRiyc( z6anXNnpP)cXEopTST^Gm35rm__#|-Zga~7@A)mMlTAJrd=H|gKhxd(Kq5M2tW-`D z_R7C$BFl4!Yc4qn0Y-SKkZ)QgV^189Gk$#OAsv_ zZ$2P#z)#oHLO98^R>&K|=q=vfZgBr1v-uR*)IDjcW=$%5#UmMDQ)4}g$uCC=KOAL} zQ!ZC<6#7gnbaNE8L&c*HxTbKn2^kca^ly#gWJHbbRjwL1UTa&Oy~oc_U4~qnU6=Xu zklc}sZcVv0ETO8qFz8JWhbaa!UrWGW2nCHV-uIw$3DLlF5Ff(3t*gGwnU`$sZ~+sA z1FIu_Lgq#J$Xq5|_s%{30n5?)BCMXLMWFcF@_pK>LuXNi3r6+GYz54K7Je9`ts_)PJToBINL7mr4^2K&|BL27}PvMCj75Gy_^H&0O1%(X2m~ik#;8A?$+z-Vl!oJAO>`+rQaqZhRccKVvzG6 z9UHHXYKu*94sioKoA!?7dO&=%%&`wL1Fy8tQ|2>|T(6*8*eReRsSS04AYc9154KmZ z9V~2!d}f*>w>ej0mt08g1*C5aEjlSUzu(7A?)QFM&qLqoGJ1IM)xW_6fvr)-9lWqk zdRY%7AoAzxga`vrQnSKD<(r}-Hb2vzGguf13@+1Ydak(op?gPrt(w|9A2}(#{j$p& z)2!lpjqoxKxE?GoZn|795e$I`t^>*zbJ$fJ949AUg@BFWo;zYgXA9}PHVB4-x9=?u zNc#8oseN%~Q#ve|Z7zZzeB8>=@&jrOcp)X&cKEEdIn>w1E*nyqxBA-aTZkz5u3Pxm zepIfQud}DaUv;I&u@*0JF5(nL!Q6bakEM?ZvO!%bQ78EYk9-F!^;!=i+vC>wqhm+s64@BUHmCr`5C%MZ zqx#fkXyUC|;C`rkBxo~*vI$@rT<(J=gAwM8e9A3(%?M68ESuM76V-u}&)3*~;`VR@ zteSQwaDfspk>5QfDcoC}+=g5=j@32jV+|L1!s=pxZui=_M*bnx-y!ah7IPHr%SW%F zV$$751)7TxcYsW9yWoCPFxF;GBtS!6Z!pk>P-|!ZuVoSWFau|x!7Tgtp5RH-hqw!5 zqF8Kse@oyAn(phEZW}Q~PEc*;-%0B6vM^9uzqRPFn{7xeTxcJunuI+?suujqa`kBX z8Q?1>)Y~3AvbbVu{vgl%1!c|&dkM0mr^a2t#N?~i%L0r%0gr9_!ZtX9D!gIhV>HTdLk1uU@S%o5sqAXwjkh zDgq1A;f_BHF7uu<-?kI%m%If(j_&pvlAT7v4Rr3RW4Oh~8wo#1zH!zKN|GGG$Mf52?IWuA5^*f?a%Wyrg0R0 z)d-mLc=;xui;#L)sA-eXS@@hCdGx3zqZm&DO8`OQrs&JeztN%&S&*%4XlS^oRvV;{ zk2NlN%4?&mrHTY1OyFF|K>Y=zZS(Sdo;0N+N*wgMPjX9mV>KWQEx`QWUkdM@3?k4c z#M%EaSbyf1rSbDeosj2yA`^b)0SlBzvXYIucKwnZTB^dCbz?^e)!;N zGMS}g#e0uG|M7M0Tl@Hbe3*~C+t_F$lx5Lmjv{B^W^rasQ0J^DIMGOk9W)Rg2PS2h z@8il}KnM2a_dkx-1EZb<|2fi;RR-lCEp{09#q44qjKjN_{KIY~y%?m4EsVmbL(8ri z@^Tu0I;=_k^|hOe@E+-qKzU;-l`m{C4@XVi^e!CFKKw1#MD6lBT~ zmC|MK4epwT#KoJAv#>a6DwbVJ1MWUGYm3#r=42U!o$x_h@IeuAefU{@4vdofErgQR zl=NCVIX(ZnX@dm;`-Pjpi`UIO)Q>M}xE3^OhSln~(v$j!Az2ATpUS<`w?N=ik6sS% z+OXuQl-}uMO-{Eyl8gV{W<#q%hkcU^|JnXBP(UHANMcKCI0UnJ{2F@BL;=ZlADEAq z?55TX`*pWi);mj7OY!A&i$@OpghX*>Le%E}L?&p_SJ+5UunH|J26gFErs%D){{|?g zetw3}tNzuNCtF6ItV7k%SU`NlM#Gt%Yv@VSVezkAoGxjB9VxT(5EA$R3y#I3n=*y; z$XT^6F&9%*#875zLX@ch9{u3VW&OF6)<&81II73*+NK$)Y|XiKmwttYwZcHWgD(SqxIW< zC#RufpUycp$r9FR8NSL(!%Hd1(R@E&J6mkMiLK5$E3A%4#HUipHS}nQ7vN{if&Co7 z?ea>_LA7l8NKq!y3z+^7xEZioMyU3Yf}iHSNLuh?mHD9We{8B*Fc41yhVD~(5%u)B z-!BWdyhkG3cBuOAu4U$1`OZ_@so!~SkuPR+>zyx zZ~dUAc}gtgba=&r(MHt8bEOqXK64+c?y^Ha2g4}FbQ_7bvie3?T^nxe)mqb@aiq$a z`n{IShszyF+69WZ-)5+`hP6rq~dJ)ZY1*YnNNP;ouM(k@INnFm+e*f`&h?7 z?8f7FI~@9)Vg7|qq#s9bUBPPxU+t6W3>&a*3H|0)1)4x#f=?Sa8}qb=^DyuQ(iOg8 zYx1+LL4g0|eDI~P7U4Ag?Z6Y(gMBJnGWPr(_}@kM{E7iPwKDZf!{TGv#<#9&cstDD zD!R^@MveON@g6uffKx;dau~Uwv3+l&#-Kn1(QU(0eJ^AfY_CPy){ox#Ac_S)XYBW{ zg@1M#h|W0$vowMPojsbC0bH}da1@l2-lhNqeYPwzFCy@yJl9&voyvODSvVATyRf68e;$^(==^X>o1hEUN@OJ*+^I5z z%;_KZC02H9F%k{_*Sl#wPyBBzqk1tB5Q>SPUe#%=u zwgnXDMi%k^vv6OU?ZoK4kpJpaOl4)1QQjp*f&YMtu)#!0QuS2g;g?FP zpn-gAxuyM5j*8?wanBHsf)<5%`A;6!%9h;WWA-tF-BHmOqYD0*utEoMuc7P3@Pd8U zIkWK!yhZW)XB|k@!6Qgo;(t1JAA-F2V-imSeT%yH)EU(WOoV`qpOT6je{xVqORhxr znbwo+HiOK0_4nUmL!X8trp&DbLh2bLDY56gM>0hXQ7`aeSoTXvAvi`OzqwT)F>2fP zAn9be{8<%UuoCyR`1OTjzPw1saW~=HSH5*9ku}KZirh=D*GinwlafQuZgFc%8 zVVS)~U1sSU*@HdeUloMk^mB;B8J$ibIA9D|_y&Z(Bp<}FY%cJfGcT~k8qe3g4Ez^6 zgv!IfpFe8?hdU>o@1{5l-n?HP}H?^j5WJdnO-b6{Na9{M?*< z_{=jwb4ddE@!hkm3DuPt$0n~K{>98=VVans^mNRtm=v_iYT;YmLebV2U*>K zoiqPIFa|`VyTs=DFoq3yqj#O6-MU)>8#%k6>Y_+y@jXTS<)Y{(#Z{UCqG_uj2Fj5i z_jsK5+AXqWEks5=M{uG=dYbv+n9KX%1M}zP1LQD9$cg;RIl%Wn91S(-$=;*JBo0Mp zUMQ>7vLHsVFn*#=4){68rq2%GGG8N*mmhvXC)N7IO;043aVcg&^)%(tM1sFqPs(ZT zotmpUkk4bNldTGb(huKOkU;zq1mq|PS+*nUv+`46R?gzwzT_iY*%sX@1D3x>T@yB8 z1()n zN&oyq^*24&B28;6=oPC44iK+M!@PZuP}FkQg<2*P4@*EjB@}6!=z!hFS$EDCI}Uoo z*$0ntIVZkTNVx&hFg6%oZi6P)HPF`ppcnP|feg4Er^$@M zh7Lcid_1&wK5QWHl;NHi{g!{3;4petah(oV@XY&SqXVh}BDTC*4p4|RHI0^7zgZZ( zk|`Q}<-R!daJ|^bY5}cL;0~Xf@wf>vOhQR)lR9zPkDAq{O1!c%`JdqB^C|+!jzuy> zgguGRS_Ex-&tAN_V=m}wJsezM9TC#&fNC@Z;@1~Y#2#d;KrI3^Gf{Sb!_{XaJj4Kw zW1^g^H`jVjMSBgBwUl@!x4Gw0gO9H7qz(%r9gEo=n%jHZ>#>I`=!<6{H?bG#C_s?{ z=@c92ahMKFM4bKgPzteMYy84u>`-%7gEtb_6`C!4sLs)@0>ZZ!{2|c$s9mg19Obs_ zb5k_xOUHw#su7%0dJlFy74A>?Rzu`#CpMRe0IxfRwhE8#h{YT_@uX&{7yr$%PDOI&|qA3*|Tph)1AHA)S zJrQkoU4}b-;V;VSWIg6v`xc+GUDIv9+vu}T67)}NmMK0&?N?)O(ZNkD&UhC?)Hhr@ xss8@SrE-jpqb$!{u+4Ea!9V)pB92j=E4mf$=Q@()-M|C?*bwY3Yb?AI{twxu<&OXW literal 0 HcmV?d00001 diff --git a/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png b/buzz_scripts/include/taskallocate/graphs/images/frame_01424.png new file mode 100644 index 0000000000000000000000000000000000000000..b0531d55872cb9d4b9b1f558a536c559ec398dbf GIT binary patch literal 98463 zcmXt9cQ9Q4*VYNE3#$gvEfyOsdhea+thxlzqj#(K-bD=}qL=7(^&o=iJtArlE%Dy( zJM;Tv$Jo8|Dd(R0oae-9Ybp`q)8M0_p%JPm%j=?{VT=JEN<0kU8!oA6Y2Xj0r>u%T z9vs2FEeN_^zQ5l&@A~+;s%eH{@#2p}h|yfoeUbN~`CwDg zpP=IG4bK?~h}}}FOP+n($^xuz!RfHG=$hR>slO32(a4rQ<|SsA@DxAFsio$qv(hoI za`shT75u<8-{N509baSi%Pctk5zE|@w z+=wv8JYZ`hh`1b&4*b; zx&I?}sHX)71x%xM!6{P}8)O3oi-2)XG$$I}N>E*w?v-Gq4utY~!PnYwwolp|B!fSE zKEuH{AN#i4-aDGKm`uGUjKo5YgKHO$o}B2qgjqKr^7auhn&i=)^LCTEtO>ix<_HO0Rs6i)x=XwapST z*i!MnZ5Cm*tX`#8z~F^e*&Pg-82C)o#C7ZOLSr5_W!<10Kt7?h=uH=z4hStbLzT6_ z(eJOk(Z~7249}u|1rmm8Pl7JA{okUlM}Z+1R!AKV3ibH{&8p6iRXmtOSZm*$mvhq~ ztDr$B$PA1Qib(49@8*5XwFV;{^GY6Li8H~-k4L_k=%q-~3pw{nbvE+bY;J-eY+h^) zs}3_EPy|@9#bmgjCWOK5N?^pzLAl4wp()ofuR@n|@=*1@&7%HQt-)#%pc#waTBO1< zRvk&BHV-GemLJo{B6+@{{S`ehTl9v4$iiYu;jV%f(4i%0?FD4fs{?!K8GT3^;sp^h zjG%Gj)N|UQi5f#V4YWYKRMG0P<5GC;ZxRdNY!w2Pnei$Ge6mMQUOp8b?Bnbu8$?B! zDPmev`&zMhJj`Ncsj@;7w%lS!a5R@tsyxFAg zHZUn(Z<;X09X3P^=!L0SSb^tl858vpH_L@y23A#IhnYo9qc!2AeUnE=!hwB_Tw=tf zEQonfnt1{N!x)SyK!tyKp%>N=)-rSO8^wg9PFiHd87qzE=o}?0!!$w}XiKUvn4nRm zrww~>P^FlX&>w_4uVg_dGk_z7r7f+FC$;G&wT|TVv?4(kLw1Ik9W|Mv5tPYiD)I z+3<={p1!J2YOL%v&1&((+jn<>qfyrXb>&Yc01~Cnmt(;i4Am7bp2>_sZnoNs@(ZQH zwZFAJ8$<~+x01bYFEfHvcH(TuNN*#alJT&mKWUsM7cC?!M-ZjB)*p559-zg`L?AFG z_8Q-pR=m=bEsb){rIhAc9UyJX;-T3_t1d}YEYs)O${6L88#edbI~0*t_3fD@k%|G? zNEZLO|2s9MvC^tmL3Wa}%Kn8|-!1jjMT30FSx5o7MI^`+tupXpGZeKXAcAXHia2@4 z%qcJHw~WruCy)pw*MW^=nZ%ZgN+%p2=r+4amm`eA4l~o2T6w;$v0O-y%X6*DLc}r) zjAHKa-JM3s3qJl%nCPb``Tfc^_|;Zmo6Bcy4j2vLv}oM-rur)4C+Z)>+gec2RXAH` z9qT0ABQ(x6WwdaaZ%rc1HEtg`jRQw;3=?L#`Pg;|f}$$^Q`Gr;C`fM6du&i0(y>IS z1Iv@7S2v<+znFPE4ME^bqfV)#$1Ggo03q`7KQxOHSSERfO7;9e0}qTATt&SM zN}O1*1T!#>fya1p?*JBSK>`N-!Dn$0=BSrHl9C^gcA=;zlHU+QnTaR3S46RWNFBwa zoEW%l=_OVXw}B{P#!H%j zm_SD^ZIjtWi$jZ~kMrY6x?D_*uGsd;wXV3Mic84KBxLcg4QAJ zW^oomn%y%BCYxo%9?L~MWdc8a?1$KbQ`;P#kUKF}xu=v#1!|8fp@nkaL2~a9F*%<% z-PC^1zxT8S6UoIxr2Gv^sIdmrulx%=HMQK#QkIv5qK9-9pj7P=Wg|QMoeepaoDuG4 zy7Gb(sw5dIn{F0wEmQEHUJA(AtKGMLqdbGKKWV~nC(<6MW-(nSq*oC;KeAPB&TfkA z4?`_io&`QFV{sG&wUjb>Ro`fQ5C_Q>7_qAE(81Zn`}?8qElhBbOpg`eb~rQdx7>X1 zE*%TLHZ3Q3GRTxo<7^M@t3?bE%*{|4h|Y$8m=|17gT5k=`^Qg89_(zqlxGT znoC6)EUhX|!xy=noAyFXcidmAf-jNM!8Nj9A9=V<|1KK|%F%izD z&bXe~uB453{1znJ912emcY%q&+i4EZ2QTSGo+a&z?dR%aONwOYZZba@Hw zkBkf+HL9aPNns6gJ)SKAlJj{Q{ZzR0U>|+QrSkxPA~wsTBZi4al}h+)b6c=TejoJ5 zRnyPO_Xf1qE^jm#cqVs8p=81e3b07d?PEo6bhVwgMq$d&(f-R(j0HeZ9yatmy$v7) zRl2*#t%aVD0dJYcDj<-vsA_)vJn65kt*zxeh;j2~w?6Y^!CZ9?(=5-Aaf8(^D=QB? z?~OXlcq4=fqX#zy%vzEhTuU@BN*%Wd!NkpH*LLG2ScWDGiY(lp; zM=s@(^XaxSQs+p-JovFeB_H5tk)nwzA8sWJI(RC2l{yF#XvO zZT#Z0G7>)o({Rzjg|7(lYC7#@-X?#-a68MbTt>heJL-n%Hf7(1Zu2*1y%W8Bh;~{) zYRBT%RFRqCoS2ZQmpJC7zE)vuGR|6X7HU$*OGY!n*)$TVe1*NAGMA-;o2tl(g-9_P zOGCA3JV6l-2;i+-CaM23!_Pb8C|xx7FMj%rk&>gIoFI=8W6Lhe93m${)SbvGW$$Q( zCnl$vuUQo%(Qrw%Rf9%LB=+gXNATrt{q=p&(z}(`#0ag}r^(M-%y=CF&tiwtb?ruX z+@&cy<7QJX(`3b8Bp|Xfvc6<%#+L7tdr+;$Lzi`9R!yTnV2!?hF^O2qeNcJg2v@9C zl_DbJ5jk)@^3qXOvmyP6?@o=ny`utzVdhG@Y?}}pubn2e2Gj3?Wl14#u9I-wx=y^r z+$LSxR?;4ngG;X8mB@7b($Yf8_<^^3$Q)D1W=XhGHN2^Z9Kc5B_P_>==Zhwn zQu$N%MJfaxp=7QD_;tR-KnRH!$DCbB2RZn-Wt{lVg0eR zXKi!`BYhuLKH@aM2zcqD?-60B=$GyHUSbBNB~)^WdTv|Vo_ZSk`kKupo%x%ybIdHS zRBJ}Z*i5sWn=(mwR`*8rHA@|7I@9}>a2|8*mOC)52d124%pQ~xZ@xLTr0iwbEq*jW z=St)SrICbuf0bb?Ohq`^XQ)~)ZVU`IXiDi7LFb^B! z)V+9`EGVXAIgtrg+(tSc%KR$L_F;|Rx&6`VX_WU|QRb*(D$-7R8=J}~Qk^48`Adts z=q$6zis@Tkg0yhGmT5hcW;eMm4Z76I9G3y+glh)_S?<4TKfP36Mkg$l5VFl(LpbGq z;wC{xBrPU&X1v7FgRX7yFb-CKwlPM`NI2N&WSrW>)~D=C|E^G0lzH+-c`B%Hw+Yv6 z$AQzHbI29(obQ;3hHUpNu}S}*0&Npdt^+RtD;FDsqpoK27g(J>7xql(jXvXV?{Z#S za)Z8ZD`Ue3quqk~KAsQ9=DKI5q#+FY4zvBuHKzKFp`Sj;AK9*k;6;_#Ou31GY~u2$ zP*4prYQ|7_Nr@wj=CJc)P6h05dphLlq~#x`Mck6Utub4~3@EIbd)bvj3(23Wo?^L%fb9a|$wM8^ z^d+MI9lX6*()yRZ2HJCIx$|=7Pj`p{T~F9h3pekLMHQ)|ZhW%_TROBf z3eyWriI3ez6xpFsR;lvKd;7R>4`(?6%U8?&-QLGa$EC>*-*WA22bO~IE>W59{@q2D zj!>^TN_(XQesxCUtcI?8NgH&gEk#XB@>%y|a*VrNdj8j0NbNzEbfZ9oMD~!9JC`@6nLf9c zn)~5~>EL&VGx2LJqm{xmev?_BLIDYK3Y?t;!`N$n>sHmzYl>(B2HfE}Bu!f^qF3!! z{)zVyLjY(H7fEX%eh4ot0!|ics}_&!Hgup9-%&Rt+{P(- z>Y6@41*hgO=#dJ~sqEI_h8y$qF$354Qe}c?4>;_{FR5nZ%zOgaJmYH%G{@|{mHKZbAc60>p@&3n!nZ#;5Y zR;RoiEMC%J$kdF4Rx{yLaPt-rw#3pKq*iH`JyXX(BE-lIEm571U`z zC|@v)+Im&X*RdoSurPZ!1uvN1*;1nISbl3?0=t)er(M4&p_KoZ4kzIoOe6QmRbHef zvM-iDy459O*+D8Ay)4RE)BCL`(FpmrnEaJw&B_^jU|3i5Avza*_-fjY%kk1gGtsav z-byQzGlvdC_$&=lr&?d&Xr5)7iF{lIaf>aV_r4Zs)7f@X4-g&69J1jtLH=q)i=6LQV97LmdGdqqpVOMxUU8h#<{wi{IoYO=kRJ7S8 z-*ermWqhyEG4E?07K>c#`2nwq2xFEk5Lk??t%#WP-{%ac@jLGDnvym=<~oR~d>Y!G z;*nO=_R1sk_OOkP)le~f!SPh7R=&Vv0TJ_T&dTv*mieMobuh+2DgXIkA7VB^?w@kH zO>MuJ8j+viQPZywjf?>qDjP=Sb2uvgtleb z?TNH))8VX4$g*D0*5S65+(l)^X_lc^k^Q3rMGV!ZfS#+jM2RZ{Ya4{eLP0bhjS*NGKzPLouZ{&*)edrPu`AySVh;9jZo3Y3D zE_}r${VvKK^H3>!WPcp31@@x8&;eD+5G8s;0gG=l!z%NA9NHZ}$}#7A5l>y5JI8g} zylym3kzzTg3{nQSnanfaqMVZuDWlUh^h%Znuh>o17=wZ}tkDGZrJF&?qH!JV5dGXc zda%v+<@Vpiw&#AW``+i$J}nULfai?{?|7UQ5b^Jp8cpivHY;d)SuGkib#Iuw$4dgPJl4#W@OW{TB_+?j(6<p{ibY^ib5nDu^+<%%%tdu)H0jjYtm<$PelMC2h$+hLNdoG}-fFRgh55=5qE(&$}y zm(gaWk6PDUJ*W8>sNdoe68iyG*sgsUFB(~jY^?O}J`oLDX~!n|aqepdq5OTJRC*ja zQe%W`1wOGN4%VbIBHIs(M+x{m=f&2+HQ}5qk2}O*^si(UFzJO1%Y%_GId$!ajGq`e z?-nUdjaE;xoLB6{AeB8_Zu&_r&**sQV{Pe(yu~(WGuHvgC~{~OyPV^DEB!jz^ZVI6 z0OYo>)=v|ZL#3`S9$k@~4;|rMX>990P`9Owf7M;pOu>SmFbNq}kQt8!n4&F@x^iS! zEI8GTLzNp|G1Fi)OsT$m@tOt{F$a>_U@bP~jwLE(9_;hc{3 zDg#ty6`}k{F2b>kn${8x1)a2t4#@5w5~-dPbOFS5A>$wkxu%_-qy(IG@p`ShkbH(I zlu%Sp$=US9mOjB+9!uh^Y_qlk$ptt3jKF{2^c&W!Y)v?$)Fw&9Q1~c&UfOoQ^yy=< z2Z7oTFRm5u@C}$XGfVCEzroK+9<#DYRZ6jMoT}l^pCmIxj&ywCQF(_c*7S8p9*kqo zUQ_NSOQKp0PlJQMeiH~LLVqGFLe65@>CQsQ|AUk3;gBE1A75d{tJdR6Um;xNdBjoR zlkviC)z##?I)PRU+XhVM>th0jj~Mv}kZ0FJxXjprNGMK3|KWU<%Zg%6&90rog?=NJ zASi(_Kp^o(mIk%XJdg}$tm4dp8_D9nSO#~|CY7UXmDJQogEK_ZQU;&p!a4Ncjz^yC zrYaq2-$W0>{Y9~oCbTYWn3mqkVC|>yE2@mD5tfnR8nJZ>qh?|yUZ&6bRGzTxoEtfO z%r0&|LK5KkfNNY<5Fb>Dm6n6~$^5lqL4!KUdR&<)pG%H9$@be*33;rUL*ZDc8mA|+ zNeE?{(Qx)b=?q%~q)bbys#$QxFj?>HD;!ZQYl|4!f}A>LOsj?{{sdJzuB>(FD+d8K ziVw1y^TVZjD!jdDM)CUrK-yqf`_5MMzOsagNe%#qndi&De(JY^=1+NxoY!p}fO$O_ghbo>D0`gB-{YA8Na1(@dOMyU` zPK`ezjc%e*8s=@D{rHdYhq~g8aUYTSS8wx~+8;3S_i5PE;t09SAiXbVDeORSONFbm zr32`vZIM=$9t6j6QB25IDBBV`00p(SYnHUAzO0S>#Y)+RBwasL?&zG)5O1Ov#VWlO z^SHVF`R%<|p>_*gw@d?b7BKH2J<{7#{j{d96Z`|APq$qRf9#(Yc$&Wx&@6b%kx;IW zrX-gLG|cu9_3|DKXGu>=dun|M%OqM#ydnr~7(8+njs& z8O5)UHnRJO7LHZjtsDiF zp@lCGjX**w!vYa9L>rFLBT$9B2^s^_;dXU}-V|Fu+CPou>RfQcI`w!1zkCa>%na1` zGF8f6^2j%BpV^S9@olvavm~Ux*IO5I9|ej+cR2+Zl0DzpO7~eCjfIQZd>JGi6&ufk zOvu(a_jr8QMUh*6dFZBN#0GrPijS^JDz)?^!I(NupT8$KJ4}RkZB~D>G!BtU_U5;` zgN(G0N7^&W2sk)JaktolKVEPW;mwK)zM-@Xgey|7b8)#F$x?Z8 z$bXIicL;)T-%Pv)tI6!SM9HH6EreTTmnqEXDuXRao+Y^k81GFsW{Cw>VYw?sH$Qzh ztne9v|@g9SR%KelUafOhc&#E@pXo3kk@JHa3hsAI~9DQHV$9Wk{8_HP0QQL z(g#$DHaNq4ymHd$X2awzI_#El^6?4eiG=(*U`h+07mtTv}gI;7`jBYDOLYxp$Z@fG1>5 z*wIl4sFyw4ZMY^QR#cWbi8RK_-<0J!uqH_j0}z>gsL3ZzC2Vet6d#+s%8y4|mR{2g zM~zH9Wlim=;k30QxaJErw#Q*lqQ<_H3+oCI@~pWjLoVJZ|GZG}r`?02MxZQ^?sLqF zrXQW_&O)LGP0P-&U#lU1LZxD!qNfc}(P{B1(aQ&}e1W9qBDRn>m(25ZTW;X9aG;wldQfxIwZ{zPUh90WoG}E1 z`8nL2y0M1}(}$|W_I@4MUOBcw-SSDf?OpY8-eS_%w6zusf#?DrjL0y@gw2^I3fugV z@(=?|wal>d;{a{3OpE|F%JzzAzr=UPcW%d z1VudfkpU3RM$P40qK!Q@GUPi|7#7K2)UZ{0rDLKZX6ADw{KNNBp#^mIg(^t{Z9BaV zGbAn32RznXMXi`3%Cs-mY2kF{PNYpX3Aa}}DJ1R#!f74^9F#!S$m+>*?j(;9c5`FV zl80(a(6f&rs&bzu=%nd~eu^Iwm;fRhwF$?pOd@vHx-YXOlBu(5;+eDczgI7)9yn5Y zQ>?TSMal}N?75$w?%{D5amcPW*x+*IOMe~kx|?DEyMU>FCiHjvqqfo%17*@^POY%K z*i7-MJFwST)5pqz&Xs>@9Py9;aRIWw)EC1*qYGezDA{R#CYQXF$4spW87CfN6QmM#sT&vuqJ`$Bk-YeWU_C7xY0Gy75L4H- zm$~XcuR0y`jQRjL5pO;hf)*tgO;iDEMD)$fpZ;-Kacwd2y|28By*Xx9HssBAzhdqS z%ZKFDhb{LP5geG;lqEh1l${V2!va5)D9@>_Qr!odQ17r zY@}wB$fYQ!SVn5$?bs!L<8vgYZd1P)PuW=EP+X~bsKM;Vv@G7^*-(;%xyrf~ZyXcn z=X0#ed1^#g=aJZh@K?Z(f2<#Vt&ui@V5Q=rL+f9Fxda7u>sI#((PTad+|a-LIio%h;g+v>NH^75V37Gs~6D7M8lcyd5KZ zz7!&njXwH|gPXK1#EX~e>%JtjgCY}iWp z4v|*r_vae_-oy%m{I3CGhGF%Vu758Qm*p{R za|(4MI)|u{9`kBFS^UqBws&4iL@(#4bJ&oOZx=*=jf{Z<Oq29-eyb>Xg8fq0 z%Y$v$b{4va;|5PsmRPz1LnvgGSyrymIW*O6n%)Hr&vZ~f_lX*>stkoQ z{+XlZOFkCk7)AYDQK{NXfPh9cGMGcgj@yIPVEwaE+0@U1R zhzX!UUh}Z|Z}nNSPurY3AM&QyCQ;Nj0856J8Y=Lq@!+?pwQz@4eFVHuLB|%{>cXAD zl>cfwJRj)1gpTT<4fd5`RTv4p5oW}U$4iAXhK1@cW`T3=-OG2D2$Ge~2n3HChv7C)LB?H#@_ zOe9_n%rNKEvcPj;x}2a?5j$9gf><#YAg4o70P=e{=BF@fh+Xv6YNT3HcX+fdr_`u+ z!SMSc-UifLo&XD#<`K8M0+hA9r}t0CRSAHg{EUYx?zTFR=DRJ{!H+X#XhBteCG~PJ zqvQ{raDS+utE7~62@N}uZn3V@hdexikA%igw^eNO*i1&fKd(8pbhfsRloJcZUJHmi z$!<|2O*jiOP^ETuZrk6W@9~%ugg7fN$4f|g6;z4ni2RD1$8yY_>=1sPWQ^9imBs+i zW{B#hbTEF!n580~t$!a9^|t4C;jX-B@ywuz4jbO(>cnKYAX^D~i9ioPjOkPUjWv@q zW&dfuvM~KrOTOsco`*3P5S)5AM(4cgzY>O-uC#ia<94UGxulG79*E}=I5aXMW$ZKD z;&Z3$!L+LZie&&?q8l}ytoVE$c68;Rb&d@|$-YD5e_A)|wm%16?R|aG((Llmczz|r zYDI`et_w+SNo>Z^#4@onW=l4AvEFQ%S@9L{X$^>{arawziVXf+q^$R2?Zk z0S1bJga-PTJ|&VQPEM-S@N*f?0^PkQ*a$Fi!CyMrf0VOB8LIs%F1N`i_5><8baN{X z!Y3ht=@gABhfm4zwwu{>beg4gZ5xc#SyuVtoRY9}l?^ltrfpq+4Eg6xRsPrx#gwSV zq7#14MBSc4ro{nuP59QJEsxy#frlSAC8*_~Gy9b+gW9gUIV6RYjdl?Aq09o-PCbHv z{k#H*Ue2RQ^sO!j5`B9lgTkW!Mh+w8mqnk-h=7qEI6pHy@sm-_xRY(!S})1)1`1L{ z!k&&dhmH>zPBt6iTZ1zMxefQ^dm5|FVmymbVb}8(cID`$q-TMW(Z0jr=jUAe?6IgG zCS!GJNH|RxBIY35y*}8Sqc{SZzMLh^&}S>4^Xe}fG6_e@D5ce9ekScSuiyU@M9uyx zKa0+P9cWl(WKA7-c2exoiC%j84G4S54us+=T5z=WJ4$;5bz5Y2yrD#YlYHaxSCxc{ zC9gQOOY83Af%1@$_g5vJ2y`W!_q5|r=6%Hjcu`}v*2ad-zIu~@=$t=4Ka}RnR7Gj2 zS7R{OBW+U~bT>$f5WOGqQbr{tSRQB!V@hFUM=4X0XL?CAI2D&8kd+A>@Vb{+onbA9 zE185u|E}#l>F%UxMcpxPlYCdKZv6$rAz5fAE5~9zmb2;9M93PS!mN%9(<3OpQ~~F_ zGRHvl{AGH_*7?5?L?8Q^=_sY@IC-|N(jGe( z#C=mxM$COX?zEtDS#=WcGW{C=?xgGp6tt0euo$fD&Z4A0cLb_Nva`PM@uPwVGfiOvZVP^GeWr+CThj_{Fniy@I*4Qi-wc zqxhl7rYMgn%7)7qq>{rBmtU&nkKobMC7ypc1|zPM9JE z%4PBo&V7&0tcpRH;yN@*Nft94DES6Pqsme%r;e18pog)j>hZ*~DY=tZZbNdE>MHkI=Iq}z52lx#izHtViJ(ZsgZy7I~u+DVewb8T%$0WjkGaBV@ z@|QDv4~ELDWf6(IMWPd0iO_M$uci3uRqHFWc|~{AgPxBDkecab4q^BFFgD_#@cFgW zFN)igOm{tvrVr+P>P_AzwD6aCIF29ZW3_xGwt+{b{xU772X!mQ1o&p7zSDDQK8pez zmE-ok`ZKI1+`jCs6UbwAKjkb5qUoHL`s_H-fKw=|g#DulPfyOockbQLwOASG+ISz0 zfd`)e?$9J{+noj-hN&r%<4N^K%@R_#1$$eg#e|St6Mou^SyCIm+P#|~42UIq*G&X# z(8$vARYlK}%x=IRXUsB>GeHwe4>9By1pX+AfP`M7;?tK+%D%jxNs+HrhoN*)^UHw9kDRNgqrvGY`S6_~_Q@2_Q>XLHlIjW+^nE#DIJpzX4qpdiE1!^CUe{Qh5$P6KOg8 zWc?%zRcbJki20uj^rL(BdvBx;$w!J@gjO_qnBa?7&whGqaKvB@HvX38yI(L4a{5NT z`C})gj(n{Qf(~IoNgkw%4;T&RvP0NsnP)D|Ys8Q=QGfLFlvG0~y8k1y! zu`N~ZyC5~Z&jVwCcHl=$SezNpWWEr1%#8qmvI%6zwmQpH|5C!DVlWuL?U%~?pNlD*L)Zw&QXfZr88CZW-cH@~u@r6r2} z1zEqx*AD}X*#N4wYBtq$oQvq-=$#%jV@Rb2ExG5_*OXb~P9J#wl4i9xqH_yBt;#r2 z*n7BK4Z1S?OB5`atL&=X7rH>kqkK#R{$#^;%Zw8jIiAs;cM!VZK@_!6y_5@@S3YOs zl}|6@Pl)Wm4rSUF+btlk&fq#7NenDwD%xCGkg|p9=qEIeaV&D4p7Vy~)MqQc=|lTM z-K;xlIvJzS8bwaMdAL`=HUVXH2d+QmFztWpD|~M) zO#G7}lI~t(;E054<29cXb_hgAewOQKM-gNOBdzt&iVWQq-2W_omW#-2n*Bs@99GEcb&xS9{Ok z+W}RDHpcEKI#I-%$hMVnvSkrFRty9MmFV2+pEa{Vig?0hq#+XI2jgict;-kHSl zz0o!jC{sV1W17XlPmgq{#}tVJ3Yxn=Na|0mpRD*O<0~EKuITB)m5WTZ)A8QEBDk~D z<0g!r_2#X${11J7U#!<)I6-4|Ae)%!^U`&X9 zTAid`bT=Hi{IhWEW?<4{(R-G0q5iewz$)`35A&P~mu{gt+hI!%J`PN-RlB+I;Rk|* zwcWY_1@;tXO_d>Jwt6vh;OY1m)$s+h)UcKiCnC64iYD>rm6$1=9~n`8s3}&R+{KF) z&={1CD}K^bjd7N_$#m!)oMaI17EG={z-lkjZBLh!Eli)uzh}YkE|WGW1Sa9x_F^MX zR(IWWt+aCGQZiniO<>F>UkBazMCiA6$Iw`& zm%D}?Q}r-W(@U3ZrIe-mWM~{IP&9E+>HYaduR8avMIz!Z`%PD@%bRo7$90?d@i!Xt zLPA0S6XSgq{J#=o;Erv7K2ZP|Nf;x_?P&na(*fsrL9`DbTr5+WzHJN1tb>D z_M1$G#rl*zd9NXV0AnDn=q0!j692%YOXH^7C!n|x#2i_KF}}2E9jn)3qLE*{zW8n1sX+ZY>u*NVA8TLVh8KP5%a1Avq*yg?M0&q!Hz zM*b8ZtIDoOcO(U+0vvcAp&CNJ(@@ZIYoYs3FI$oTwQHtC)+(O3_wJ{_l0)s-yAaA2 z%$DbIf^XVtq#?VQ#H zCmDEFB79T66{6;0%A!;L6-MYlcgJUCtU3XGUj#6uC;|>7N{fxbI;EK?=uK&0UUj-v z4geOTZCF4koqcz}&{f7+7B!>+Dd>}M2CVAqqj}d`(4aEXG-bze@}L^U>Yh<$+ZKgD z0ksb=&46sSiPgsxC4pRZ06iPdZxkIL4lvn&oy=GRu`VU2A}d!whUt5@(G^mE;-bL+ z2`^1??P_gB3N53CQ?uYA<05;;Bl3YjVnu@^YlDPaQBMe3N!jw5hyWEqJe-D8YF1E8 zYk_h+s=!UVT-e?k{KSm_g({nUug$??Ets$YJLp<)3d@W~Kb2>Q|9=c`wq{1pA0k^f zP&BsBL1jQ`@N*;FMUEb1BSHtHlS)XL2@#3UQMO@9cw9(8&(=Z4yb}g83P`ARnyo(b z>VyZ3hN&_E!+T7vAO|-6E@_i~-G;;61ELmuMm`tVf z*{{~2QR_(zHF-J45w8BD)Qd!xP`nwv(W0;uC2AhW-N|uB#5sG&1-s?lTz2KWV}sFZ zcfg;_R~ENfZC!i6nr7@1{w{|6T^yDAcmC{Q6L-PsYO_D$)NdvLFb477CrKA&O3A^y z&!2VNpL_=V)ydjtzK-3rqmQ3Y=udq|&EKx={bMiwcUeVDCt?L2Q3JN|=y=Gi55y+F z7!mF{pMH6Id4Ko1zWcg$#g+W=p{DlZT_3Kb^kXdMG&DiCn#JCpUesfEk1ym_N$PIa z@~*Q3 z@4vy*kjMKZanKWLWX=ThZy6a7W?#SuvJmQ<1C=qSix0np^sp;g^Lh_5=Q66sF8}rPAC5AK^vuV!LkW1m^G&Icu zk@H=4saxX5Gvb5ax5>emWm+M3^YuZ;uiy0C?gKIGPw(U3-iL|ahqzZ=+n0y)&mM0^ zfw?Km$1BVGpyDyx3*bcogDYJ}=BL1*TC(KbkDX*GSMbQn#y=m5*Q@?Vb(Xi|um0`7 z`G;c}^6&OQaHQvEn2!e8(9v;wRUh)Vf-i{E=2z#@$HDrm3Cn*M*}V^TqlsKE){vB_OWW(Pt zT88|cE_^vwQFQw?O?d#?cbK|SCI1d$li-;&zHV5HjpO<8zAfBJ_Vo zRcaSOr~~c%_J+49FN-RR990M>(5=B7Ate|Scw?~{p5Fobxh18uyOx|{(#(2$Ete9k zQjT!V7A+r_XM(TahWvRe^>{lo=n8aAkH1#y zS>6m#om~xXo&BeXfOW*adg-={O4X~}`ZH|=c_z9Rr@4q`**Y>TBnV=KChXKC!$FbXk;zwY?-Mgdu z#~rDEHZ!M(!Zr3yDk1+Gv%7!s{oCe~ynIJYFXFaIJ>!dG^I`rFIs({Rmqftzhq^k^ zX?#%t?6xcLEgI-Xquz)y)JtS-?-761?pzv0oAe-WTL&b60MVME;3B#JwMlT#(KM^L z@sVilWyA+;jro+hhMt}zq2%aB zaMFT%iEI5KRO^em)a|PW_SdI*r@vQoO`}QrRM=*ctlgJj0>&mrD+nUea<>6x2 z{);#M^Z6&APkT;-P6ObI@f(sd51ut$lTyflke~7SXTu z?)&G@Zc*7imlgSMdi*MML;)W9JH;}Yja|Ul68OKqzbDM$`v7D_cS-awYZkb+>r72e z-EfJUM$^ON>Sv9W5my5nx)sa z0%UIhj5tVqzZpTijSDpFJ5PJ=dArUA0OWJkXT@dXVP9D4x*>V1`0LTto4_5~q|>IJ zo_`J@*AAxv8}}9u+f*ATvaLdHz=OcF8|9&+CnveNRJYNZH^Lk@}BY&V}oGnAMm90d8~DtDxG28hx%G z-xcaqJjV7d;_wQ`(q!dbWem_}8wqS=DXsx_ud`OT9xh5di}NXAzeHgTCT^i55kWLw zcOdh9l^xnr3?QsuOAR(R>Gn;xlMa$ivk2(OzrP!ge*w=s7AX0+())J>u;ZZKoA_Sv z!&=ef!-3R+=*;c6wjS>Zp0=)w%=)gopS2smi~f94zz_&F2AZ+`pPwrwJ^TrI{No^X zZ#>Zm(+D_zZ5goUy$JB(udc2(wK~oQ2!{Kxde%+6G876GRMQ{wdvEjK0{-Uo@A_%b ziDxl^V2#yMYtUgu_PW=+MxEhN7%s!jywuXi9T2^`P{Ni$F=O z&sKb#DTw~=yC%1Oi5QcA%*ia($_T9Y%qAoyOETjaI~1nr$}8*w_097ftFmcaXuK4x z>F?MlfkC=q+a^i@w6%FW%nFIIRf9jY@A`Bhu^CPg^pb%$fUb92)UUZ(`YhYEx|B)F zeL&tMKi(vNKIuIA&%d---nT$En)JE4f#4B*9&Z^4$mL=7+22{f8>$Rb-Sp4;T8kB~ z1D?IrrT4_Up8WBas^=`~^~Vmjo994Z_9d(JVZe8o#z;=3siY^65wpZE~%tpt_!L4k3bITSE0Xw@PQU>BS5 zH6C9EV!7rjeu5Z%*d`=DQbn0Uyo;vER_)sMD6ctRzcYex(GZT=r@iPu)%pMc+(rg~ zh^D@5t7!S_wyoGD9CiZSPOuFwF+&i*T0R!s3+v4;;xB74~k?Pw{R&>95ATW}| zAi+iH(>QGVv-4l#n?Ovv;O5>ecV- z4~0DTI6O%Mj5bJ_e(dGR#s|hYO0_kD7&t6nV>VWI5f~0Hq8?27mO4qCvwP^pCA34L zV5#()jC#&qr5QD>Hlo~U&UE6?hCsqse1IOffFBRm^9><7>z`;~WRBS87?dTo z@j<)J5J)>74Ia|{yMxDAvse8N3h%FvSC*FAAOGF29Q)K6uB@(N5r`grXaYJY4uz;J?R^6wy|=kN8(f4(EM@= z8kO|;N{Rz7&(9v|~Sy0oV;L*J()}%|q zOS*X#tip4PEFe2oZ{;%Fp==F#rb)ed`)edTMc(cSX5MaK6D81SdVlZzG9KlKkh$3+ z4*BtK@bCQ}sk9zSR8-&B#_5o*aX`A8p@(iHq(e$Nm2MD`?(Qz>knV1fE@=q~ z`4Un}iM+?(TCZ#Qmuro8=H7eG+0TBS&;EOk1vc9K#u2E+iI@MOSI^y)6I%R#VBueD zw3+$Wt^~_MSXbk}r^49pVt5vdU*%z3&ENHEO3=yCvBjVUoQirLHh+HBt)dyFrgaD_ z+ijRKso~)J**Bml8GCLZ9$P5M!1-a1#MQjQgAohQe@5&kF*1tE|;TpaAx@cUc*w$nG{L3WFHbqZ8YaL@Fok8PmQ7jn z;tbm7-0oVASVA%K9334i`NuJS3zEgcTQO7^p=i-!nTS*&it} z`!ZMGrU4-YEQ>`n`aRN*N_|?{+$|QPxRxh_tvYBTW@=+puDk2&d1mEVrGZWR4=C;6 zxx@ok_1_}PpC}M-@A>eMHFdydd=Uk%&)9uVu0Kxaxsh`}$!nuPoJ_=_aU<4~U>8qe ziW!v2o32AKC-1=*4Wypl@K^Bphk{}#S{!^3HcuxLDbrO(wCal+Mx$yC^>ASMk%x=H zIflme_Q~tUvcoXBxPH<$>1vAf8OQ+9kWSLca{fYUoVmEU>&DCHolRm@IdA5_F;cPJ1U{o8e^;MCz2I76;q<>$XY zYoGC%cBAya-C1|)eVltqH5_aK3Zj|!->v|BL&?I^D9WEpE{7N#gLSJO2POUlzc)Ks zZDcATkVhNC=Tm*N%&RwkK^n&DFphD=B9o!$@V=L`{2e!GSX3UQyk$G*WmT%jD@NK0 zg|aH$s<{qx4vH&x;bw;yc@RyvXh2Sw9wW@+@Ev&5()SvOoX}_d3}tRz1~r;7*olBm zvVswgN0p~q7PpH!K!??J>fqD~URP0a{29O3Py}!%oc&pe!X6Cl7g&{r7;vCktVIZ!pbtNFlSIuSqdc2Vz%Z= zpqXan%mxwPw7BE>xf^0MgLjhmsrFK^;j2wC;Ck&+kqTjH_A8l z@9T^u(#4^Xf_Gz7qwU@DXY0nyZ}E}~sGkfcIapWWhXrK7x8;RKjs}kd9xWUF5oG6A zjECexuK)`~)p?X}Ky{?$RJW>R=q#91ge>-4Sc(6Xc$BeODIgYy-pkK%>}a6gK8sPI z@ehhBwkA^v!U|6c6y-tF%wM;24OU?LjApx_?YGDassum9|C`$s*Hbq zq>%V4G$X9D4VL_7_G3`6f1pLiMR*gnCmfKl;3vMdvNFc@t%*?fW5kI=m|Vy*EXD2C z+UMGK*k~sa2M2k^ai++91~J*TFp7W)El1v2iiq`(*%(TQ3k$Zqaqjqr-T;TIodN{i zoR7ZE3F8JlVr>_bnmqJ0b+EB_Qiiu2MW>{5&DejArp0bH{+;~HdM<1K{udqN<1Ljo zv2_t$h7YzKDsl;bzn|Z_HNx6xhm`BpnW?`5<&bCSC(^d_&g6$2fK3VcuutCO7PD=O zrKV#!8f->QIyR#hKN2G-*`tWCBgGGAZ+@Re-(+4NA);XK57|joX50dZl$!_P2*dLC zhv1b=c$s~>PV(k+x?^fFb^E6hrUY4m%vZBsdMTHLPSqX6@e+S;gYm?n2W(<0r5`Vz zQ;}ikK@Rfs;V!f<91%@-%vgX5q{2vqA$@6y(Vl3kXxV@d@*D=t9t8$9yYRFS?9f1F zgFT*c5Mrpuyrsx4x=E4#7gYmx6|?5+kym()=Jm`164B1~Yj+-uZiDHDD{lpGW97pX z3TeX#wxu#}GnU>Kji9_hm$gSE^O>GzjX>?bjK@xXE#MYQl5#ZV+S;`QbcW zDspBfmQY7C6OB2f8+QSexHuI)2|5a~uIuPyCxL8|XgSi$#1HMLy2uR+CBnUb@Wp|_ z{7(j;3~93#o+!CTCg4334T?g|proLa>zx0U-fVvP^7Pd~*v&i&Kbl>V z(mHluY>tj(I5-nGZ@beB(YV*-{rpO^8JI9F1`=G-sL`~4aq zwk`r>vA=$Kkne*6zHTRf=J0}v7x%JKBo@1a*Yj+n9r&`KFR;;aRnc8F8bric)Yn+z ztVloOBd|3Zan{zJGx75B036U$B7SwAtzPRQu$SZ?%+vnPLY{)@#DYJK?dl{>}a;D}mkb%Yg98 zdP`?~nUrqiv!4pg$BF2ql;*y34dqDDewWnVJs#T zd1etJ4)8I5ThWB=)RDtfcza0E&hTy_3daLS2;LxI3XYE$5!Kf-?dmHvc4X4Q1eJT6 zz{YAw_Y|WUuT&Na*mqwCuZz7e zgK!Hf(-7er`FDKQ`h_E%Y;-Monz|ZXT}>7;Y@#N#IvXVsiihz$ND9jcEETj;&rwG>N=vl^N=^4o}gwt za;Nk=?{{ovGq4c*d;2|keD4cevunE_sQkq*$LR8=CiDL6G4=kC}TZ z5vxXk=VEyh9e!K+fP%zB_@a=ejQhY+D$h4oo~|b=^hg?$4bLnaipC6G@BR1$3asXq zman#ap!V}Rn9TI-d;~ia#hc>|xW|9m71|x!L0HCDS<4_)qgplyUGSioS-k=^GSDGM zMn>c-OLI^*s5TwbYoxa~u#rzfesC#YY0}Rj^`qYX6m3xup@<(X(Hwsa0lJ zjQ$ z&E#>(*Vs!@?|{S@MZ&tt)36yag*O41`;Lhqn?K3O*rH;+9;7xdD2Xu z5lZm_EXKUPT-Fg{SidMu7=}1UOLjDgl~$EXcB3XUT`JM1sjx5BOuRP0hxnC!n^d1n z0!;?g)PTjqfz2@+VU)^C`*K}frEVSM;-H-zIq&K!wSTaO^- z+Xqfegu@)Se@BJJ>toOk!V?g{r4;)D9poo{43CULK}8T8R2Vms1raLS63877Hf?U3 z!d~yyis#%wB4XbMc5+w2cQnBe#jdgegd_HZc_8lb6&4#L4~4|p;QX27n9wZ8EW>Uo zyq%&!dWM>J(y|dAr?*ixtN*IR*Un3hHjGACR_?q&784<-Or`;4y(QmfUC`d1T-b-_ z_Y46leEB>~Im$f>n-1rJ$B44j?q4__7pKKL!yt{M(8|*o8)*DBL!zBPdj$1&UGT)j ze4cFd7jQLfyyXP1%8s`MV?|Xc$=WL1+(X(eA=;OiT`Q2GeCO$EO&`b}?tV z0{cxR!}5%5TO(fPNzdWaI%mo)Y?7h*S?FH&gd+UYQ4T-;Wtt2(26_Ve{iDfN=gqdW zH;gTq9f@*bcTM3u+pXUfp(WB3%AL=@n~_TQWu*p0BSx@;4VrATe!Ec{z_)L5BG~oh z!|cOW&9treLBX@9P00&;tgp34DP+dkWA8i(3Hr%)gh){)b}xky1&+$EUxr32)E{ z`PUk8hW{Cw`$V<5qHzCGA6HZy-$Tjn%0gVZWPWXptdi3#8^h!Y1FgbkUiB*I6PHg8 z_dBecZ@D5~U*Gc2uJVbB9Z3;>oXj+<0F7OWJfNC~m)T`){!^guSJfGDn|U}8dbt;@ zLHG|Y3XCm2#n#r_hTiMz-@@+eff4~+$7Z_M`w0$3TCQJ|P-F);Jq>f*7E31ttR?8J zz{9vvbuC!XkrY)<5U|MS0IG-n9=(|Qs?Qy7Ei5dAbMyGKHye8VtF^yqb`X#*(dOS7w2@V=Ew3|(%nlsLaK?^}x)Qla` zZ4&Sp0ov(}vR!7R{M#{BmG4?fs3B={w%Y6vvE1^Z!A8xBh~K`e+E0*gw{%- z&H2p~h5hLtiqSMC)DpUzK|+|IMds#sX&UKhEvjQ2KNlc zP&v@+m1~hMQICG@SX{)D9;_(Qy0d6n8)0={`S>ufO!{r2*#Lb4= z(@p!K_*_H8D{^vNiH`b$a4v!0mq(?IsOYetQWQYpz$#^+|`rFUe_ zU&wJV5hiKe-)yzJF->F52)2q+dzr1bSXI5C4cpQE1psHU9q%1dCibzoy3V86J~O8( zfp)Oi!%3sp$;zB#>&@wUD|q}3Y8aCu+3Dzte!b_!V?-a6#G(8`@tD;$o58@xc^zCf z=tZWGVD^WK8N8L_u-zB89iy+15Xb_C|AdhS7$tEZB+%w~x4)Oq)hkLT=mk>ZV-6&7 z2lYKOpVnBbCFB(j$1i%t9iwOLM*rQ73%#UMw|uW??z)Qr;ndu+^28m!AM(p870OkW z3Ufa6`AbCq`$_-P#Ya3qK=`@QPV~`#eE9j6+t2RZ*T$|F!})MuJ{{k{KTrLp>YV9r z)t?}v!PT1dc9(KTL|fk+U4(OgKRjwyguEBt;tof2-v<0icF5cp>c*L2KU&gcH{;r6 znXgIP+j7Gww;vc~;!umzftEn>4E6lMvR5c|l3GK1q3O0+$os@Ym?B0$(w1K~8boJR z7HE6x?Zh=!#-P!55hwCHYwv1*8KPX5BHmHs1dEGIm;{4`5DCI5ixqWQI)}lx%3U>I zpktIDQx9~^ENft@C4T0A1p*FFHaBNaH$~C@wOh4IZ?6xxYL;)Ntj*)())EQoe`$Aa zrkUkYI62b4i9Al`W~?b7LBQaEj08Lxd5i>J)^5UEW9 z@H0wD%Xl+ohz9%yPNFC~xIYH}i-&RGEICilDIn4HQ8!pZqvU05C*#VAp0By@^W^ti z=vGOw9g^3|5LatD#BKSB>#~B?CDqkmwoygc`)S^nPl59^m>0>sR$LxFOrI4WB+6D zU9o{^%a=5hK*I>CT8_yV%$S1>)qLbaDRE&Tk?ep}oBDUaD5PX$MRYW$eoor2jSEi# z?N!XF^QWD0j<{5%_;y-qJ6U;WA8`M)bCjo3?e1KrFQNjxyc^o@?IZ_cAb?q9o0az8 zej?%JuW)=PS@c1SHUb*l%u>t>z0~hv~~q0a&^3)p3Jkj?Y#2cH>D)AO$g%Cw#r)Z--z*Bb8$v`9HtK8jf=3 zCopg1U;a*J+9^%N=25m7LBMgm6j}V$dy({m> z9sp@JM~E?VbKm;`!T??pe3lrYSlCOXVYP39uADi^;+|Q#vr-6 z+Ax9|2RRmBF1QziFzN*{4oj+1rCxo~EK*V3{6z4Rtd?21H-*phEG7v&$?T*-f8DBeHe{anc|X=n1AFMb zs(xX1(n9XCy#NPjygNAR7h^^~B0oC+*c*R0EyU!H?kN%}p0kQ99qq8*RO})Q)bs1l ze5i~>us&-ms;WJNy7p0vmoq9Q43TBzcIUmYAz;qLz} z`Y#lt4LK|s$1#hwnMO^&o60vX!GBhz;*Etxm%6w50 zq>|mxpeF04yad8QPE82BWi-QfSz*!YYdxwc@81jZlNMWb?Ei~^4)aBBf(Gjz18G=2 zaHg*d-b%^L;5NUNNmvZyn;c>hJ<{-=EigPYg{`cuAsNP;7bwcujw2{ya_AKSmB7IP zN52*503H&5c1%vc;i!yt%WCivI!tN{7vtAUa*CxiCTK9pewD@fw0sO2T9bU(Uz*fy z6}1$jWx@yjbgOhQ`LP*fu>=A3Dd!PW%^QgjMX_a4+1l`#y-W)}$DWXw>?&bvl3f;P zPnXHV?LG>mRu*yE+8GqhgV2cU8jBbrQ2d(>6hS=$R$fw*ySu z^TeuZZ8DVRYs&!Em1RA32v`Dv3?<*EV#mh#4|A>}KfN6M_+x%7D5rp-TN8(A3e*wR z%f~=Xs;N&Die7f`NqLg036iu&5s3QLR8fMAT6k+cI zBFx&XfCqW%`P!G7FkFlCPd-}sr>?Zaf1L6&V-vt%2UqKI*j%7J#Kke_v|@g1GO*B( zV;R2P7h5N48`)yOfnufc27G|Z!u4VZA!#GC0W8@7l!5dGDx?T66e_@OIWz%o>f=ryd@6a@NG|THqtpypM!lrN?4D@e5HVfEEIpb0F%~1lI3@-6My1iD_wBd1et5RF=vO&z5bayv2+^9jN6J)xdeU{+xu=~PSmB8h(U;AkG{XJ zr8+)S>(1`;$l^dyFTYt%$JRc6A$JPZEu$06=LW^j0Tc)qaOff8DIaLb$_0#VwXf%#udWX!~qU0$ZxmA#-Fu_HJ*;9B#~?wpP)nm93< z`LBH9m(UC-5^!K5Ca2{aC zb6I|WFKFq!Y8V~Yc19i7R2QmRbLS+zL4Mw`+Udo`89#2h%XFlS6yqbFPq)jw&P8b> zXv_<4Di{5al5Lr1N10GR+Q71(VO2i^9BKYy-tahiFMYVlyt@0CK5@2;-CcZ9s{O7~ zpRrr!Zos5?(-CO*B+o%UzFgO~+ zO>_%|2UO&}V$|0a1tB=jlkXS*Tv#9wJ{S%P-%<3`TQKZlh|4a~%=C_77)CtRgh0g# ziJlv%*x1LHtDn1uhH=D>dwB=o{Vx`d6)HW=O;EfNfz}O=pb(oOyl7R$pN-|17_eg= zO!y;tnfv(|z;AF>vr7Ae#9p{?;g(O+CRX%-*FI`zwiZoiNc^eCDuY|$UahG*)68%8 zq%r^Dd{Fx~K=vNYaZn#7nrJQ#jdQB6A$H}Rdz*7-@8?lQhCA$htIBmwFRuv9<2lTc z3{sud7z(TrGA|h@`(n7Y;ecoo5<>fl?LLhM5#ipPEK)%MJzy! zlu@dv>+Y<)*p=0_$8TShmxMiz_c~5~iz4=~|I^=AatZNV)y_@MjqARceLT^f+>jwl zMWZr>!2nX;4JI>5MO?_`dEq(!o%)E7FWdq=x9vv2Pf(Xjfg8Rk%oM;~&So&nkPD|O zl8e9unX=s!`4&Y6aoKeDhCx+i@0^4vxjC%$fbE4g>dq~`B#LAUpuO4#SJGj?Rh+D#xEFl&J^IVADg z3Lz4bIxcBGjC^CE**MZJA;- z?W!f9GLG`DJaLHdKy{DV~h-VTN? zTb$}<_hv_ZOE=aJ+kfLo6egU#5U0FXSW4JK6!(g2yR1osFI$M199LFu7pNY~wOK(A za9YSXJY#92Y!OfYYp#6oL%$d~E@&%#`9Tt;NcRyVn|o_OdWI#Geb2ZgDhfjS62e!j z%T>fqZ0#1A38{ox8`}EPu7&R{AKE}$Erc|;T=qUIx7f8qF z@c@_P_U5NKj4qM%|4epe+HEF$b&KwIz-c^bYU;&|5IUE=FNJykEx+#6b8Qul$X0uV zA#|^9V{{VvEuxCjMLf&B!D5zloLQKH9o-yl@ZVyviR8(1e?{ zZCbaaM*0r~1UZkK&3b|qMTmw)z;K68C zY@bY*EZr*ASvf3pq?oBsXz5IgMX{S!SV}DIzEiP;_DlK2WGZf{ZX&*v4EZa1oT1PJ z0(1&imCMIVZ%HU}!0#;TE>%x-_ltfYyx}E^xa_JTus4F>TKM3F_r}i~0Nn02e(CSM z#`ewldRsxU2#<=$tZbed%XlpYKTP@*wtqr*y~>x-i6L}pl8mN3y41N93O4x%8<&HJ~HatPl%oexv@TCk8W zTB1k7+g?jZx|0Kq-Y$oyC_PJ5%pg5X&OROSPhm1suVL+je%L!{3!MaksC_FRXPI$X z&1h*HEOIGwZqa99fim)}VKh+?B~isAVS9mW42eQcu2=JtXaDjWl9o_r57`;^4RyV8 zbBN8i?>MMR8B2Uqxv=avuju;$@w5*HKLlkV8L@AJ&s%k~02!(FxIOJYQVFSeBsI`9 z&>-X)q$XjiFkxV>v&&*>{)n|&Zm;9b*c^~ZZX_!&tDaH}&?ibv$%?J(fc85+a)|^y zv93zjd3^)&+{MtLM4|H`NGnA|k7(-~DCknUws*b1ar}dU*i4@UN`88F>H%7iQ0ocR z{x2dyi}N#+%l7XH!Y6@Gk7-t%#bfvthpPt%)+qB7Prvf9&fN!3Q?<7v|HuL0j&z-^ zer8H9MrxC-9a-Mz6nmP#M7?zwCG35y+13w0oeO2-p$&3!s?G{zriq-G4-2H;eR7!t+^%6(w6={KeJE!O019}16^qjh_3B3SK=yH#vPO-1^m zclo&Rl!#J5|L<7K^>3Oy{9rhr1)U81pc)STG`NloT0x8{`Ug}kPk^y=B({+GcJgsCs>9!KCwX(?*=5Ex|6$gSB1M9d! zzZ7|I7J-B$3owupM}znXSQIF+(IQ20hF6kVu|%v7Sf0PF^g3Nj)mLfmW=;>CW?I_B zN00zSpm1ObU2}%ZR;3&9Vb`gxw=D7GHH1uCW@vVC7MPHQ%^kTDUTY~l1IR7GrKA5K zal!02TY{lrf)`}o-G%fuD@n%+{1E(XHCKWU?*<9JjV$2roWj&(p6wQxx%jftyGo)s zFMg=LzQ-#ky+G#BE^aZ^{a)6hCoO@sU!7oXoi{J-Adnp^59W)3cM=$IufMqYi-QNN zccl=xmT+!KtxL_+2iUcWzdY}&o6C%mwl=bTjBO-HCx)>8W!<8hZK%HU18$5Be|=ib zJqR(56dSt_ryb`WeU#V?n3h(f`^m=rY>HhM372Q0!L`t(=(bDqO-aUH)$Cp8{PD+< zu?P&#Ytm6^)FO(Fs#10Zqz-|CgkkcQF&I7r0jWGd1-&JZe)!*TFlCQ>RJvl@9mz9n zQ0z@T7m)UY%T#=ec{$u@COlWrFjXS^!g8P;@)(*ZyHegRGU32iZu z>Rh!Kpp1_mHjiWA@B{vmlVErvjc$=aV2zcS3lJ>h9qnZoREI*9b{jJ!pI<&iu*g1} zYva>|w5cIFx2~&HBEVJgnO?4N8}x}l-WIBlF)Gno8d~WPuNz;# z#dL&ar#aFXg42d}r2%KU28R3Y5axQz&B^K@w9&zEEs@B6{Dl1|lhD33)de5@#uw!y z7CLS3UkR7%AC-v&1bO3)v#fn{Vv1i*R>Vc{4(w8KOs=Iidla!Rf>x)Y^S$#DB!ng0CjFGcABpjEyb z1~zs?j*QG}-|nuq9i=+0mc|&go=r#PWsipS21sA9Jc$WUvC0b8a)nMXynr9k(ZGzF zapv=)l+0#OXp8-|9qiWg0spqEtHySBSUst6T{|Gt*ctU1a;b&W^4cfgr zpz~=a4LAIUeR6slddh;eL!H#+Nb`}89sx|Do3m|NKLtx?_LO=ht0Gz~6VF6w4BA<8 zvL4`fmdt~=aYGsK`TsPox>NSMHzcZ$8f@>FlrmF!Jc#`dz~HetlN56=M^v z^F1U1wMi7t=-3EGc6U;rBAzgya}jQI?Zo4xpGA5=wv#os;sB7-c5Un|9lXi zIeXJ-YHjTU9lDQMX#L8m&1u^gK0!3v=&7GLD2WW7kvDJNG*g?)7t6JE$xzU6XhTQM z;6*Ly1*8qj$7W{XRxRy10O!D`-BPFr+{V)^tuzvHCJyAyHIMGE~@bXF*U<@j}SA{;P zW)_^Eu9BcOf@5{`a1k@NIqLuhDA(rg-_c?BC1MO}zgCu{cIEiWhpSD1#%ce{cXJ6t zX3ro7dwnM!@3lZkfSHUcoHG99Cyn#FVRM2I%BFeprWt3HU3+wi8Z2>%RHeioh2E{& zWeycxeEJG1f!x=n-(5L%3H9J#DxQBDHf z%Ty)d+>tmyZ#xLXtXnk`88-~!dqMkEI%;s_3EyemRtF~{q(={nG=xxyD+7aq$1{`SCzP-ENSc^RxO#?#i7rpU7=?5fSrMRqyWig#35#!{Ks~;UMlcO zt1x>pX)Z!)SWaIyaA|wAo?DQJf`{xca6YqY|Fu7V3JG6tTNcw^Ko)Ba7mQT|9>c>c z>-`o#M1JS9ilw+c*XJz;p5LY_CL=J`%*(~lTOBqqWLg!RFD|<;sglza==+-HP%Vn_ zJDF7=Meq39ASvwLy!J0?vEONu_aG0e5rvjb25~~$?lnUK|ARD&W7CD>bhE?m<*nE2 z-852lF_ejgV5WaL9j--{%N8Pvy4YuR5U)Wq7Z}lZDyM@pgq7eIw)?K_6%{P4sTrp~ zT!U7nkq-kG%J3Y&*}K7}t>tAEq?eIz{eDCj?9;%HL@D&nE({SoFzoKiH5FaFM-9*WZo@KySRzq%?ywzU ziwe6X>SWJFj|Y=*5S!#e>73TV*(Fh7&sE4qWrX99dPt6z`(VtLd?qHMuei`2#QSAw z3EahgT-_leQnC7>t9(}`%*yLCQ(qvu4^*_G2^d<3d#f3go5l|IaWJ?)qt$-m)A0U+ zYdbOzvRA2~7${iht|BJS_#$>uDA4YbNowPt6!`jg_H@wvvk?Xeeg745ke84qa0XuRk|EL%@f&}bsOJwdqY$l_nzYHBUHQy5i$uLe)1AFMFW5A z>gpiT0bf#A+2alg_M?QR-oX5^h>VzhYoN+Hh+q4szXQ|1i~PZ z5cb;<%mZ3d^el&(vDO#Vo25!6(&S;xNAH}`x5KP@WQW=uj>gq(WM;*jbgLu@hra1} zt~hRCCEmOh$nGa@f})ei@wu*~tj7(Tn>)U^k#4MS!I&rv&^tVa)3j1gpp!Ok^O~cW z=W^|BYm*@VwpqeCPq6hF<@w7VxW} zg7l8^T*|Wc4h~ZWxM&zo0m8o&?_w;Tmh8V|X6JO)pD+LfWrYv1=f!9s*d9=SvBp{32D;LGR1YPpQi#Q7F__ z5bLHjxVKxd%B@0nf5%4!TJk7*0MN-lyAOy*WxhglQ1S@qLvlsyNxz@32I1?ySB5oT z6-+`}HvvcW!&mVo;NV4@+5ZrBdj8E`p>Z|jUHJ<W^sp=~;&NZX5_T-g^ZVpr0R?us|Ek08^gB0I z8I#+_K+f2f?=8(Ev6%BV!T<(J-J?I%GXJy>`|qOr(BCR9q{A4J-NKh*n`mi_M&D;| ze$LOtpoTCWfLoI&)w@*|Fmn6Sf(y)m%em65ATH|BLV*BcYyQmoOxE1*L$Daz zRe`glFnHYCkRLyOG;umO)~0Ns5G$15dRlzJ1DJ_81`1Tm$Ou=NKrDKA;mJ63 z!jcE#`P0PP245^l0dhU2FwauFiht0~DkPBTEee`}>W6Z9HrDIWgUq)5n*?|zqXp{B z+N>l=P<;~CQcx2_kZ|p$YCWvWS}sSn3FL#VciJSG|<sBKq>e#zMBjvq^%P>y`V_aj&|@O^rrH6hb(G=c!)yoqV( z@$HZ@gvUGMTEWkT{R9eRpTbqk8XFpTl1O=y93|<|Ymgqh5q=(Mw_*h35xn~;^!eu^ zQp6y*Yg$@b;5$vKHyh{>53efIEX|;+>=;t%G3Bb-SXTU0P>|NSyuRj3S_S98iF^I; zB|TEXl~v%5BnV-PyE9_t3J6(XC;g}XE0lA3ZOx$DSa9W>drd8bR-9Eqn;7*QDVTQs z>)Fm!*llxY#b+*z`d<_yk?-dIvqUFt3RQfON@Ennr|39wYd0l$KjAI0Q@}~-bFHEE z^p+a#ESx%9q5>X^>xW1APk~dzQdQ4gazmn%i4?K4)~Owk%9Tp*^_`)%JswV!%#XzK z^GHhP-)aAvxIfb~@hP9Z*0ksmeBFGHdtbX;=i4I=zMdPO9;x#M5L zxzm@IuC>d6Vv%M@*%hvrby#aJzOE;nu38rPMP91t7JEAqv;>prfsMazQUFj-aM=>1 z3%3j6>%{2`kzs_91!GuvzYh-%R})BPsRaHThdbybZ3T8_Ha^Oqh+RM%hwp}b*%6c9E>Jm0%9V{;pYW`PH&Y7? z;{-H==q2)7=D)Q zD9TGaKkQgW0;^9bz{hc~Ud_8mSV^kr3)du{#cP(ZH~3th@1fYquy{&SKF0lCI({O_ z`}_P$2U+Gutj&-{ZeRV~%+UR3h)nFShiGqK`w9^dwA}~48XOO2CCLdTMn!i(liHD{ zhT!D*$J?UN7q&u0DWR`1NJ3O+iORK{E)YHW4l72(aS>JNN zwWKMCUUArb?$+^4-XJ7%JjVGYiEIo)V`WLM(BViU{fZb+s@Ne|7W-dKVUj&F73j%n z?`RVxGnvME6DO9CBn=X1HS`#bCco=V+{`(wPrEh?xCZ;=NJ`oJ&W?YNnBY%Uiqw=z zVc@8s>nq53aq=YvkPdhTY>B{$ zL~#9>{esMIsfT2e=oMw2gBilt=c!Zb|L+Cx?FMNrjQWLYG}xmhJ@0@===dAd7W-?l zogMJ$BX%~F(>+>AUpy5hvL z2^VH=$~}8M!CxZ@Fg5Hftf?kaWnCDrAb#apOvXn)Nc(u@-{uAPQ^R*{znL7J1T(5= z5R*e~?}ydKkYH89+d0c`n*qhNPzJrClfDUXz?DS#>Q$nl zTa>F;`$>UhSwu~g>*McJVTSSy=h2_DBQ(a(03NJ}IpHdqUeSgM|DE-R+8O~=S`MR{ zP-3o7=9{p}pR$juF4Xh1sY;}Lw75vl(#fV(j~fX#k*RdYY|Nl&c{d8(X9-Qc#|h+xYzJg@HgR`=WT|AkI*Lohb-f5n;l`-qDEPHk7jJ{#GI z=l~V$#gGMY1VU&aBeP(A1tF6=II4RRK7Exm6krzY;Q8e%7Odk~$jAZP88e5A?3}qk z&Fz$-Oyr{F&6s8TK%w~Mjm3Ez_QU4r+_QW#yyR`wQjjU^^V(WxbXDI{zGG33Qzu%W z$!m8U2I6O-#-TS-JKbYHqd1)bO>9m3u~JC zQgKU}zNxjf;axv`AU+JHlXdyqwom3cKX4kSKPrS>KTqpU6sN2Cuy`BY_@Q|;5Wu(mz`3^@ONHj_w3d*7+c%S%9P zf=Ng`WdYa{fKo~SG@&Q#uL@MSNyZqfXE02Rn_@3{9olnaFC4*{>@7Q|(#|K4Q- zz!Bgw>Hpd_w6@7CcpMh`csb6rb^o>Xo8Q$`@9%fLkVsi#xx3xxz1~|NZp5x8d4ZJ> zWVW0>KCE+fp78jF18P>`s{J?5b%)z6z{2Chk7Qfl(8>!G3Dk0m^5t$jryQFRq+MJCcEtJ_mL*?6f9dQe{(S@q>r_&AGVxg)_6 zs-NJ&j{mV`G|1U4X4>T40WL_})=!B)=Yy?V{txjV|9qd|>|{?>+Cc@RKEOUH=L1_Z zfR6zZ+nJXIIt>+zep=PifByhP`&W1WxH4c=f{TlbkB<;x0J$%bTj26nOg-qi?|#eylmx*|ccR#jfW-e$ zg3HeFruTygrd|2lc0iWu1phU_Re)c6NpQnirfLb?yq;eG!S~b`hzv+CR6@!iu>S<8 zf9L;bO0rLtFjwu2b-WKhWV(%=TwE?dIPd)fK(+*e%LP!m^jjyY57%@5XeSZAbK_yI zfxry;Z?6$vnNsYv44`h1?coi*E`A;@x689LU$$l{vq>o(_BM}%r3#MClgdY8c3vlx zj2NK{V1DcnH`b&V6o&X0`1Svt$f29n>kUaMSmk~GFYSDGEz!;5w4sO^`@aK)A$!Zs zoOQ{2^7>D7n$jcnZU6p&G4V$ZfFY7SRb$xdxY_{TgPB8_kq8g zfYyYA1}JJ5YHT*si-wVoVPd)5#aCR1*Z};Y{}^nM4tT=#p7#NNv&GM&)Rw_70QJye zRQ%cR_}B4r)87S~yvJ@vvGW&553RWKa_Pmb=;I6U?)F{a)jPnNV+2iu5M-O!iXT4! zoh}ZbULx86Rgs%oIeBcCCvzAu+o*0E0QBvIZ7ty-3TX#9&Ks5wwFt|xvJjJ#aQM(4 z^G>CsPc%LkDI{VlGuF;ejV$x;gvlzi4ae82f`yOxYhWnc+qeA>pF~!4(0cg@V#3(x ztjZYv)jiktbd_|<&%^U~ACMiv=W7B^*yGp(u=$$|<5xr7 zp85hG`r7WkwcQm9v?%%!A5(*7gl1X3SE+u*kDot9oWCCkHr^*6q*0^S%_|orm-eXI zOPke-%I*--&;+btGIs*klI%99D8Z|d35CggcKQX-j<+wLbeLx>k{&O% zD(~^g2H+3_Z>F`>g4$p3hy)3Jlz@k#Kv(Vk*ZM*6*GemZ3hBJ}0*JL2qzVwR{ZsQx zQYyM2e1R~4{k?i!-|&J%dc!yeZ0ZNKt^Q z7HQUT(|XggzrUaKQ?#=7^4C(svnIEwNzn+LyeOwl+)L!TNqn8u# zzraYqqH|nE^=Sd)v-(QgIrGL$00F7VP*o$Pk228D%*<@t8JC3k>seI=8C(MMMaLv6f206)uR98&#kUv~#04~A4F zjWo$HOYYlnf!`Z15Yv?pYmZ3@ewM%pjf#r;?b|nX64O{<;_1n2eDv(Soq1=%_Vo7! zQwlJnEl;*dfm|0WLc71x(oL8ZO%|)R=1$u3xIiQUitJ5>0T1VWIvi& z{ry7uxcB~rq58+hEylEI;{5?(&I^Q<{KIm= zu}DuaTqb4|b_y!o*DK`w04c@f&Q8~#ex6IjxgTUbi3wL_D@fNx=e9)}eT;I+%e zwLlHYFLE{da^6R1glA%C89OpOtdQ*9ga7UYFy^;c5Yf%rTOxK<=G@>5$ZiL1pr_w* zyOX^Jpqs^-%ud#pG%rCSFG(D*glK3LIveO{85y}}sVo=pP|4h~>;x<73T^zgXa0W{@20)hZHT{R!DDBuEK6ZQLd z1`t{9;UHMVBp0T88UTN))fyCfK5TnB)UUWMf7fDvv(eQ6?(gO0W$;)>h<`KMs1LCp#EsNUK|U2`im>yLj;Tl;AIEoJVv$8 z02kVJ+YXDl3@&dZ<_n;f$SawXRyPC8vcgUQ0y>7-B76sE(P4^(?b9&nMS2&9z6}fC zNajdv1l?Zu*TSc|6B0L9Y-{bN#nS*Es)ezG0}bn<@qpq}?XKSh$>! z2N8R9lF9A6fIsg%j~9y`|LU^N9(w{vr`nGnKLBnGgqy1%Xi>mbNQOi9^Sc2&Pk?mx z0Gj`{x9rbVRO8;~J%s5)e*jxVOT69-JX7uEUI4xNTE5#Z^o}JJtk5X{NE!hi|KA=$ zle^u(`1dITmH<@Au^e!J{QNNX`~ditToeg;fFKCKy(R2z53V6@SNCnt_b(wdW4!%T zu?j#}K7%Kqwx|aXna`z^t?OQLq+g$Q!UphE6f3ForH1Z9IDdZ)E4O${Li>9E7kj+t zwv2vgAps69>c$xe{zYrXuhFgdio?pIrxpYPHPUhQ*!9w;_IbH4M+1gs){`Vg;0~0` zt1^-T5H)d0%dCiXAwN@|{$Ul0(g?eM_h*t?0i`HiR)n7fgmSM*UdTrvx^v!61v_H@ z<_0vaM0HD@eDBt(3Tyv?GwaVg5T$K8EeYD1V>fg4(j@f<0^&gX{uCj9_UKbqq3Nq* zm^t9t4zxqI*YAKMxE&lsA!s^}!FEB|lS-zGqCtC40by4d0f|%P5sPESoHH*8ZJ1zL zIyCMO5vppaWQ=IuSj12`Yz*1cSmf7CdKI;V4x?GEcnU!X)IuUq2*t^vh6Qu1WM7i5 z!$T})uWMJM;C&I>pPy9&F;>h3&p#{^-v~$EgNesysK_Gkmtvv zZzhko8(lGs&lY@nV`C#+T)!oZmH-a4*==`JC7WN56itQI^;O3F!rQ~s-JR9O+U-zm-OTq1I7l7rvD=H zyXKVALd;59E|}jzrw!Krv(6(%`S(EGO~ZPP@y=|dTns6VNb;S$;;-;B^j;>{%VA+* z@|6DC&wRiK=IxDWF}5jw+LpHx7mtB-1r%tJ1v&>E)BPbT9Oxjqs&WsSt01lb1G_qf}YQ6kC;4u+$$YNQ9P_ zo=$8LAjJQ|O$4XgG$uz{T0MAA_S){Y8GmNx1wIWh=867ijtH^5c*%*X-c2EKR-FnT zem?{l^!IjATnNP-DTDo{oL(Tu@44xwff5i7=~CkDP(V zUVUzL>~KUKtce%i;D29997<7#CAyFa<-E|=(Sbi3D+=WuKb#B~i*5^M6lc|hEuNT! zdby;0k(fVXTIt+jQ*Pgcq;T$!8GC>?P?LXhVo7ZVx2^N93jT}tyE{9sO;@;#Pmkw) zlzmlmw!C0fC_%yig4(Z|WgtmhBqn9>bZEG5y1jk=w$MEalNFIHkDJG#m|Pr@k(C8V z$5p!D4cXr>R9kIt5f2X|@xR4uTv*e!6$$^sZSkEawC?xkr~axciCGR-0RnzHHi=Qv zHQlco`-}=cf*EsDh)%5RNqVPlSH_p31Sg~d(%LwAYhy0Z7@{SJ ziNh+c)4GMf}d$#Y!kE2q8fWDg2R}&JD_S4U2Qb%!30m!K|9FFh_>}^&mxb z{{D>S#DW8=Ea!$p)6gIvc$2}XBUAIDFfnaHA%6U5e->F?@mRs}*SiSRrVP#`KJ23%n$cYXD<8rS<`X(s=sm6M%&N zTd)ZPu*))+Q=sJYZCQmUG|d0`g%A6`03q z^`82r?5>|>iOglq_NJpO^-gZ?^(UpmT(EmhInU@jCBy-htetCnQeOTgT@%2YpqYI= zXi29ibmE( z2eq0kXBY{7{KV+dZi3;H;<5k)B}+*>j#LO{4^mZ-kM(2id#B{d5N4-b9~zsih%a`l45Jp^R6ycglX zzZra4hf+?0L=%xu8HNNqC&{XR?3dMY7s4u(sqpVJv1B>03*$*Ok%UkB!%nmFAA;$v zr#GNxhm=1dhi6Ig4mB23(DfPEw8!O$8XfbOM>>#T^~7a0z?=~xMO0KinpSxtDz?~h zHhyeWjT`=@QlzNrCD7GIV9{`&Tv%Vk&Nq(o{b6N?!s`(xRR9##IvyS#_}Fsn=@ybG zD=%e=$K&@PbR7DEG3|IgF~q^~=zh}n_-|*jD4hb|hvTwcXw15Xc5Pgc!1C5dWd-3H z4@+EK02FV_WnW=Pn=l&07^vmUw>ZoNv)+>$>k>7oj58xG@kE~1Ejg8D5Q!q)sy8N} z|6zSi)I^()L9%n%&-LDmhC36jUe6x3p0%}v#G6T`Ri|>Tof)0)b(w*VVeJXI*|Ib( zm>?s`7>rttWy=sRLzDT&8bpHS>Oidvtjs^0_exNZ5^Zf8mOdlv&j;}izXs|+KV8P7O_iSe|yX81H9 zQR57jr5f&^t>vScg#*iF+f{c|NKMT(;(OwG2zB5l7in~p3LOs6J884@Y@K`lYWe-! z@aG(2NJqD9Ow!H0zva))x>tn*L-Y>s^=lU#J$-zHb5$)S3O6g(P7MN#|H-9)hm17Q zdLB$xn*0QC#Q2fv!aAr{jqUq3bJX5&==8zq>{);IbkMS0mvyLHyu0&hSVHe-E+ubT zqveKc#wbK-R%ZfDjYK&13STYGy(k|rIN-z6k&T<18^YQ($SK^neUDA_Nj?n6{P(SL zRc~HeT2~-)1Hhxxo1{Lg9sl)q-Q`U!FKXk*N7w5DJVnrJw+2IEK~*YFuEQl}w&h#f z64uYBAMu8UwTnnyozCkE{p5&psy=$x@+W$slJ2N(9aq{h^V{igu>XX7TPTJfyS=e9 z{gOk2UyMV9fa0mWz75X%gk2NDX#v`Ior1^aP{+a*W>e%!CLSef= zdvJZ}v;Xx%MS)`TH>yrBPZkXLnU4V_GwKgZhga&>R-SeLGKdR2oSn8YVBAC|uN}XB zK#;=-Ig&`lRS8`J`ovn>0x8rLMF*BziiBm!MqXFUYC4KBZ^4uC<&V!INrk2tUnXoz zru2-AX4kw*s5K8h?by;N{}`$m5ok(qjcv4dY7HR zPE=Q2UAU+&3LA~D3-w!nSW3i|>o~f%4fKsG-nMW+^b8HZW*tO<6U(69QD`fcN6x1V z7ffjb0qNqtdk#^)kU|$sI+&e72Y665WCZug#2Z~ShxtOuTA^I8U!!*!L{@FW$oIKH z$%|n>89&-d|F1;CJd}%zawP5+j9e%UBSAy>alo=KLj+E(Me89U`N5e0>>rY$RE#iz ztg;%h_ZG4=*rKkW`{>TZ#PE;Hr+3?8n`sc$C<4EE)KpoSmN4@Hbl*U=R8>5CB#V2r zG6bwA)d^Ry3{8j78Yn{-t7HQ&iP+@($5@`x#($eJx#U7h>;$SgoKI?kyP$&(s*unq zKqFx7gd{o|`*9O!*_gD@*ENEuH9lcDj#=sRg%P=MK~iXeVN+H*29*)}I+PxF z$GP&VC|b;|9ACMHJxj&ddGWIR@*EGhbq<=nD){;exi8d;$z?EH!tiD-A(X|&KY;`) zC)l~qSvPVv`r;u>*H74W)J~e2Ag0jBi93^b^~wep(7bpqsx`~p0eSu<3y-b3y}eZd zTzt2_gl8~BrvS;q)6;V(jSYa#1i?dzb)%N!tj5(r)MZPi3u;j)nUb@I?}g69)=G-A zI*ao!=`cK~rHQFfoAA}V>~Y+whJf#e4YnWc`HRf<+=2V8wT7m8b4j|j0##fJD&i2o)BQ@ zwcaewu>*{K5j&DK<*0Q7OEfmKbE}3cib(pbBnwVd?58Eo4jw)>v6Nd7|8~3&DMW{! zSTf}V5pRbbc1(|`+io@TvVke^ET+OFzisa>REy}=)Nws>%y9gZ6QTT&;^fZoSC>$U zq^QwqxhN9z&p?ZN2&%gybZWxeZ&q4#*#!PHN$r@@*Bb1(!G3#3@ae~ph1yPGI-DpX zjg6k==)zBTpKVmB^Tx!%u^tmc`&}C8rP{i3Xnb>Jo;3O{r}6LO(+@*XS$O&J^t`Ui zq0Gtk*dpS$)r|gP?(gra!<#gEOFlq=1n~Q`q1`$jW8rUY43?b6^J7`{sX}z^tmH(H#jr z9mcP-q3OlN#b!%F@ZHp{`1~xmf&1dhOA>-?6kU6|-InV*g z#C^^2(k`AXzCq=A8Hzw>Oq3n=as;02z#rGU1{4=(7Zj2z!Vergon4M6AQUT;3uSog zJ63P^{h+u1cKm|365KnR&A@3DT}byy@lqPW-r1wQLikftTog6}<=wob(&$hUVwD=) z80H-wMD*7=v+jv)i41KN?Qx1>Vi;1-C$;rpnV^xRDQ6m=&pO{`WN3m8aBXVna{gv! z5~m!}@Y_hQluIuh1*j;QgY=r)lPPFZ!B?_R>>CGNxq8*h-+p3Nws3fv1JM&SCnbq?Jd(c zlF4*?D=I1~Ry7OOHBn7h)lI`BmD9(9pPJC^EI5#l@-?PmJ_#K#Ubt#YMzzU6FxjN( zJJL~E=vSOS0O4_#60)L;#5m2zZxAxbCx<%y3ESy19YI%TpncDs87N`5;xC{3KnBY- zdTJq=Hqo-5a-GsD!6;j3oi04$1Y)T4p~@Y7Jw0NfhR9DjI#W`c97rf@)taQNOaB^1$H_}OU7u0*U*<(;6 zt!!;QQEugiRKe?L_iVY0v4T%5gY5_FX|L64NV9^&E{R%yp6GU6nh1B#RH;5+mOE)) zB0Y+?!aZ4%@-(<`W=^J_T@W`K2gF)$)j=a$1^Bja`l?KW@SSm;puOSA%ulOD@Qm2(z9w#yfbL zeo78z$ItVD8fV+P>fxI{PiS8b#tSkHp;Zx{PR5Or(N0zRkUkVqvmhip!OTaLNnjSc zVd8HY@Hx&#EY`ws03Kgz(ePli83-|%C^}VMQ6yNr>Odp1i{yjZ;-is-#Sq$xW7}*F zi$vW?^QGCOp@~f;fa`XfC+#6YwPQ+@u7lyUIr9Qr6?XaL8|*L}ld!h_rO)dvAEJ0F zU7oPl4xB{lJM&x63SD9|Zx~=-nTg_8ZcLp2DV+4@*e8(x_|>H62D23C{O`0c*9gwrWjv*wBUXT|a)5@*`x@nd^6 zP`&ayY^Sm-fZ5?sRqD8AC5Z#$LZ(X8vN!phf+H4g;JAntP-pseep~aBc%;qM3b~#% zY1eDDV}zu^oJpJSOv7T(O2||Fzys~X4A?d~8f3Y^R3-Y%H-%Rg;B^0rj!7s}-IalU zK53|?;LWcUIsB5aq(e)&jO>jt-*d8n4tL?#JUpL9tUHQ8 z1#^AyqE)Lzz04jl%|XTwahHrunWZbk5+&C?US_mn#`MA1v&-O zA8V)Im_#V_B~iuj9OLD_81vUFNN3oWOk+^!r_uJ{V_4PTSd1~^2$NIdlFH;N#g~d9 z{c-}Ljdy7^sNti&70a`JKMCV{Fq{`J3BF}ww^sS`$SCIueeDg}?^&jCi~b=#c+zwZ zH-WL$8|;F=9LpWb@=N|AJeF%L!_Q^A5ef|fHdLgQQn9xKzJ;~gvB+76%%)hoi;arO zAqCZHR}zU8NqaJEGQ*ZPy3-n^6fp!yw*_Y8h{zx&BGz0Dpj`A{BwX9}^T&@_`#R&F zC6wwwmDP_)h5dv4)(m#LEdUp>rB}?C`hq|k3HhS8NQkgW;7q!*y>;ij@7DB8LfGlJ zx>H~=S?IMrVUPpaQ{DHEViBUen8X*GF1=NbxTP? zOm3D!6sz}wXrd($2GqIUcx{YYX*q)DxGm1wl;7lVH1!hvbnAUJjj8#)xagrE1YE#6 z^9sP6V%qbv##pGEhm(CT6LH{3-}w4fbm3I8+lQtHKN_wQCfu#?B%Jld^aV5ytnP>2 z#jKkj1?HX{nQun>UBLjNqcpbXUAM8!MJ2KU=6_)>@ofCma_`I0QfQ@W@$eOsx2vpc z{2@!OY3?Y(OD6o-qEMrjvsA^d-9O0Kmg6}e^0bo43?t(?}w*(M-mVK1%6tu?YdkfImVfDy5#OVaMN0$ycj<;clN;2{O7DxlKUNCEk>eVsj9XMQPC9eF{v zXH6=>9hS&EpcY%ivQtEIk`eJ*IA#n64HVWP@KTzwu_VJ?DU{Hl{GhNI@4JiVC%KVYt4sOz;CKf6KH+2-Bx z(nvUAjvK$_(WZ$?Kss43&+C34O12R>DES7a*5Y* zESBh7cy8?(E6b!D(Yyx;tUAmx7Cm~lgkq=I5eVYZR#ii>{nzeSBbuT6ma08CiO@jB zsb&Gq=!+=nY-H?VMr!y+tSg|i{$>v*MEnrk$N8t8M@5kkp{-r@V)v27*{0$`Q%vqp zS5BrQN|j8}YHQMl-&&z}oX_xi5fXdaI)3pRiuZb0-x*1>;#z4BW(66QEFt?_YnmjG z-i8#=o*x{r?2-k*QS0*vGWPe4-YAg3ew0&XC7^X+7aP2e#avnMEVb)+eWU%wh)|z) zey`F;h1?!2I6~mhaTS0$+szUALBqmsq|;D7TEM@VymIWxhN~neQPWQJ3J+x?8eM}n z*c?7rUush-riVrf%hKSH&laO@AK4NXt7>DQAlju82DoU`UXMZf-&7}Yb{a<-CO!AY zf1%n5p)0@x1@&%oV96wnLrC+N*b|^#Cv_Tt;XY696_j@HW!S-!^6}jX;2lxQf`Kf< zqVE)Usp@|uj|gia4G|bJlX>5`fnZpTozQ+_Bu-Ipe47NJ23ly%WqNn1GmyO)DAF1&1vTW3O%sLy!3 zwx-=p7BMXi!={F%=B-TEzPk@N#EK$uQQ={NO_K>&6r{Ie!3)lcsh2i^9NQ6bevKC1 zej}xz_=PX{_coZ^*?-Vb`H8?w+oCE%pdwICFxG_Z_#geaauj0qjD6G~-SrK&4nLiH z5PYG&E>8x@@7k($B;r!JFB^K9C3U4tNcmfOBqqwGh3B|Y-4jyKfmUg~0yDAYcM-H* zsFOKGrTzNRZzF{O6>PR7YZ;_8$*p!|@KgdNT-G$avp|COek(oCk7Lw4ZN{Zw$DSn6lg?H%L8XW!gZtMVzv9fm%StCW94+*@1x8ICtnh z?7Rr45CfdwDhz^C>`o~IiVUCd%p^GQdB}`Q^e*N8710Q@cM`b_M~b>N8Oo^^fewK^ zpNKrwF4<~9KX>@ajT;NNfi%cj{u&w>xGCDw9nt6PnR0)Y?7CFydN)bG|R|;?dCF2sqqqBJ-w+K z0`VZKN2K6J|CXTPS6t6;J}vS6b4iJY-y;jS)z0)s)4a|9fXdx?Dx^BDH@kQi4NV!G ziG0VNbU2a~WOe1$if#_wUjAh2UvHCcZ(FW>t}pkK&=WmZ*PI-6QbMx%TQ+WYJq)Fb z%)YVcOav2kC0k>#89A(X8UF#+pl1JY%xr7rA{>&6+e+y{W(%EksL8?=XW&MrB9xkJ zLU?vr{s-o=jWW!aA$p$aM_M6BRu15g)ggakLT0dQGGxc`)^Hg5o?oXMPxu4zv(wi< zxsT^(%K5{%7h*tnE_p*vq54YjBi+W;lD`KP7lR@W`vS`$^kb1jM6+ouxvsv?TA5bc zhfyKS^dVX;p`br(L>BiXasF|=Y&<+~-ZiD#`t>_?oWZ9%Ql@S~aDeEDw>8Y6<8PdG zMY*O-NvzJwM+N;*ni7fabLIRAlDL~E@YelSz>eEu)8S#qT0KW=}+%0p><{8XB|*2M*<-x6JzpN3=wa5M=lKVi9B zXeGbG?~=jK1(8%t$WkH85-Gf;juEfI&`3SSd}kU&i8=V|NAr8z62cj|w3W<-Ue99) z{pUP7f+B?fH>cQ{an|R^sTdah@2<3? zax8+A^P7Z%yMVW_;8;#(Fl@dicjc^DT3EC%+c(|6U+4J{QWWZXh+d+0YNB6l+J8b= z8v0jLRxj}n*tx{d4V#zIqj>~xfAWo=lM&#G!AJ@FQ>!YfQ+Hl2p0{~o32tMLk&2nr9i{P&p zz!Z&UPm`VMg2l%Dnfh}5(&%<->Tq{(n#+X~@+%83Lw$amaaJ_7|472eMw=L_YRU`i zOWQyn?hgICYhOA7xRi5@HF^uZbMX;sgP1xMo+f=R~G!iT%` z3f3CVn&0a1uN@;3DzZ~-fRiJm#k!{H_ix9q(X(Z~Z5&WF;41$#@U_lWl(w&+c4C$q zCrbSr3uQ1@eW2Yw0nao%p-(C^3kjhy9J-&yS{a>U8m_=(B_0^LWBaGx)`y(zR(U;i z3Q$DyX|a0CKf^=xvFxxD`a--r_djCNmuQD2mLma;fJW%XJE#(P%-@8+D}D(URSle> zE#VS>2H?Eu+lVS_();{`n}96i3|Fi0a@f5Oa|`)R4smSG(!Qr3uC~tJBe?IxONCd90xO@QkUE5LBT_SpA?Vs7hMCqJgmEWgl$JnoRZ1# zoAZ2Cs9n9#4p@e6J2KsX!iuZ|cylPkU|dC}^w7vRL6}$7pdW6!J*VKl4mGBhYIy5M zVduz_64Itkly3BM&Y@w6D&mp+x9(0EPA{e24zla~Xv>f*JtXCBWs{@ZuAUY_u_ z!+=IanjlLiy`pOfya%S=UVeXgF{9+aKi!&s+O7q7COX5z(#D-p(l9|O=^=fWO38Ex z5Ow&cmQRfDL4mc=hnXTQqNl$Dk^lEPN!a`I_wQf3bc(eFlVh0^jgv@1?M4>!;e^>@ zW3FT|8gN^G+0t0F^#w9mQ(rmg9MUBYOmE-F^Ntv7gdnv1=(ZjllsClq4fiN1(ubq_x|3oXHR3%`>KtFF}9a1I! z+=L>ASDj)=P+JM>615dSX4XQ{<7jByt8)uW%|xB8x~ee_F` zm7iCC!gQjB;z_QvF=Y^p=<7gvZ1k%~(YVUEBx8E0myWJIA-xXK`WlAB)hEEiBaJt4 zaZz1UZ(}%Rw$Z=j11l0c)Ob9XjU-LS2GcqqLTLbzTqTnwT0e7<2Yz$Rd~3F#h!T|K z11#WVutoH~Ua$d}<0qc}kh#wFmNmTCX@l4h-GQsDj*p6ykNdoySUM!6YQM@vr-Abx z7!5!YFnoF@MfN*@^H{+;lu8qjChE_!&XNg2I@%>CrQqNppwn$_ji4g~z-DE1c$Cfa zT|%8@X1=gGnUQq@1$1~b%v11V$4Jr|Dd24Kz0M2HFY&0iim1_#qUj}qySb1}?GvQdqN zdchv%@KcLfUlp&S9DL!(*nP>$WYV>?3|!AD)Qww-T)_)jL`K*iJV*0RWW)+eOrFoi9H^h&rxz6a$h9rc<{%v$ba`6|AXOz z&EUgI6v`mRrq-0{ogrZ=hkZUhN?Z<)u7zCHFm;0VYb577qp6T$Tt}B0%Rj~(6jMV} z#_ef%O%mb2w=DRMFzm8Kr7l5wYGmj>2#L&AK;J7dCd8OgvTu8+O@swE-B&UN|#dXf9haqF^`R{?d* zlhhAo7bq`5quwh3#GziIpNIaa(^*1@mMf!ep*D;&kw!VlAjh5tX;wKd3yx{RvQ7bH zn9_&L9aUR7_UESQ!rbcWF8?gOtoxFNup>h-y@Fy1kgN?V-m($^%0Oga*}Ld%h7f7& zC@jQN1@YARRXbmI(n_i!91Ho2+y3!ek{=owtGcYc;rLL+5K}ul$+Q-=n6o&PHH7C> z(oAgZx?1!NvLPkDfkZz?1JG9kGGpw|3YJ)IE4liLf?YI@v4Us^&KH)0xc+(m z8VQ#wjK^5^7Q{7x#4&`9#`IjFk9b1d1MKSm7VrxN->vtBut4H0aiQWdKA`HQY4p-N zk4r**C_SAf&F2p$fB*}(tPV|5rA1C7xYpG94_#clFIv`$UiKqX10N61m*l0+%O@ZQ zK%69cCq#>?`}?M5JelCf>is@S?tmqh8K)UyJ(-yoj~C*Gu47O(*Yb9(pOaFxDzB&; zso&mo)_^69vG+Il6RhaMQb%jqXe7nA-D=lEjbw10kXmO1asPha*W~k&B_$cY&$lIdvTC7c_+pBSL^X1{7Cu}u5;w4O=c>=QO1_znFfV4LZ*v24B0kVqb=E48? zAD4YVumrm5=|k|;pL@}0m16p~K>W6J985XMZn{j3FFOFbU^1O+C_**u!>cNRk^ODk;`PYTzskqwJaR%TeW zvt#xZUN8YoBbB8cU2v&v9OHE zK`m?hjS{NA0*Q)4YL$Iyn4nAa{a(te|Mp7Za(_1)Ol$x!^iJ~8R z?$u>q!(genOPQ>`iN2K0t!6m_CZmzwl|&d31heC%{=XI=dOhc~>_zEC+eJafCWC{K z8&EksetL;JvTTIH(T#yBUpvz>99tO|8w20O7AP5l$zWFy%hNNn^F(XVF8v!BkcuFnsJjd-)4XMV89krTO zxUm_}II$+3HXWLShjx)0yBufS^`AE7dPjTg#|W(<&96dL`UoN5MvW%zEkB{Iw4y0T z3Zc{L0TxLm4&~4u9{hypku4{h?y`v8I$C`LKftmv6k9cf`7o*;H`)etM%;wkl`B&u z*r@J?N`B2B;&!@bNBraD=x~HYr(>prba!BG#8})kas&0JxNW*n{Ro77g)Tl1cMT&^ zZWaC5JS*aIHF%;i1aVDl{nS2?DmOKOw7YMG>#Ya!OL35#1Qc>$Xhx!h(QvHf~UFF2jtdRnT~cnVX$m z*5gOCjava%-?V=&puLk_PfjMYbVB%kum2cG@s=>kCjiV2`cDH%W2{BDy#r27!lM3X z1rD~23aB4{j5f;QM2HiSd_$1uu%Yu(_`OaqnS}Y6EVyIrwE+(hy(l(`6UmGI zAvv>y!|Aq{&Sc+3}PdR9qW1g`hvRVrf?abdoSAqgY zh+#T;nzW`w{XD@gT1-3nPSlfPAp&mNldJktqI8#`aFi&yL8n^>W~}Y(EgR#PXo zG?tLA25a9P{>Jq;1SlL{(VPk?Eui&o&sS;=2MPBu1?^^56EPewZuxRO(wq;7xG1Zz z*?334jg*+w!&3)E6_rP=y*h+WuY9%zaho%DNj-`)i|&PU1Dgynb}1*_8lR6V=HwAx zU3Z~2d=a*kTON`lK6S#jnb-anuk3}KNMLT-%}W-*k}6eJ4SZ5J?YT8tH#m4^be{%* zwP)xL)!E*!^E4O8Xhk={lP9K97P}csXGUYS;&^A=omN9fMErcCK%;^(@g=sV7N1qM%>+`0#-L z>eV8#apIp(QK4Z8TRDK5^k2NSZX7g`Q^@&&cPM+r8i>Pv>&jAjoY7xJOQ|J(8^cyW zy^uySa_oS2`j-@o-HKg3Q(7m_rwa@pbN5%NlKP?V3Jf*yh*)?3dT+aT6JrFw?-E=u z<>OibwgNe+Nj&OKXB$gw8iXad42<2f5&LLxbz@CRw|Q z*;AD!Y*R21utFwBE8(w$Q7f{Y-8|xLG8B{h1Bx;w;CGE{UDP62nXvfsupi5wL;dgih6$fF7|rbPiVUiDg#VtU&U zZIUhTUH>!yHBvConDYM~Pm5|q0mM92WLE_#p31k*L)L5)3hG6&x+usx`85lsBfq?+ zUg{MT5dObrht?3)SG4QFB+(h&+Oh+ZksG}QGxo%<&G#L9F-U7`GAF+=b*hz28FGq} zq9K?v=pp@S$yD#m9I<}H>9Qdm5|ytPihX$J$Am`N+e=nV%~s1HxNeBJ6Eh@8!bX51 ztcl*+h04z4=R4(mY&sShU0eAa{w8t*V1+ceiBxQ8V?o_}PleOLwuG|F+wIB4BzNE_ z1eLyYG4)II29_CBII2`=&`<_g8YwvTNEP^7$FxuwcDy!;(69OxG}UfDTRLB#nN5Ay zDrWQiNSqcPZ>X*CO)C@(;e^0Okh5)n4VB`~oK|F-2Xsim%362GUuy+Es&Ejlv6Rt- z65jbmDSYsj)*t_uDV>R(ep&prV3`1}qOS+yd6rDg_C0sR94$g0J@q(qX*ch5)G{py zUyzG)vRVd<{^E6Keg|nBTd9KBKk?Lz2kG@?`#K;G*HGy1EDjxebRM2Cw?q?|TQ?$J2Z2>>i+P$%^ebR>^84{A>)9N^n*u!7mf|z>=mA%js8j z(zOj#n$~pn>(YmDz!6!P7tPS3%gWuGf7Dc6ZT{9_$YgwhOoGtM39aO6Xni#eLP0TW zAP7&!gZpQIP2kJH?~Q_uOg!RK@A~IyW`2g>5ZVG$`v?9m)RQQn&4U|=gD`H_8^e5s ze;ZFr+8RwCEkjey8pE2jW#BTNGZ5RQ&mW;n1DXXCgIh=;FfrOJeAM;oR?;v!opxAY z^bJXz^Xl&=T~?*!?fi|*rL!l;dMGED>`Tr2$AB*r5S@Kx)Dl-3bE%I0Z4qUo$Ww;6 zb5ZH6Q(%v77o;RM_R5T&>{PW5{Z&R&4lhe|>$eIT8Si0e(?9wNp$U@x9>Y@Ref>I) zk9ny2+y}CS#Anw*#514$C5*@{zXS}b^w!&_v35i2A=qZc?sXoer~@=eAp{wrh`YmnUU}rY`89I&Eo_Rg)AH)dxQXv&X`PMmop&x zt*P-QW9>6&%^9J2`JYu-lNREFBUEhk;cXOAvGKYJ^j~FZ|=}TFRcJ+A6?nKzaR%GccdSd=k}Ih8ud?`lhhrL>;~}2JKRYS+MPquh*=3bF z0Sp7Hx>Zj8N@&m?N%kYbOlS!TyieJz;ItzFgLPHpj~a7eSj-vf#Sn-gSu4#R4n>-h zQBb-xCgtfW&>CfRs|1+Ky~yQ5l)DcHeUxB4Q(l>N{_MKdS{cTh5#*chyKbuPZ%BK8 zqFfC`w~whUzwL}Hup*P&T*v80Q`fU*BgbjONU7a#$x(iJz!~r(86r z{(&}^(2W+UGLiL`_%nP5t8Ncbn9Bu7)PK$ap040$*)&_=$k+cAug1;Z$L}E2Gtf|Z zLH?CJ_BZ~-(02b7sB!%Pej-9JCF6A^q#uE05i#WHhswl#1XA2r>HyiwI2f?x=)WrDQtcqY96opGPxOwvkTG%EIn;kpYqTU zLdNpEi{CnTa}W*3Vp&*fz3o6(ASGp{k6R|dvY!lgFZMqVE?Lp#eH(UMl4La;hNDfO zI@t+Jt>Ge2j0EfC?@5C;DinRNjx4}mFL6T{+aFR~W(v1hqw;)Z?woqm`1akq7Q8Zg zDo%h41LI}CPBiF>ze0DqgG%uZZhy~9;_3R$8A`2DWsLxlPgo_4Im0Y+?8HPry3af> zJCCsmd+n3ITq|mWlkyIKx7GL5rQiV;g&5O#6GT5?i8k3v4~=)&DmD~-pjlfV+PFMC z_!mWwbLzXjYI@s0Ro_r*f!h2b*cN>yA+%jVBL%Ra4-RMv%uJgn$YKxcYSS!KJUl#HT#}uRjb^J&N-z+7QO%qg86kHP zR(YjpPp5d3Sce9J!(mBf&?TvpM1p%m*C0(PkS1F5X(RK+3lwWl+{CTXhpwS|O!X;& zgsixD@Su@P=c}PU)}JF6qG%{d@(cI^;(7t!`&HtDL| zR-s~(*>79oGiwwIi(WAL0jqP{MlYI0+=BccEa###V^enQU&eU517}`XECeyX>wbhN3OV3Yn&VOnYfC| zxO2iP$b19?an2ca?9yMxZ1~>o?-@Hv8>Ry2`+k_#(+{FQeYX<)-bt13saUd%3B(@7 zv(l6p*Ka9Uk6=Swo7-viAfpqOWT2(ZyN9~L8muBfj(%(2`JbYeSUSZ^ZwV!RM z)%r*Uvy&KK0=p4N9rme{AtOF`P5LWl1o}|R>NiC4^)e$XG3x*=BdJMFViCmA+<4H^ z6_llnv1Kw|3@pvtHXGIA(fNeHY1@x{&?*)7p#AE>)~mFl;_kqm(YQj=XLFeq8>~ED zBDofZN~Jwa=S|UZyZk8wAp{c6+sbl2R^Qk(*wabW^F+MeS1<=xpc)ypM~Klf)^=xJ z;-2x6AC$8BJwN~{uu%eV@y(l0L9lX3V%P?I<=>UkPzY7&F!FFMR2jsec$bHOX$%5I zh{1d-g?R2a27W=jshGWsF!|5E+rfM8uO28IDO*G9;M5K)CVid2k}XEW({%&(>{4kdyzx1N(}X=TN!7ZrxTXXiABs&S)nui zRDu_6o3$!a7)IyjNqVG7;GaO`VR#5cIE&N=Rcr$=tMcs@n$ckF3pERp`r-9ra_El= z%?RxHvU$x7idKkWKxm|=rBz?uI2eXe0PF@KYv0Z}vyrHYVOXiYC z0T()GS9o#rO^1hE~qv@`&@s*dw1tFb+ z{@W~!l#K!vNvrmoLLdRTy$+wFbo?^;FYemRAJqm?x%1;w7L<@I4_VpS)JCe8Zy(fa ztkL)>(}~L9#O6#=P4Eq^C>YHseq1(dR=V?({ITOk32(=iI6ItSh8I97q3_!?8-FuY zD%;I$e=@^N;wx3L^>^1gjb^>X#_k1K)jqxr?~d9~-%MdVqz{o(#!4y0WD=ztuz|p{ z%I{%{Hd~egLnZz>71CkOGPqm!+lWGm)Ax)G8~yZB6$T|NuXH4O`v+7BFGLBvDHG62 z0A?5gmoy;3l-+zv0$+k7F=Z6x`5+WIOJy{ud)|_-(Qsdo0L0 zJ-M0PZy>A;Qu!M(K|6%EKOB>3P+!~v8;}LAspSv8Lt`tlcIXodOb%q^EI*ugNgC-D zIM}<+7xhfoY<%I2Z9sdMf?0|phUt|Hy$>^BjY4Qw(bm>RKzpVq%JQD^Qe;d3 ziU7}O;|OTunmEZ|K3B_8W9BzG2D)2ZKcD5s0hIB+Gr+PpXaaW>u(1MtBSEBd=%2Ns zbwUOG;T0mGPDUhKGU9N4J*L!t6P>GR#JY}_mRr)7Gt%2hVC@298{km39FCKLzWm$5 z@eHjYXilLgX6M*R4I(#0&=KcpnzOkbz$H@i*aV5XrePoD?!}!o=dw9R`uJbqZ>(P; z;`$l8@w+&??J`l8$+DRX8OA{S<@@jV1JV=5Y>TuY>=bB-%9HppK!Fz+=Sba81YBBF zMv2>J?wlD{?ujlvVVWrbu3;?t?ne(1NItOU)S^!&Pk`$~{6#q&?d}PU>0OW;qPLB! z=ER$oV$wRh4{)_K7!e@i^poG8#Z&8cw|zIlwXdxVOiUz#-nX}B+W=lE=mjR%$uc_5 z(-PI*JMSHC!IskBN`!LKx;>2g1o#$Kjgh%wz*Ra4z#rgAm=B+DZbQ!-Y!1Nl*#$Ph zLDQ;y@2`MyEjtZFM(viORHDV&I|UPQS?Z+w_e+qpgjX;8w;Q{_y%f{=^NeH&*mF*0 z=BzhQ!MU@T3Mk=qZEM!h@5ui&=JwbcA5f!9bbUVOE|D?s|L{UalbafHH&DM~+dL>8 zs|%gvHqr55PD>)LQWN^Pf7mV$Tf-~sZ> zJUdCE1{L5>hKj-4UIRQAfNO)hH$M#{Bh2>vR|y8Jjb{WnQqim{86RWy zKKukX4DjYP(6*oJP!#9@UKo8i%=W@88n7S1aGwRz1#jvchla=VsBvMYlSX_UEtOM3 zQJ4lsJbqh($-Kl5dLHqtl)i5xQ%nVLD}$M8jG_(ffGbjA)5Yi!7`j^lG!k^g!<`!d zqqhuv`zefC#u{D4N*Z3Ly)*r&Lf4B%+q!Xrx3WyFP<@neasZyj z7J4ES@O+2Vi ztiqZ<>WD!0NlfFlp!4lC<+f-bORVnpN{?Rvr~oV*!Bt6_`RXm)jlZlP$muDbox+;- zs0GO#81O{CAOk0Z#g?+)ojD%9amszJxe7+gH^;gu)EP2EC(i5VNhHXx@4I{-8S9D_Ue(MDQT0Gu$oWT7_G zGOoob20uT2O5)Hd67?&)2@aC^O-wh-eqO`OXP0lC^?U?4$N`H8(Am5X zwAR_q@5RDU^8WN0iJ(7>m$<{)3U4k$UiKq$8gEZYz>`u~Wt(|NQ$v*?e(Ymb+x%Cg z2Lc^&Rxe8OMq0wBxsUts(-x^wCxiq+}Zi!)j(dm;6B8YVP%w+ud5frrC- zfGEa@@V)_9O@QfZdqwa8Fn$XF4-Vk!0ZwU|-}CMEYiv$o`Rm>RNPe@Z0+$N)ODi27 z7l39abUz)}b&+)f*t#XxPXA9_EnYqXG*&y1y0v~g#1;R(6ELuxb^ZF0xgG(DvM1-z zG@AAL6&(d&tl8Y~fY;gmudEz}g#Y%RT6a3@wXBs@7@#j!<~*#hPx50%4FU$=Q!HRp z){Qb1EUer2BafS2G%efqzxV=Jf$I%ML(v%F4tR^`(U4bBIK0<;L+ zexSv`(t>Ss9vQgW0T={V)6}LN`C|47e?UCgH;L!)tgS{`m{Oq+k;k}vfG&H7w;@L1 zI2GE9zbLL>QmO)C#R6)x1 zC}~9G$pVCXzWwnhr$zWOT})m%uUXiLzDzwAH!s+5Svh}S64(7Z=gV@c!tcP!aGz=N z@IZ6)NZR1cU}4DMc)K4wKQrUfc-XzVuE#2F;znC@T&~9eXw>XPz-!m1n$$1YSnyic zYh3fD?c|`VsyN&7Ch$Lk2~*dtA(Yu?p{Gj#=v=$}&egk_#M)#siW4&hyv7qe4qL&m zv-3M6(g$@0oQJt$LG1rzdAqy20wj|F{1#b%6@>Zeu5x`&xX?Kx$ z{7JB??o46*{^U8d2p}l-fD}gp7t)m@wZhAXxRh3C2z@fJW}gNC;~ z0A%D9y$fI{KHzL)Tz3TYgx8Pt<(&CNbU+x}UiGq@f`X!&&+{@;g&q(e-BuiBL0o;# z1Txpk=}+*hU@_1dRVbCdV&^)*a`b(<@$&;d<5vt@t@>j|_g%)|phQF2R{QTy(XP^n zZ#2mawJK)y36%o``EjBo3Z}K0K&CG^-qWkl=kZ$J9GiYZG!>bF?#Dw3t7Y4Ej$w(# zLBoU{`6Y+=rpQu2I#n-iMg|!P1%H}J`0)1I zw{NA;4Sv<=@ejX2YXHP7VKV*w+iLY%Nc3o!N8gvzQx+ozk~qJ+QN1qks(~$TzCt7D zcoxi2`e5@kD+RgXvO~(bBgOE}hZ0u72t79hgpmBCNiKo@=EAU)nHc1jU%nn>T zFDlgw-FM@5zPJ6Pf(}~)I&P~Wi=x|C!VEae4HmQ1L(f)@bty8PRS%&F%eL({;Ay{H zWPABOPPpk_JzhkJZ`~hMzs#g|IWOBZckVFSwE}uw`g)!1YTGLxSyu0U<`st1{3xfL z{Et~rkCs0}ivOv~%^;{7Y!O}8jnz+2>n~4$B(QcG$^``(0C;yffysdP>jxMvpD8io z(P&dgH|_D7#49Wm|00Jt#@spWP8Z=G@4GLf^D?eOISWR(TG zEgB=*sfn0Jfk33f*nx#hNT!y_7J23<7r&>`Ha%Le94X2vEmittz&O;`TXVbeg4uA<5OaM|9nLh`_VAX!Zd}|`O zLaeOla8M-Pry@k5mm3i@+23+aZR2lLc z!-wmmTJ>l_@v*?M@$oEx+bElH;p_jL73Mp99bNK`smZ6BQk^moFBbdllh3^xl3H^Z zOVri(J?Bpo%DT>xuNP1nPuKNOi*kCTN3ETErp-hX_eJ*vB(J%{j|<#PZm zFAQK^ON+DfAHybh+yACpRki&q`}y-Hut777oVh=qJRhF`%KrLUtef|~tR7gH3d43_ zA$46GyLEe+o124?D5zAsUQvE46TpSeWxLYiusxJ9UzHU*{37c{`_>o;`FJ~RS9v8A zIZf*)2e z55b!???Ig>RtzTHMl-316UJp<3nS1dkf!g7+`Nc^mS)ZJ*kt+56N7wU_0Bp5JW#ae zi7PRsrlzC__togJC-Z4d#qv zeSLjk!~B6~bVxEJiPwI-U#zC*z8Ml-3pm1|PT4<=uTR!r028MZ;C^oId_PQ9cbt!C zR)!jp-a^><9BBESC*|-1umURoY0oQpbPb%L&0q&?X9gT?wH$D=zoG|wUVH&Jv+ZhL z3-|!Y6Q;Vdt_~KI4YoikJGfEu8!xZ^M}#n+n_93^FOdAZ)^Vq(4LPx@g$>J;vcf`nz?)ivcig9lhMMeQc`bTXc#ZWR-P3iAz?|AmhhG&Nr;cQy1uR7S#nRMFSJx3qf-i9rm5`PJ3cVVmx4M8Lw^K&$^Z#0 zc4%m5Rm&C^-UFHuMYO}vQ1?aF@WIu(H{h*~0(dE1f|T`Q4a5@JkUA|^+vE8tV80`t z5wR);e;opJTOse~i=3BN;5aCXnjdCPd~ez_FJ9T(Z+I^q9xhpc z`{6VI43Tm^f3Bx;FC~eCU3eUMUk$YAV9H*=JbDfgQ@rzWf`E+<_VsGtt8xN0muU&y z0LpieI}v_(MEvfj8|fdc_D)E_p{Qlsy7T(cRka`Uo?UUIN%T(^6%_$yi8Uu;?FWI& zTRIKSL7{mD?#fP4OXlWZ+L4BRd8DG{Pi@xyxnk_|(q3IEmX6qA-zp%CRBNy1ip-4W zk)ts8DmWttAusT>WNX#6)zyWuM?&F(xUeKw5B7g01kz7zZ*Cfx+W?LL$Z?^SSx;kP z;j5WrZoq&4rQ)^V0?O01t17qB=bzhWP>^pw3${kqvJS2Ft7q2^;~VMSjjVzbc*&hV zHU=$~0440TqXz6!##a(CIJxMs-c4X)M^<&5%M2|}bJ7I>A}_#+djFvyy{qyEWk3^g zhR9o9t_e`AvJOdp{D^O~pzSoy?0Xi~ABGwQnf1z|f1ey_8DuBPLGW~*+U*Sp-6TT3 z9pwj^i9398B%J^a3gnoyJgh?~MBJyTes`&_2RC_g7K}W zRk3*c6C+GTGSg!(6{?kSY8xotK+0aqa{hr^0XxpYA30OHLYO6JbQo$7GEapqp7oyn zvQFF5Y?7d|+qYZq1!tv=Iv;W;JUT7n26AM`Xx(L1c|Q_T8h$b_?;R!o^uZ$QrUl($ zp?z762S_x?-~x2%hkxbOpfKe!LK5lk4WiNYP5S}iFtj$Z_!(boc=5pX_;E6eZ@t~u z=j?nmsonKRh7#j7K7fs%A4JCj4_l}#AYw%6J(8~hu{9uKyDsRs*?_&$-MvMDmLJd- z0S~8j^zAFZ6g0Sg2hF}#SO0!prh;Ev1$`~QfI#PK?M4U~8T10@I;3|}qiWByLQl$q zIRKTU2Xr5{-A~sM5fLDFf-7fo9l%}zXuo?O0HQn2hQ=Bf1ljq#b~|st#30GAw~4^?Hi!h8 z{)-khxEHQh{)=qpa7ISRis=3Uds$Ib-lPJdSj;BTAL4u9tp4G>%OK7h2|O0e;fd67 z)1Y2`4uTI0foVO2fjF_rH}f!@rV~74oLAaHRFS!j(1Yl~4qqCyt=`s`X;${hmojg6 zgZ5}>5#XPFK;muAWV7SW2mrahs6QuOMR$%Z`mwvdHHwOUI(Z9w&d&b!V7~GJ#1JSj zr5e1anjn3IIPL)ySTo)%zy(N3O455iPksG5qy(D20$}g24hDMyS+7AD0&9ZQ3Ygmb zt{nvI_YRkXFK2eods!#ebbsRIs6hjkvj#p0!5lk)js<%-B8ttl`6Z~D)?aDyVI-b= zi>q!gPGmI}Pxr5=tJmlX7nJSSxt`GdUcqm*MnFeDJ39mLTTqrIH`I55Gb45FcBt|v zC~LgvuU4EULC)xFn8RgN_tSE>1hcKI)a$Jg*gIihVF9(x=Y5~ayf2%wNny98CZ6A8 zFmd~{)X`3p@VEY-Q!%o%Mo2v8UCn9=!K@q{DwSVq-{#YhZaTJ)7KnBChZJh{u|$3O z-E5ardm>h*2RfJ&%#jLsUV`1%I2u6kYHMo&68|LU#p7(rOv@gLhFVhh zuX?VtD)Yad6qKt(@OGWHhL=4DmAIeBanYCcHjcuYMw!@Zih<5${1kL54+N zCcVZ7tIVAb{`_fDDSL15-g*qo$%#-X0w9pZ83=&LrX=BL$OHUm9F61Dbe!K>Ad-^| zO^Om@LNBn!T(|?z!(BQNFu?!IpRSVqvE9rgB&1ip@KI7SQyFtOgf0@2&vpo}Wpi9p z0gNSA`^3b=Y@NRkXp;-DJ_$jbTpukXdy`V}qs!7qLcY8!yc}G>gCQZomu^2+>-FR$ zFXkbDgX-I~@6=Vpf{yVfGh>Ou#FJ`MDOWZ8sCA>m$jZm(1%g+Ixa~IpMjQkqJ5B@6 zCV)5dK3IDgysJ=JY&>o^Lk2xa-#!!2rZDu#tR~qMQ)8(f%dG`l;M%BtXmIWOjN_ zMq-);XLzfzp&@uA@V5N;R(F;)k5;+vF-^5xz2H*=MY><#UVd;jmvcP0&tW8Bg!L%O z_cr}iCOCvsGZIW4bCF8My9ym7!a$HF_g#VZ*1JoAc1~$_Y*RQNv3Wal5hV!8w!(56 z%}toM!M}F1tpnY+4+NsPSHIjN|7BEJkC}p@kuXYk7=P1G^lw5v+Ot*uA>ey~HWMyF zhzem)?dMs1>WmPQB&JTIo8wg=awfR!cjteu&!Ljfe*E>K>x%Dn)BX{Zn;!%(USk&k zzdV1lMrKek+=FEZtulkq;E!YL>$mOh#IAKG1>iUno^c65R2@(VmtjtVRq&C`qxfhr zu&NGMsug6Nm~+r4x(oypaeL}kt5Ljj(_-Txj`{6ZUnQ$q@}?*gq^1HL<=Jq)fs5#b zI3W20up}UvrHPp7vg07n={h*)F$fw(e@$I&iTZWm5>l;>w&lY1&VXg;30bPhY(1v>(W63GhHubiWt@%JVZC-^2EL=+?W%J*d(669h%3m6=X zij;VKy1nJ@3_Vcz(L}mOw8YEv^Y@-E+9@X4C%ChmJzX-Bm_e?Lal_1vII3mk(#oWy z_I$+*>8uHB)M-@d#Q0inkVq2Po69h|5vt|Gz>0Va&z)21Z$P=s{sXI*3joH**mq}i zqFvTKWmwcP6-DIGJUVsh6YM9Dze?4BSY!MNoA?SO ziUgH2iB7{rIuXSzC0 zsyqddT?@`A@SUYk2KOqHyUNLrU+72Xe@gScr*J>6V+iuz2A0Ii{6tDB4EVX~jQX4T zhiOETT2=DC`*myX)WTR<%Lm3d< zOTb5}BH#Y|&R)AGj|jQfR3}eBcm_IO?t7Habn!c&8Ga$bbQ7c*aD6=*fZ2ICH6Q@} zWzI{|@0OXSls7*DY~Wm-`=1<(FppOC*k!kpu*D@OYXau_JcSY+`YU5H z7EHLyBB6RC_W)ix8*2g~h)2vHOz|$@3dH;qhMQpYWUTQy@3=}zggxJ1#MKtptXHj8dbGP5SPp`SUo7Fx^T44NvGG6Uoca zQ1s04PL;yL6t%UljXgrOb!t;HY> zFNLg?)!WW=;d97lS}nd@Z5E}pPxR3b zh(ZdmVyhS0BTGrk7ns*-M7sYL%u&`>Y+f%hif4Ti%~SB-Palf#kV!dOGG7j8!7T%0 zcrST1gz-c&4)}YgJOyr_dyXS-g;}--K}~-{zEn2gFeXUJz}uCgWaH$7p_i*~*7obQ zaiFdzSo{V~lf<8e*h6J0fYV<*#A%2)iu_dxhj1K>h)dH{z5jk7 zlo?yi7{i2%h=Px@$?CevJPc6&$``K8l_9O>D?IvVFb3WsiR0Nn3~YRBB#$aZWVM)P za4iiq>W``dK4p#*?>lc_TiYrHTI-3OrYu1l7x!L5(M&VR8PRqEiO`&A*57yc$Nh(E zh0r~O3@T`~28wh9y5?~;wYWGR@e$MR{?iWW?v?^{@B zWmq{mCEr7v);wp$ak&HAe0Llh^Lw?bIlncXX4}=~Nxsi7Mc`HnXLSBrWkfyrUDXfw zt#6Z|Y6z7;mwxbfYbt8=eB^!4?vt&u$OIwyBY0k8)OrYn$QlUVIAk^| zGNrydHELn7h5e+2A%$l=0FFbbMm7Qs2P4-~C7eB9PA^Y%^P)FPAwxV_@=@C>D7Vr# zqrBn+k=XB@yVywL?0}w7sg$1>!Dg{WWb&ZddWk%d9XxA}iF{l&u#fXhFb2>DAnIP1 zb&aR&d94L9i6JqM{r)!SKtf1Z;P4CrxJY&Chw`!0oB&fWp~2qQPL%!hz2s#@eXdvr z>9`r&DP8Zyd66?8wBo5(nh-gS?6+8Bs;iW)J4#5ZuqOL2%XMBVGnX9lCbFi9O(L zK<~1>+28GUy#zDVN?~r~WH$r(^ID%}@mECBkcYi}{N84o+>q&e)8jX0btEFSIN`lb;E@x)et7X{)(*MhGVxX0eC z!ECUwD9Sapie}&KFnNzPZ=Ps&>=na~g-{9l4?vFe_3c`<0uVtrF!z9k4CvNu{CBW) zsD~A0SM>$NH2pfh0Dy%ahLGZ$x-V?(|MoLK ziCwCHu>(v!+O{@*GM2=dXD8<^kGVgW=4XloDKa#cN4WdbKeWGq06Q0VfCqra<5OK% zjSV3~Fq36>p(MAs3$QB{Xd5W$FG9^bSf8`70fH5PW<4go)@IN3UK2=O=j|i4lj7uj z)t%SCxjnFb209G2t1a+aUxyv5V9p_6wcsizi#KY3mW6`};%%K}FIEyKPmsbix)GH^ zdwIGkvz|vTFJD&bOx90pu$#%?k@=S8v)Ue3oIY7E8kmXCw+@)f!GW2WnK%BJ0 zrB?1&<9hU!LCabCTTc1Gm2%$HMSnjoGsE5){{ftgCRC6J7)+~gquWn>nts4=%4zy( z!!w&EM^nQ(OEHzkrL-d`Em*~@_0F!rQk1bd_2X2zn#PGgA#$QV-2Ar>u}3%%vP#rR zXAe&(TpWFKYWUd_1&Y-RAkhp+fq)9v^xy>OzMfjs_C3x*k(;|8&q=fVp4UNm`Z8td zHpSk2(?gT7<43mAbzP$37|lwU9IK&k>n97Gr~TFXyf_bW7n9uwi?vlG5M%xKb(m&>OVVgJyn5@D$sN`r5j^_?G!onBb$5PZE9f2G$VqINRQ>Wa;8>;4L z-mm7EGfae)bzi;U8v^mcUqgC3TJP@Ii<}B=9>q#mmn}EQoj_(cp#a)jP{|$C;a229 zcNuvUb$f{;M`Ne#TVsH0idlZ^+DhD%0ayqA<2dEkLCTpwu7AO>QD}PxgNEM!R zV)F*5c>7G%T_309&!h=g&sj?T(BM>pv$_^yN?{w7GQ~u= zF8`#GJUj+1dq!h;k<2X$)I5nh#M|2>ac1V`C9PEIyI=n@HS_NvcQPL^h|wA14C;_I zn?xdE;c9-?xrdud-?+Hr$V|Bsw!|obu7;DQIB>^XiJn&$6}rIbFu>tuVG-Upd=mva z^C0}>EC!RO9-|4f!*VU%MBZ4Pm)0N~PaS+)YurciYJmwi7vY83cv+A43*& zXFZleQRjyo@wmf#$nlQTD;zf7dQ_^=_a=C#Hd^`%u@c8zCpbH9{vSDRZ>IJIFW$=tghgqm=H9@eI!=a1eC;J>W5PxE@B;dpPI~%ew?lJQy6eGS5 zyUHlL^XDm;J3C`lhVJDY$_)JG-Kcn5JRmq?bOZS(hEkeBNv#W4pR%B# zY!gTh9RB8_#!WSJL;v|F5*vqJ;>LC`+j)&Cdpk@@Po-__kEFB;4Pr^2;eLXGO z>YvFKTrI%|PEH(~lJ#`+iV^6y==BZnACj7TJi+jXJOK=}5i6S(yFoN23c1wkjiUa5 z4%S3UE;vAi^4VZQ@dh_HQKlqa1X7W1?> z3?ZtAlw+{HtcWyGbCcHo<6`i4)_(PW%%+P81rtnLGk>$@eC3h2>`~o>&Mk z!HPpF_w)RxaPDw=U@hF+tRC+2>8~l27A8gKK};IB*QpXLQO=gFztMCXh#+_~8?cBW z5J7TMTe=0ra&SOEfZ$)90n4wGy9IDT-#M)6J@s)C;)^1FLGeK_POp7<$1BQjH%lKw z7jtGNdB-H++QQL)yclSz)C>zPjfQw~(o2BfQB*_)Vk6f`gBV7quoNXq8J#3bo-cKx zzd()W(@@i>sg|<`yfLasT@gFkgg&6|12{4`s0rQ!hRcjiePrEfIWm^*o6pz?yxpGE zXy0NbZN6*kxP;!*AwXXl{Up8Cl+RHx`e5#_MYa^&kiotE2V ze2y9Y%B8(-_;`4D`1m3>jo=Do&0IhN@>hjc%?CS?Quq0wcPV&vU_zBoyS#ngN67XO zC@V;0n)Ww7Rdyfg(*5PhAavdP;Hq<5O@`wPf;h7SAgHBLDKQpI*aXUi$-Sn%Pd_|b z{4m%MI%D@xPWnne8KDfW@0W<9-nrzNWs*u!GT00&Kn#zA46pAkpqdROEIn;4eV%^8uSu{&Z znWBY9Lm1;0ZWSA`4lO!!AmA5~2;n2+nug%Y?QNh5HJNC{2bwy>xSM-F+6>15dY^!Y z{4y=BlnKuz<19$wLK2tb%5G$q7dL{Ghc|U;)fWaO7PtjMkQg*n zyu9P!a*4wf+9e_(s(PTR`5qWM;Wpib8#O5P;X~@EONRFWvC35Mp!ylMIp_4gf7!*` z(tw>N>ILf0)NnBi9_B6SVh%$V9ilqdG)&h+U-1&<5 z@5r#3rDuNp__4T%*I1Sf5XMS3-}SylfH^@%Lw`AZgJ+Kq+Uf$JaRL}Rm{o%62omCk zsrX*!vyFeliaIcjiYdHlJp3C%@M4vZQw*1Jj#I|p6u};mP3ywtkGW7FugxCLxif}T zNMhgH39V_T)2YL+4hsoERMBq9`T8CLsKY3w7e&WLwBCF-V^$Z-@>Js(!G8!@f6?ZD z#+g`tLSvcGt#(sw^xR!C>%N$y6UgK?m8GF~Z3N<4iqxeUuyhc@5Rcx$#r^)7&I(Jz zQbN5m2Ds(p7OgTxbu~4h5$fG@T#dBU`H=T<>YoUL<6Ql0hXM$)Gpsca35)Wnm0(I$ z#-ak}Y=%38`4)JG)?VLb->zW|agag;^zF-_SoQY6&stGh3%TT}q|DJ|#_7phXH5E@BAPm~ga zRpQ5t_LL~{GP_bQuEpDwM2SN@l+0TI7+pAMRN;v=wE=A<9~ztJ-pUb`%jX#Qe8Qx5 z;a_M2#R6-Alv1P^*xGXu?E7V@0f^bU&QIXw<7k-j1~<14W_^s4|5!kc^O1$ERr`2| z;-g?~OL%t&W7^zvBo>Dc?Eka?N2PPI^%EG#`Hrob8Ze5hqK}LjiQ*q>1&Z&I+$fXA zlhK*D%OeixY`nm2L8!o_bnx8d-N-VZ1ksL!$eAC6qMMQm=Mu- zDpao(#Bw%rRtfoR-&fQIrj8bnm~N`9v9`Bt{d1xBE6h4H?>u)-;j9IXvtIp2onUF+%T+1oQ`!4r@BAn#==f#+^K zE{#a2|HXYeHl)ndv+153uUa`VsYxL7S~$EwqjQi=k|q+mNa^S9McP50k)nHmlq`{w zjAky@>M>4953tkpM1V}%*Qf&O!-uBbe}&MPwDI~r5wKQq!^-r@_BEWYSPc*@^a^BI znGFgM^&y4Ml+rTAvmnX!524Aif)=5cz5w6>b2~0QB2o6rz#>8=X6l%%p&3%ZAKDAL z^63kxVnIieec^{L&1)>$qRDWFN-zOapk6mI7sT0V+WK5-puq3Qg=_SXRgl0dR6*$rgsSh{2`BE2|NjF*|WMUu*-DDyvPCdbFG9G3f2 z6WP=SPSHe_5)5J3G}j}P3YOI28BG2<)l~Sm4*!C%$qJ;8s7!R zkJcVDqK)}8z3D50d5X)`dA6`!9ajzOsYXzVl*uwhAhFr=%Cb=L;;y6h92sW_r{kUZ zCKRqCD={T+)2u?1H!g-AC$6(x7{e$KrSC+nBXBUIm1$UQun#UuxgfR^Xv@or*dB&9 z+r)s)i;=2d3xlrYG*ymTVt410tn6fP|M%*_c`8P@FTuDf_<3Z@JrQ~z<~m5Df3pL7 z*9VENeSPAn!SIe~=tAZ{JEmkOkn9AhynXCL^vMX;f5=#3@~RdNILf_HnJXe0?M=u- zX7ib6P#xy`6yAuzudb)Gk-I}UjWIeSJ|Z!wrdxub!jc_-ua<@eV__@A52d1cQ#a{f zH%|k(JPE*~dM0b<=`)uuR6za^93kj@x*-%DjhBmKu@d;pBrC z+Ho;t$)mnLNUQ*x#cMJW1R55uDya$)oM}#B3!ZXN4;>QybDjFp-U8hrf)k)fg`r=o zF85(h0LBSC#)+>rUsKxcL73`m4j9rmh%E)jfWG~;%Yx_ay8Rw5QOoF-uvLQWQUu;3 z+g{$fkbhc8!MNI|m;bKOL9nV(Me6|sT9~J?v9V0b&CoXR6J^Q2kDT!e?rm?TI)DaG z^TqS_^vEo0&{Oa%O6=1g6GBbZv!AwhsMGR5gOdmInW$I<%IQcxi1SUeFk$`cBDFVV zN*zt4EWKqC|B3rKC^U#iwxQl#Z2LzoMoNQlgOl~{s1FyBJ0fwwHe05Xi)NTQ1c9ZZ zm2BJqrPbG8#v1eXq00$!TjSS@QJ-l?zB1TlWzLmSOE(K+#KUxbwVrIyC~v?C>)*^u z_@PlWBgF(OAXv7C-|}Xy(9mK7r!7RI>gLTZf5NyRH(MQARA;N^(G$)<9tspBr5qG8 zZ4?payD~zQFrB%KTrffVPa>$^&5n=k(kzdUBa&hwp(y9JUW44x`3JWsN?ewrrUK2Q z#y0f#<051N@y|6LjXovI@Q|>6gPJ22E7oN8wCD@FJmOOnNdXQ zjWsuv)2ePzMDrB391=52!kWYRpJFbX--g)(EqGi7$jE)ASDvfdF`AkxXILxWBvn^M zLD@}?q_B==uxQ8Kvuam0gxAj~EK`1m!x7k8l{qwy+U_nkMNWjm*Ddr>uKk@I&^@rF zje(huH&><~;t?1;dX_R}pddtw5Q`e1YHBgg+RA53@g`kM<)q(Y0mqidC9TyjS#n6Q zOc@dIb61RZ4chmNtl8Thzo2PD^0u81Z z@z#>5^7c{N+s7TX)Js>as)n4`0~=PZk;omvVoUl7OrU1StUOH{fw>`3w{ctChA#d{ zFhklv6?p{Pq^zWJ1w78zM`l9F}K25LY3`OBlVR z#r4&5|I)!YiHFa!mk(;yBq1{7-<$>_ie`>mYMMgSYa;!fsyC8(DO^s&sfd7F8vXbI z*6I3(O`1F6Q=k;iv=d^YegT9mkByNk`rlz3*AcG_MBwswA&nYjG5LgxT}!Z~Y;Ssx zB^INPqDqq~qyT|#L?Ixqxg0&Hy4P}~cvBrwOjL{C2x38g!#aVxb5HZ^xo=a$+m6he zA>SH$7%yp6^N~l!V&dY;)9;a-P+Rr=w`K~V)j#gBv%P#1jC_L zl{o)@&$`uIcu4>JD3Yb(nUqTFn==+o!PdEXy8#7GQ2%A%*WM^oEf+2TN7Q|srw^>% z4-T*ugds-Ae1cR^Xa#@gm!)coiPP%m3gLE}IIYr~qM2z+T!V1t0{vL=icR~LdEjz4YcG(mB9Yk0l9!gCi+GZwEjtox1a;C1#KT`{vHnXrLEOlX&KObGRk` zEc}l)W89NJ+nwK5BDh&eQIk_ap_%?#KcY4{GM@ln8+ArgslW{UX|mb?Tzx7~TtwH3 zlW)e8HI`4HuZXsNC8R;0yg@(csa}=vRQ9DDFn>DRwZIK7ND@?#@_TE~7j6Zb$vhM~CR)UA z^*5)Xd8wcPI_iOjmrZyH7ZF$e>}>6pJ|N)Kjmyu3cm76!vtN-p>1V^}@=cV*G}14{ z#kNA|+@m6So@kxvlBSgu_qn(1{+_NZ321q|W=t;0o#tr`Zz`&y!{dLuXXjxn7~yYb zQk(~I);nbf`s(X|0AV!Dy88oK9S&}8^OO~wHPNc#zj_T``R@SEX}4=G zN`U-nXk~inb`T|ZQvuK8q`U^RvYcKtH7il)O77OJNqChY0Zsh zcXGbCs)+iQz4%KxGAb%flJv+jU%bu$M?J2T=3c^%u*tx)hB!2xdp78El6UbtoO}dx zy!@s+2+i1J-{B`OZ<&c7l$uwWUg=07FV|-qA~L~gws2X&rm|UE+}j1z3G)&!R|YN9 zWkMl$D?i4_9JqliL2agOX!*tKQ^Kk*<#`HwRYt?#jaUgS@0OM5j_fR$vdqQt$S-W2 zot?eCi9Eb8{2euer-^jKlRso_(SqM`0+(fCs_}t!=UZFkhk-o1$K(C%L-zBIu@oJ1 zMJqUtMKA;3Gv8vP|7t2GlTJKa6fLxGQ<)09s(^WC!P~Ulgcfvy5h1Rf&NoPh>?29A z8}zqG#DW9u+Gi58nIqg})#I|nVD{<^$1O4Ys6(!e%k zPw8(}p4B|1Y#q=;juXX0$#8_TLitWndXKd~J1jUu;$J=at>atVsIEcF@OH=HV&VOK zps#x>C`c`w1^T(Cd1t-G#-gFIb$KK-M6vI?0xJ=aIw0oXU|4=4K!K(_|@ua)a$47pS zETX#Icif8!as-^h`*#0C#XEJ)qGMuTe*gw~)R8{fk$R_yxP`FIjIPh0w|!)+TDLlh zsZur-%<1+53@_BYkmTn7>MX@<$JVobkJSitYuJs5JyNg*LYwb7Wp|E0*-{0enfB0} zT@b&yw!^|GRN;7y9WqEk91YZ2!!chLGVy|Vn2XBluKT2%=eg}GUEU9k1wX@yZm80` zODunNu1m!&y54h`AsM6u@O!Vnr+{KfFPNs~pUj`8wWG~QqcXIiHrcXzutM@BZD(k#taKiDs!yWK8ncOUt7bLyc00b5i=aW$_~v&6_Zv)KjKLSIqO64I{!vO=0WKK;e~r&{l)7BOj+YAwkODhq zHt3nyb&MSN&V>yjG zJtItsG{Dix*OVcTvybaZ85J*phbif$ukVjl;dRzD|CR|Yu3IpI?}Ot`PI8ISAKu*5vIQy-KAo09;Yot5USLrN(6ysE z@DNHmxEcHGb!PJM(e?<5iJ_~%aA3OqHgASvs?%eT?+JQDwO}`&h1{z^>XzY>Cq`<^ zs;oB@KrM*z)Qtnn%g4bb3FQYlP@HKuTG0Wk|UPGeJrI__nX1X4@z!N=h}16jfm zXuBVd4cjwH!46t%OKg3o4^QOdP6h_OrXz?{jOrBp{|4U|#wDZM93Vdy?RkK47Eht( z9ISzWC(INUz`}DAg|4J596k=s07~dF$sp3s*Tx8Q^Cr`c+LCiqz zq+O;i6MEg0!b57`19c{e+s<9J z*okax5Ezq;nz$2dp1G?5vwG`o-P^m|Tt6+Q9J(y3B<5lWA~^4NU=Jp)Q7ayd@VM$e zD7XK-UFvH%d*rLZi%DYhO|(Qx?0Y^6x33h>dIICi**@g}jfIjD&InhbXfl(58s>aM zJ@<^1T8pzEngNEAv(fT(BC#7pM$P&kq)2;X;t0Mc#Sknab4x5Bqia@bWg{lYa}=(f zhFehB(2N{gw=KQnPW0**e&g{%`Yz+|5=~4|Z@gEyPX0EED)`^n{%vvzzY$=)<4n!w z9-m;U0K7YXfJIjb=3Rc zWetC#zf)8?aCfAhxt2rlla6ihX|csbuV0Gt$)8>HA#q&}XSY z|H1bd@p2r^N24er_Yqaz`Dav?afx%im?5yTEQhq$CX0hl0yApb>(n$d`+1$77MV>6 z=QvtzGt`8V=kgvZUIivRp-0fj7^qXSR^}&j&Fg1b6;4s)?sy*@O#X5>Lj>d-{dW;* zknQZ=u=YR#@B`7|w?TZ<%k69M8eW;wQ<9+i{H);a(Ajg`5E2?D7eX+OM zBRg!2!8n*G(df@#QWm{Fj;KUI4-$t08rbQ|-=M4xXaDMtN@!-a<+!mGmUvgtRr3-J$8U*PiWSp%Wo+xyqdSv4WzHYq zy!{p6|9z_v%l0@fO_<+iO(f_Aq*n!L0>g`mmgGrF(c3|dyt1i*N~aDwR{No5*&j;* zAHOVDe&ei;M`_l7i#q5=z%ao3t)-dT5@bW6g~Euht*z~t6?SixOr;*?+W!8@b^lOK ztStDDeCmu?Y@u8cyMo#9PDVx0Su@u~pcEIbciyf#|#&U+Hs97!wD z{z|Z$NHN%13L`=mqO~riBX~2-e>#1yBxjDZ1$WOpu%ejMp+>`cYvGyeUc^2Ge%j!F zbLQRHC>qj-hW9|{Q}=RL7Zg4%%>wmE(t*{?Ldh)etR<)v&;{0=N&d?~nPM+}{pOH{ z^ykUUpOiSGYu|bJ{fPxa7lkFGH|g6F5hbMahmBc--)by{)dQ)o@lcY#+3W6gaTI@Y zjxDDmBC^W&l3FWxQ-tkLSN^^7O)Af3__(7v7oO;-qJ1BX7G*L5`{)ju0&YQZi^f*? zQHawg0tF$!HvCRtPGN0NixD5FgG{)O#-`^I^@sj!gXTCAkCMp~ zawG{GJ+q%1dWi>4dwvU%e`WEm465^!yC?mV7W~Ecbq$s)?YF>?tP4l`1LOV zjeiwQ8w?a-(rZMey&v2OpN_-RRzkBe36INFkS%s>!HtkeH3=GR-|Mo(@B5H0p$|~@ zR~ViM2s8>ZsmqyvSN27#l+pgaVzeS8|$u(PKVI>irTeR)ua3<7EBq{{TvFCRxWw{rbb##SA z;-;+!0^4PCTzr|4hw_Be>f5qEYVN0l@`c)eZxs{aeKv4DxhPoRZImNz>S75un$>CDq`Snt~vUq4|@uEJ^og`%e@H?yg~EM1){|g^Ebl zgEMb1(2=BJC6?l`0)i?uTwbPnPWQbDaCKiMxGnDcSHs(l=Y36HJsX-ue;xO5AXTx! zFdTUj$+=5nDD`tMjCosCe92kBnI5lFR6K4Bev-@Po@KQ0Z~Z8|JNrr$J@Q@vr&O@; zsB*Dw=NebOSLM`R>+~;~rqig;oS7QxrV+r|eAW5TSBgJ|Q;fYQY??PGP2Ew4zbK*0 z(*Zji&jLqhZzb{_31JxZ;lYsQXW=pX(rh1NiF2t`vnT$SaJMnij2p5T5ZjDgzE}NCNWmE zrx;OXiig)D3-Z=+&7^aq)}(NDsM(s+8&uOn+RdG#GMqtM@vCIUk`A+Rda*C^KH{Ae zC&203!yab$8s;Gr1`&$*_EoBdxxN@GH|%Nq@Kl_()gqTYG@zm;Hm|5PoPwFwQ2Uu& zi{PT-t8~PZ)6;4>Pf3%utgUGIF->}&8rT~j6=J7aHv#2x!VJgLB+%J=pN`CJ8uSQ_ zu0cDe|6h3P|5jYrM=;o{CSA(k0KpF#@1%V+AG; zIzN!dm`#N7tNXbRdYS$7ms53qdYARt)FI~0rfakv)1&n6_P=)VzhkCa1D+*sBws0# z>m1FNgif`6TzKQ3{yP;n^w?pJbw2r3zPeVNkd;YQ!OKmzH&ys*YS<_N*aB(9NMG?} zH9?s5Z!wL@UA|+AHjHe>R{}iq{Tf z9w$WLNvZQif`0k%OuNvPUWH$1HmI70s{3na$;`_Ehcp2%(P0$e9MYO7_$f(SaS0>c z?+dF;nQaRf74$j3p&(V|M6IxqJ`ZulLAmCDu}v8|A`_X(q{h^>`|LZyo_{nKrCA;k zxz}N(-}&_?GZTQhIA8KD#emSF0`mm@u~ zfDk5%JS?Krb?K;u_RlQvrK`ATgvcJ>?;!<*{;hzJxF}Uia-Gwo5Bd-rx;O=~H=fDz_XIZv3NOpdS5 zWQ3m$x1)NHK9+ExcQ4ASVsd^yU7kVYuQGkiLNWs`8q%3mJ%yuD1SdYe5kC-+5T$`41h#NhtGuGk8PUK zW1W6xNyEGgWQCan`QmfToZ-2ryY(kMrS6=}56yu0t4BY4v7!Ya%516}FhoRaU%$wC zIl&=H87UPVd_TC&(AmfnsMzP-Cr{_c%;K|2b^pf&_%9Rme|v0U5a0bKB^gGG(Vh>U zb=T+?X%}K+Vvc`}AED)FYiaL5gxvLIo?3%GB?Seq!}}>T?bpO6AVVqesdv#6@g=Kg z-A4nJNZ!67&2%!0wsoU-&thEq#y%h7%=G;hS;(jbEjA?ow}<_AN!G5am8gp$SoBqr z+)I&E)AoBDJY6XEo}&4x=S~nuT7T2ilw;3^@8}^C+Rp`zr`Cu2MQ-X}=3Pz- z%{rXatd_3uC}G{i>!)*jWi)ZM&J^%Gm)?LE ztAj^sj)J@V$@AGinuGhSpC#zEZh{-e-Zkq$2B!mNk^sAxcabzNTbL+ z4hWk*(yf0AXYaT-5SpW7gOW0Ur|+yz?BFs?pQ0&D(Z6^5;TRkHzQJQ?JDv`YmvQBO z4eOrOa}Zx7boi_1o?s|uEAd6qDkQjh`}gL*FDtI2B__iUsnBz^I(=l7=V(VW7zu3K z!{)6@SP(_Ri)J6$x|_Pk$>sbOT!2e^kW%@`V^}5S8>_zVFL zv*1=WmweSh7Qj)95tqYLpej2jp~9NrtZ$Cf1uvEtAIZL2W>C_dJ-7ZAwl2GB$#qsR zVwLW3Y&nK9OVVqTbGyuvNu||3# z`#S_XhO~{}_mJFF^3=%88Zz)c2U1W_bc8}y54>-I7axfCZj~p1y({Z}(Ui(7T+XeY zudXl$rCQ9t$NDCw_bkMs8Z{=MHt;mTvg!}Krdm7hgnhvqY{>%i6w)I%jQ$EQQ`c%O z^YkLyVNM1HY+g~`vnTYS&a7Oz>%2Msf$4SAPAkhEfV%XVo-$@_|TVjBV0+%HQE_Z?C*%t*NQWF)%j3={0!C!gESvi?H)fGkKDoo`xJJQ7pTx zb59KE)QW-6X2ai|kY*DZ3ESTg(|yh>pkb=Y&Q1H2KJJ?c#eH=J^?!7I9?-BD+XPUt z&+d^GYM;P2C7(aIISBWI#4|^YUM1g|4zhU{zzr~g2Z$X!A~nT(+{*D>7kAaNWIXDR zJPU!fn{w(0sdmXAZe+o_7uw&7S%Rr$;|b8eAqbtZNyabJBl4e~oGZ)E1R@|@C{Wp3X25*J12N-f90V%X@2i{TWrP+?6Er;% z>&`hZ2mPkq=wbdBa66QM4r5!n@b7|d3hOV1(fT~VfEXWrt4YfHr-76D-rvRl0R@aWTZ3gTCIiSCD`A;FsJA*2GpXp?)=I zQbW2biGn(+gQC`5S&UPeL~CchM?H|+K==A#q++KrtR`cb;09?wnOq!w1xECxOaFz1 zDhiRXm;m{H(K`o%z*KBVCR>wiV2|Ck!B@wi(;78rt zQ~l8X#p(O#cTOK0{FY2<<(H7j~GwVTaux>iSg1g9bHU9Q4%C)x+S11#jW-#3xpQz}M zn7?@{WD{|6Bi0puLaSu<3A(k34t);a0H6L!_i^tT^@BG-$O`g%?<++5bQ_g~H2<(wgDEJ5{}?HMT<8JTlwAO@R4TS-IC zvly(~XC$5Vm8jr^`QH$(F(Pr?DARhbYkcsW_|vqx^78V}GSg3=&T^tr|79W@3boP$ z2Smo^kbLL_E^|CP%w|c+Wc2*tMlun;c6s><V6eOycY{-s1` zG9(H4s?&!dOPP^~x#S5$BvWkIh%7B1NRND`q>TpZ@jY3O%)FM$4ZWymC{J%uh^N_i z&tY^tz2XkBmvMs{)58u;w)hyj?pm94@&`Ht#O2b!(_Agq*Th3ZLU8Vk5ZQk|tCsis zE+V-#*>`tDA3mR4-GrX+lEf89NkeLro}7~$k9;fNay!0=h=RKRQyEu9f_->_&bMcl_Feo!lQNP zFPywwT!>|4f=aZlhLJ{&2X{9x&Pt>oph@U@!wL7F*={HAe8)JVf^QjqV=rSXG;Fd%keL6$(GSTBZ&YR5lGXk;KxR~*r`xgrL0qs;pq>F5U--%XSyvi; ze*A)hgS&4&8a-PU0(!(xm2~xX%^?|v0hBFQDI4JHri3%mM;63QIUa@95HA;?*y2MQ z^qH+WNPxbbN=#`^MEQDLBtKx(Oie(69t2E4x-znyt$fPrzm43iJkK8C##T=(E@sM2 z7icYbc`;QykIPV=z^2)3MN9cM%E?+@A`+X)@6vgC`eA<`rmdeo{qXNq+{$pSk~IK( zyoOwBXm=4I?rclXE9}vmb8WsgpV{&l^~$$m$14ATNT}B2d7~Gilgn6DQCw`8IUQ$x zKwwOcz`!l1Cf87tGJ*CRlpvn}{kx+j`^kPfxQE=1QQ|05Z-x~PTU?U{%2F?Ui&9j4 zqmK*3rOxBc3HD!AR*Iq<9z1MJqW;Q!J{~$54@cYvnH10YU#kj}p%AJ{1O^Obya0;E)a;eJML^sg%H%mR;JOwekZAV0h8UvZ>}lAgZslyuTUPrd&d{IF>4 zs8iW>j{k+Nt`|-BZK1wo2Qm$Hjt>&mF@InsWDXzuw^ZtCnU8FL1Q`@OMtpI6Cx+}= zXYJQxf^d5V@k2$SAiL4o>m*jCS8q1+$=sV@KKwF`>^&V!Zb%Q^lau_+W5w59wR|Xq zO?EoWSHIA1`f1JJw7hXO@F>8|k-iLD&Ha&>fa5>z;!kPBvKSpidA^C)!|5OUoNHLe zmRU`;V0aT`!t_obYkd06HOEC*oeg^pT)kX1aH1&ztFAjN&ymeS#YG!+nnh&bo^t;6 z0^x5KSWRK=Zh^?Qb zFu@2r6sAz3iMAgrd%)nqLG%+13DNuBKh5C;>tMoS+Lk_PvuJQw>!kF%9F(TK6?6lw z#SxIq3TiW28`E-79skWEtbC4$dn9=qAM*6E>5Sm1PTA; zTi^Znt7$hA{l2ctUC|&a1!WF=@E8<9?~b{YDkw+)R{^k^)jf77hAo<*CGz zh1`iQ=^WZe!o##$7;N9y$&fD)xSs9SemfU4e@@ewu3n+D7(Q%*@us^A-IW=6uzqSn zN#O2J!FOJdlkY6n$F0gcP!|_vVK#Hk)W7KK?R6=5z<2oWMNM|&orVt}%4xW#JQUJu zvA%f`_;W28?e0)`c%Ij;i2ELu=98l@FY}xie}*EVV#7%GNdBwvn?JP4iRH>h z;7UE7^bq;3*ROGjlORcJNk47Zt??M8rylE|;X~DvZ~bI+k4=R-zK2>~g(>#SO@($L zg@B_e=$V2Pd&wHbW6Kr+RKgLor~t0x1$)h#R&g_O1|=pN!>P>D4=FgG5+Zr&t5(*= z#go?@^eV-`%l`8UOWC5J~niUawy@lZ%XXszQL5XuLkq3K4S3vka10^Q&K zRnQe)CBvcKgMM2m%4VM021t%hfQlm!j$q7Q{m0lK-=7_3`$p&JMj7tv^xp`$pMLFO z{jM0z|hZP2B5hF@hOb)lSs|al(C^dr4$J6@43_ zV$5ay)!Hhar$z!w27KbTV|-d(2g~qd=Due=usk4quL3v)>$@cYHN1UsvclTBg2abw z^|EF)b*jdRfxmE@gO4v>F&C!c#lvj}N+KUFwq_RE{ry<_;c>ccO5NN32VO3D13+}uv^qQ_wK&9?+B*t>&38_3DU zHc02X!(O?ISj48t*`b`AsgUsJn^L%Vuze(9+nVbw>zo->Zb8e-%1`KJDEOF%5zl<5 z8(7Xf%po!Y=Lcr1Cmlqlcq-qHFRmz}fU>xJyv^KrG*n{J`QVYsDvXeV9+c}VoKoIctd$Po$O1>9>JrA znO`;ne3i{{1u?C8sP1*|pS*Wb`cP?l@-j?0d1LgkTE!G~$!Pr)4v0AW++3ZQmrYn9 zX@=hiwdbv}e%tZlg@Up^@mhCjt{RcpgJgb(i|yNy^`0)7C5gSFcmv-vc!G8wJvuNi zy4;PXsCvr#J}!~t7eyA!bCYk3=&nNbe`^(_vJvm#g_Uw}>HfQwJe5I+hSY@dkZVoXMOh zq<~SDpR&yfAYu_6jdN5O+haC(`H>R^M>ck?b#u2#G^_64-(1lv%l@iT1;sx(Q6_*7 z)}3tSjacnoRv29Pjs9Q=R_Z&0m5}Q3mpJkSurbiXmIe=2m{DHsOF2iK%d{nT%M&w9 z?KI{LkvEPM@4c5tYyMUL=uXqVt)5aLiuxVUgi8;K{Co+#wHNI zPOGTx_51E*4^QNBJ}Kr#>v3?lHcN-%e0of2ElMS(nm=Ms!fQ`UbZE%I&&4$~RN@0? zP5;x2g5zM&b)~#H?d*PEjps~WK358Yb+A+1L2{RM?o&Ue^r;2UcNO`L1Q-}FJhu!j88dL~t{N>Mrlh1K zC%;-@_nX*n$a%T`Cr?$9V=NSoyxzXPWBYEaXIO|DMr?w}Dw($S_V%C-GaU~Vwqh=h zZ=$bmIo3x-*pZ6iAJ?kCu~x1897puJqZO>lQ&4^&{wszP<*|~@*jrob9>+orq=pHV zLkUrZ=}(-9(K4p+O)cOj59nf$_yU~OBdV6OW#;|Sw9=V_Yi9RF$-Flnix`}P-bOt3 ziSkLfB+X9m=S(c3Re2G|%<31(+aH@+CW_RU;AU_Bb>VQVpTDIOvJX>BgF>E+PqpVW>KH!N2+XFDz}J?I}UH^UEqzU8Vx zmtBbRh?R_Vl{&i(MSXnUd_Z#a(|Phvj%6YOi2!*8^I+1LP^QMV6XHF7A`|}2O%F2X$~FNcEY*P`Umw9jboQex!kjK{7RwfrQ0wWg zW3>%Y#gw7BtUAA&>o>eYoSYdq^7kp9PG7}V1`$yUFd)sba-4=`0;eU#WmL&u1rqw_K+1XA&q@fijLJ=k6^)_JvL;(SMAx@n&r&E`h zp1K>3D4(BDBa0ADosh89FuF_{BtHmB7zff_$Bb?I+=^QH;q&nQv!f&BeWO`o{G4!X60H~K z)8^YHvhg6;Af~jy6Fy*$j?~35(lnY zat=UYRNU#X00~vUsk+*w_sc_OjkUSiO4}K$Et0B`}gIV^dX!i zp8A)^RZTOJXDqX#=3K*Kx@u&rUqt#t=2yn=NtillZ`<0j6$eYv;6!*3Zc&rIEeey% z!M%6)d@UNObKCo=0Sm$4rw>g=wX~khUv#%nIjy>xe}>sb(>>0aYD0VVq}p7mGHLN> zdD%2drxdRlnf*fFIdW+I2tII16PrL~ynFIPzu`QkJ##mV+16)?)bA?3X}Zi@sq;R( zGnhP(CS3;6uBXyBd#SRr7^53fwO1o)e%EJX25EkC7nOnHb%I=03=9lzJJU6wwrcyM zhMduIVWT~a-AyEiG*l`Js7`79J*6x?fINq^8!Pq=%(JC;EdOTyb0>O(3|}Wi@8S7F zdMV}|0uzZdP5B#xfZ)9iHs)VUy5t^%e-=#-*xYI493;rFSq30HFYfc7*HAsf#YA~BJ0-8xY1te%J%q0xw_Pp&EVqu zXMd?JLd5Ilie2AzBCM)`^`I4lz1(XBWNe^8qvan|{isgxk4c1>81!dDBOe|G!!ZX& zJv}QRaLX1i5$ON(trLBQR}29SM=Ew0Y1KG52i}i={ zh?~{wlO&7+evp)Woz!=B<{_Ng`P6`n>}Ug1dU5}Bc^O_y@>C4Lp07k~@AwiULHMnk z45US0lsXa>{@$u=gGl#}(=ySEW3zZY)xxJYmxrvP&d@BJy=B_( zDcx!mJtx#FhbIH35xts}I`1k?k*ek&gv`JQoT;rX8$SBFiqHe(mH+C;EGUNytR@flwC6w5DJ50vzFU-s;(vjdB0v&JLz>e28pnu=#I>uT z4YzCeq^_#sBWH(zo7@|JZ@UcTXV2^B;t!dXJyFpj#4}*fM2?OGf!#&@NJN|qGl@1k zBO?P$g4QtcuQ!_<-KO`idf2>XIz@e>Q>%0#1VXUC`d*qCG8;nx+P#wvUR50!Fs)V8 zbWwNn5A-dGJ8a?hfvSCVxhk6LhZWogrIv2l>)v0n%(uGFkO65`IK1XcG`L&@zeQnu zbeqZYJP#d_!%qZilXku>Fljt{F$=~hyA9-`P^pD-Z3?-Sj_J$my3O3g_V_p$8!rZn zUM@gkpq4*UufMytwg&Iwdj9^kA*7iRCA;3MLZB-@2N}@P{^`S4y8F3)=eh9UNDQ1$ z_ZIBDmq@)%c3APz%^|pgX0hmDV!!v638nR!(0P^LMb(QWUQo{m5h-3G%xf?g3B=sI z6?d=voY}Hz*Le1qA=O>{^5Ud^RpeIlRtUQ_Ln64S>G~X$igm$aB%(wv#}R)2W=&dD z(D$->Zg;NO2)sh3?MKvK4gU8H;u-cJR=m&;@>G+zV)~g_=ut}npob5c{H^E6Jo5Ma z+T%&--ruly=(ug?EXekb}UZL-r(4ZeEd-av|M64kbQ0`?fgxGFOi-puSIA!(=-t2-iV6!~ie7i#BqSt& zc;g%_il=b%@Me9$OFM1_yiI&S3k&Qye!gltAMtFsKI`{uoFN3(U6jw3Nr~o`YTCJ) zFhyEQO3?3aD42}IFWM-t_gYXX>z6(V?VY=9j&gSFG~M_f;fWk{(L%MtYO*z|WA{Aa z0<1iEn^HMl!9!)*bF=X1R<@U#&)s zjI$GPT<%(ZdQvlG*VHg${=En$(BZ=$(_H}JA3S1;Myz1?E=uHh{MM`^RLOf0zO%Kb zmSlm#zl+p|ZHG1FPP_3SPDzlAvc1}Q3e}U}+CamK__F5(DHM#>@|=lqGbSI)i=|(j zjQL%SJ^7-dl&nd;Wzl)F76sl44qA|^v6c|S&I4Z?CxBWiTKNXNh8EXz(?%b!l+X8H zcm9tH09)ZiNxx;>KSgMwFXb?@>6o*l>doV=nYvd(XDvj4hMs_eGK1)u>|NJqJ=sAB zdRSNq|15{Uq_q))K3By^egj3+*y<)n{?o%mR?+k5lT}esSoRwRkW2SLkh<8(FF$;r zl$z?h5aa|lM)&T5u5h77-Szoq(=|klEvTfyQo?2uaSp6kL?Cp63q;$^uTt-;e`5xo z>pA=tNN~1%NlLnae=48r*LQY#c?s@L@iKIvu3XmtbaUVRr+zHFe{lLZynVwUL=XNO zY(EJ$$9{A+?M#$O|MLWgF$grWUTWnIVCJzsRRs>F7kFZ8oWFckx`sY zRq*jmpM;3vz{r)pi6lNwue`Y0QTSi^SqmPAnom^wTvE8tV;DINIiiansT@Ay)#?7L>He^X0A;l+ z-7fp~a6)#S$}KqCgu%M#?EcbcVUX+Z>+6FMv*?dX(RIOhs2>Y**q#llUlshbhHfSK zljbg8uMg$iCJcPG$yE2QBJCraFU;1R8;n2U{4>Fbl;M|0DC&J(=HL8wndmWY^+XvB zp&qw(wp8Gs+1c5N3F7ef*L}%}Bj1dn3JbnZ2`vsuDH@@{Jjs}lY3V56gSD(QOma`- zrH)A8_tw;S{7&HPssvE)_}Ep}%5Njjq}2QO8{UQW=UEIN$j(H4d_Z@>p=@Hw5ldvd z-1>Oy?76{QXHC5Gs8&^tuEfy#pAY0wB$$h9gEx=vwH~az!k-V!$x}@OcO^_4u5l+ zFdp*jC-SiqV;eO1id_4x?JeYw{ir(k-nxHn^jBf7D|*=Pn43o1rH9qO=`!f1_V4D8 zG~CeosmaL(D|JBvJQjk(X8%46HVd8Vy`xS_)X;n54D}BuDkDu3G2h=@JI|Mzh`Q6%=OV#?|1k{4S; zIXipCK?mtkJNrvv#kTj+y;5E$xM2Z}r_Ujj_=@_i#2y{=q+OiVdmOc5AWL0yZ%xZ) zNe}1$7%I?`l-xgk9QAbaYDj)R>M5=QeP=?Oc?m6nLag-F6D9_okg-AiLb>f--b5BBp8KJ?Y=x3zkv-LxtSl zQSzux2+K9Mb~han#Cm#q#ml#DB7OWzn2dsETC*B5r6zz^s1t<4YH=J$cU`|5N~t9`eksBo+Y+I1Fx=5HN& z`^sTsrjm*x3LpyKH)4`&TC2{273}nj>YX7Ri~H`3_~1`ZIw*A2ryZ-Ix~7RRId%Ji zFYyGZq^+(XH@DNcWNWIbe}x49Ub50>`i)1EQb%5yq@l^X4OL4GC;FoQ{($QLUHJtv%Kr||;IG(n$46KHepIK5EOkUrz33N~nB!LAs9w>LSbI+@g*WKbzjAfSdRjlD zXwo^<@7K4pIa68bCcmGJ9vr%si-bBfkb#dq96|g(MQDnEjqE&#o$j63&3mjH&1(C8 zpA)Rc-KXBu>|vH!I1F!^7*URQ^coCX64{&HbUA;)CXFm*TtrG$`{mpxV4z!{;FHvl z^XRCH_E;i#s4ZB@XF1xLYr<{0tFgk`O(|`sakmlNs#p+2|3x43HP(&HObY(%Af3~1 z5F`#K7k2$UHz8m%$>Yo<2!0*cR~Lt#%7QRtHrFywYO(o@E~t%CA7yCcnK>TA*)WDP^x){@rQ-B$c4R7^E45Xa9KS)2?O^GkK;Eq=%VM}@G)_ti;deG zNAiFCx<{gog3Xi0zb_|D@pLtrolReui|<+?S9BjGabg1rrTkb*)&0L`r=+19QX z)istl=P5{1D9xSj4?|fl>~;KROR&Xo58Neqh%hOlRx8HzX@Sw<3~8SHC+As@)e?!y zwuMvc*Vp4ZEb?O4yjwI7P@n1r|&J@16698!8<#@6>+aEr@(|{`~GC z*Y@NYe(2mTm}^3>p&(U)B(U~58-JM1L}|MzSBmSTC2!L9zp**2yXPVE;`|8~9D{P1 z_d`2yY#xzJjJj^_PkjO3(Mz|v>*MYihOCom6NMba;jdbadqEf!K7SUR(Ix5bG0gh; zZI+nC3!krj>|wh|+Xajpn?fM~6>o5KbhPl@o=fGo_N9XolYHOHr81B{Z*B>g2`4n* z^AW^Rxk<~#W6%%njNzRcNt`}3XKZQC%FSIpk$iP}MGjq&!@agCJ{!S9J3l9d)a1DA z&va*hf1e%AdWq`Qk8@kc(axRQ0yJ~l5)j<@uaH_-vu&QRy}Xn5YU9-w1NE`f4?r`e?h&EO_n3d9{Z|5)A6vEuRn=}Xgi z7>o3_Ljy-MJY%yFg|9F3#ti&?n%^Rrxb4(8G^B)>YR&$GH$*9VU4crG;5=dWj>@~T zVs~1W9pFkM&*4Mx7@ngqRVb2+q(S@uu=3mU7}={UNBR|d-4Qlko`;2 zeIiUz(>fUzsgm&~ywH%5^MC@wokJr@75w>Ss3Otx`LYHR6zBPHB2XAbaumLSw%T1WWzoukAI}h8fpQHi zfjrd%evbn>9O$9h^Cxc(=VEXygomj=oSB)itKXM0H)jZu18qWkq7+3{1Imkg#>%0L zGWZ*7{9eafe4_ftoMbsrHn=TW|44D@Ahwek(kdC>W>Dm(#hI4*F>}pOrscU1^ds5h z^=#QBd+WlsWOz0IJS4436j1W3zw*aRy^L?#Tdkb##~XxqX;0(8Ja&{Xsd$=|nmUK4 zw|-(^e|fsA@9ofWw626nL`BIzFfxMti_f8lb=dc6b5Kd-AJjNeAv+yUBb1_D_j@d_;eHQ;|hp&v!FSjQN1I97CoPJgC)tfooyA*>Z!c zje^|NNeOaM@6AFj=?@WrSDp+~;mmj+#=(}%l8(Z~Q4GsXaV6QHK0N1?`NZ>>%=Uh8 zM!5x7q5}P}KNjgVbaQaE46qyz0DQP!%PJ9ab%wa^A7b0&Ba8z%J9A-S;eFA|J=Use zF3+Pi*_Z>T1KX;L%y8h9WQH?xUpIF}*F{RPs}VxrhMYcrv`A0LIIw3osy9XGEVTTV<=KBB~MG7M)bdh^}mqZVE^Nw1Z2ZKEMvXPcHb` zY^$ZxG-Nhp4f5=2$YG${ypLWa_aM++xDg6DV(!-tnKU!s_l&i zaf&%CK9sPv+qFRHTKPNpImp2Gh4|M zG<=uOW?5oAR&-kx%ls;)ylr0Y8RkxjxDS6-3PF@i3dNLc3u%?)vDI}1!^gA|`%HI; zH-48(l<-%bQq%4rLjA7a5s!oMQn}3)RhH~s1dIfRCn=snuli9u-d_hOwQg-sSyS-( z^!!5Cpch90p#gxA5JB+S4(UpkEq#y!lzR|8$3$ss_D9OQsbLLUd=#(C^^Vr={QP{{ z_9V*pDC?1Y+?v@XpWO~!W^UT7l%$MPGT5nsK~kFNP3`*dkJG<=a||Ei(wREuc2BA* z&Is9(PxB32l#;8BFHdn$vB{q0ss>VlP+X)`OgE3^P&OD@_(ILSrL+ybsrhFA?d|P! z`Lp!zqEvs{WPH!pb3H_39uXyaoe-j}qk}9~ zz41N&wjT?L+97SF^ZJk^jRTMa4x566-?bPT#x`RIn{hKMLsIi8eivh?=$!Z0%# zcGgtlh?R?AiuLPs5ef#gx6ZOhZX(+AfjzAE6Ggm^0exbg4-8*m z9PAHSYesP7C!1dmzs5GU78o{BeGsSxuvr491p89dH^kpR4_3BISqz+df0$~)Ab{B3 zYVY$WOMYA9aj2AJev3Z`PG);{|H27Qj@=3VIAFAKB^H2EQCSJ~@9KDoiL&uisN@4s zuv!O&?NYj+oeakur`fO1adB}_O#aZR>a*Zc{KAtb`p{fSsXtl3$uJv17nm2flmal1 zrdLcJzbOWFyvceG?wc5>vHxDh-GX@Vgu&F%7VIQ@Di8i3npOz4f@Aph3X6v*YSom8>z1ahmG5D% zA5OOK+C_IVz-sm^8TU zBbkg`ij5rLU<8#pn-jT(tmsmcd4K9kPg9@Yoh4B~w_1M{Pw#iGL!Nuvy^34`W~ut(|GCRhsCEQ}k2hnrTv1)wk<+^P`#z+q|* z4%|m!i$D|Fty!f|ZgQ==x<#%5s?`Tbm{8xn#gr?OJ4N(D)qz38fyASPTM|XDNMwXn4Nxy`_7Pa6@k!!ZWolErtV2`wGeY`v8%} z(dFgm%TK7MoSCgxneaL=DVkFM31yu}#*dVGuJR{3kQ<0Qq}0fBV0cU~+N*}}7h8Gygd$F(d1+4xsRtB!D1Ujj_#iMv)^#^P~ zYkjZI5u30TPr(QjCy*7km6f&i=K8urje=@!YK8qmf_0GxCoG_3Z$AL26L>6N`d?d3 zQu?LF#G9S%cTy_R#{d-WVZ{A+dY@fy8l?ZmPq`^;!0Rh2q|$D#?`gTipvq)eW&2=`>qV8$-xR;>&Z>kEPx7NL40!q*3T>_2N6=VySCkasS9wC zO#81x*Wd!dBHH@kI;P!NtJ;2{H3;M4Vum5jyR@o`k0NSn`xySrwHcZ|@nSIF6P#Pm z58J3{a)-}Mr84j8-%Yjr{WkqP)jV?WSs?#I1~Xg9pmCZWCGFkyoau7xx$WPt%QC{U z3vA6@P6U_cKT{K9=d4w6XC6sHU}hI)&^OaSP8 zVC}#`*aw*dsGprL1s-OKDpvpaq4eNxrOw+Y6xG7LfhrY7zDzwSMLd(*S%F^ssKAYaAk}l<@<2>14d*2&=599C zt%_`J;uI78N*XoNNCg?r$GG)3u5~HMG?O)M4};jj(c61CtaZH9tY@;KiBja8{npt& zTQ=rBz@&0H;GGHYn^A+zUb4) zUauG(?}Ibzeb~z~wxMls_ds3p&*NQTe!S@?qI~CbtImVy5v!})yFIx8KUAIS7f3hEP|QHP@uc^)lPv}j7Gi@7 zO|%S~bs^PlPnkFAWOvT4YAU9C86qGgr0~}!cxbJo^MD0_f$3z8Q!Zl|ycR9i^kl`` zno+=Znm@RWS{BUmBvsIKOpoqq#nsi-#dOn67RO#lg5dV2NAhx(A###X?t_tVeuxFc z&Ya6%zZ1EEG_o~1s$u^v=oOJx94ux)02Zdz)2bk>uDYyHau_}{qYDyx*)jl{`OWmH`PYsJ z-j<$_J;`5!Dyw|%Fr>c@q5jDBd-GyhgCPVQ&pk|BsGWN2tj{IW(N?%w2*PtL48-|sW5#EkiN8ZXMt9k{Rc6@X+sVwLZpQdl+Jn%Dr}fR%VgBh zU^389Mvj;LF(zd7=Nq(19$wYAE%1AvavZ1z-5>6H50r4&E&bvg0WJ2UTIP>0_$sJ; zmlG`;yVbN-{IQfh(f61XFC}@BdXY)Im@w*dfZS`!HL>k_h5hI>tyDXBPa_pP3|~mP zR0+z&(8-o+7h0KJ{L&_eT#zdf|Ly`fxj)Wn5%e{KqW$A&To);GxTAnkpr`WF(ZkUC ztK+F0KJGd&RgqLNea;N;Qu1yIhS}>+(ib{0?Y8{kDA?t!lkl8 zHlR96$(Q+FgM*9{DFu9{+vOU6V3a?iEv6PmNVH7$OLn7X1R^R_L95r^aF<%5(KU)0 z|NNwzH{;%EvQ$FgnkX|bODq0{Xt+WR^FM`!tkA!muH#Bh2qM#bV zh@Xrt8q#<*rXax~Q!W9&>r0rUJtow@-Njj1p!R{}m+%_H%gv3CDRJ0}QXd>PT-8CFe|j2Q6)1t9Q8_EAq1ZE5f8 z;W{b;3RTy-hIie)Pj?+_KHk!C#`lsH8RBAVv9N_`@`Y+VZbDnD_gXPP`2w&rI7~z9 z+hMgmb9oRwXz?=-8s;r?kmmiVbN#{;nkj#Yw5y&8NbNlXGB z!qH6q=eIxwoIYV23;@}0*z*&SLi+U-ZV}Y6ZfgZV`e2_+npREC*t%I;Lzj!3`?bPs zQ-Dgv#2bsksp4)_oUb;J=qS(Gxo%sKhLlD<2h>i7Mh z!;zJ}=W!g_IfM||E1P3x95b6_Q#c%ZZ<0Na?Vz$Ek&=uYGeXIZloj><-k;ykzdRo2 z9Or&r_jNt5=g0|$XJsOEnMRs&P9OYZIj3J@fqBHqSgM{$-sP;^t3XW^#ZD3&Cp24V zgc)!@$0r}C&pw9xJ_mSg*qLij!dU{CtK0aWy>sEX6YRad@Y-8;vQU!$cEAROzFTZ; zv*C)2rRI#9^|yZdR1rVg=>MQG+MY(Q`|3?ShQ4B`Zf@`I?;ji}6}GPTPJ$)>=SN_D zbr-D6K1YpoYA0j`#60`pF0FQnkrs069pK<0)*op&#WF#hbys|EyMY)(T2e?N66Wc0 zyRsD=cM0vyDBo`fwhIj(J-aq+$7xInCDkV73^WM_H*%H_<_?YLwzU@8mc3MwRL;yO+2@- zP1`xh{Ct(*Biofoen=k0KPuKJ$47EBnT=TAe9(P>`p)P|aPJU{=(q1rvGC^yvWrE| zcX##+v3b`cOwRxQ3sg7=?gdTxYsU6PZ!n%H0FoIP*p)B7F`hmGoW|hZN#*|{18cuK zK3@i)9XQqxZr{GGm;Hvt3%{rcR_Xr>y_o307w`_3-m6(?d#Ycnz)EN}Q6kX8qStH+f$m}~v4rrODog5ZMQ2*)GL}r5vir;uQZi~nOhD%*=PnYYMEBgF z5IM9NkW?w>y@2d|j-@YwJ7iG>^JiY~a7^HAodf@j%satyTngD$PTl;?@s0`Kgm^My z+duh%EfaVOK@QnUQ4s5e(kr|vLQLGwI=Px-vnn_`Ur$DS-es6(UGGPm=y6TcHOc{< zJ4P-5oWK9vms>udS}3{Qs_FT2%WS~Ix(#>qm+7{0a&RDs5my zegW_@ciG~KyWmWfXMw6Klq?cZ4IUK0n${E$)0kaee)`;%BPTT%ZoI~bX)|&giNI~` zxU_^cZY;4<+vRV%r@^*DB3T$w;w&#iVpFMr8A_5UF>^gM> z@nDi56D8m)bY;gU6VaQ(=!opzXy}}r;Tss_=ub{hlPk&9x75N@Y*A%!5D}J*c)8+h zm&2mLL`}xtYPH9?p0MIAi+8j{D05I^!DyYE`sXp+$_t3ClXdcX=u3y|GdPIgr8)>F zsUpv^~dm2pppBODmvyd9?P zg^_b`okF(vfOCl{O#=Vnbc>ERF?kV2cVFwB3j>XEhnV{Dy^DB~I9*^a@pJ+wQA~(- zkml=em^|yy;8xUKXsi3=!BU?WBxxn>IP&$^#c$>tTac6+7Ew^Y>S&Jg4o3+}0}lt- zrk3_1c<%9KV2tBak1qjiLC~3mN4-ip@0EH-&KZ0(s()Ua%%PGI_P0+(p%0KWqf%8B zCm?qY#uVH0G^$K;-c^Tu8PR~ppc0o40ON@xscu};ky+5#Gb8BqsZL_K4MPp?TsNOL zMN5%450iWwPL2^9FJ~Vl;8~F1(zfL+A#?XJVy|VTsgqgCxabVD2y4T*ty@W(HRLXO z&wZ2y5cBt+p`cPab=gFOhtqweVMCTj*Fji7cFc`rCm9PmwOTja%a`n= z`NfLm*hKCZLGF zooswF(eTS+9h>3n2KcN-?%p|KzNWyLJNma3lC9H9>;xfxY)?VoF07*gdY?d1qo#jK zN=jN)^h#&D{u5m)&$=Y`Ihp4NjC~ON?<)asoXOmKHObEAS zWb#f1oEHpkFfP!XqcQ&(?;+ASmi`_8QGj^2{;I9FBKz$7+&N4e>%TJ|0oc(95QQa_ z+%)ZWQzS`T=oVe>yN_=2ciE}QWSdWJu9GSyl8M^9pik`s1Fg@f2AdVOq3q(Vm&5PU zHkW_$uFpnsq!GU^25J26&h@@;sn!Y>KDqWmo-))1Ma{ARp6k`e^(>x5vp7V~yS@|f z9wg{m_8(7jE>vi$C>hN}+B?j@MU~fc<%u5#_i}Ue0s?%#8?-v@MEied5Vs1KI1i*R zcB(cN7ApWAX)AZTkaT&f-xYk_C*rN?NUihnJ@4W?B=9+1GrqG=CQ7X4K=OJjKbD%sb+EJJcvxqOyKP>+$s-g- zatDMSSzcmhJ%+uV!R`ahI~BhsENWSex0K-$_uG^4j13icyQK zr7_L7GQ&Drl7oy%kekC3B_OGEFmz$u6 zaY1zsdF&z~%kb3sA0n+LU;KqdBePxJp4n^5J7~kIPYGH9>sJ^mJ0-}SLM$tr4_8(L zUw8W-@V*8IrCRQ?q)2j3ynhj{NbetU9iJxr@!+I@F%XO*1&u6BL7=UUoc|P{om5vp zJq>)N>&nKWJEF55a4$=4Q3ZNG_|8R>P(}RWEIgY{Vp2)C&{h-tkvrS3#Blx*EAx<*0p_7k@op+H&17GE}lC+9950Fu0s#%a8< zHqnnE!B8K2M@OVx{}6gm)~(BCde$V?P)wk*t}a1(P0Ez$Dh>psN78!DwLgzTKe3^k&9zTEsH4gJkgK$~4JzJu~c(Md{65#s%eysDgtX1_RtP`AGrN=MBm2Q&jn z$q8I3qK@V#*_PJ)fi(y{!U~yV8D7@b;6A7gFs-T;Enr?)*jqm;K6pf~9HvZm{l_D4 zFBaD|Q5pY@5e2<^rBHWwHv?GL+zYzXc*}SzTL8e(!7=Y(R+1|3VJ(nl2iGN zw%jp+1}*R0G3ufO(a2HYk*8C5ZIR1HBPW|6DLK{cAhXz%s($$%L# z`H{O8)}O97$21Su>!HrH7EGP-iwmY8Y*f#Wv?@Han#uT!aTzR0x&NvDja8Tf(}<_l z?t4r%E&$%7jG&hR_uBdW{SGh^WWr;FsDp4KT5N2+WcAc8of>oMjxo>X_psN<#7;ZC zRVu>;Fs3Sg!(h_wT}dxif+`8@QF9Ys{;vV6)-l%m-`8FS>>d2|r@004sSHlAtWp9$ zw|UaPG5vxGf4(56&HWT)z(~+YrOov3HVPUb5 z#Gm=7w@_AUN)Et190-3BByMcw^0sVCC`a?_$%_SGqW9nR1{)b)l zgL;QfmHcu!yDOy4ycKt&A8LC{uKHf3-&SW8;@NM5|3m^7ztPFfXJ-_d)OInDOV$GfY0QuXa8 zBrPEJND?cTu{YZ}Pui%|`Q2l2P7Un=GYTDP>wKdZF8enu>fOOXk>x)Oj?#olh8bZV zZ!CnW`aURt5kD4cRritAdE3!FZwg9Uq)#oBw!5s^9DWT8R}8H-O}=LA;@b!2j&{F8P*{86u~hSEMM zQ|Fm;Y6k@wr@CnWxLmj@3oV^4RGb zSMD{@#%nDl&&d;p$2NpEY+5anNv$PGb-`dd|CY5{1T;({KkoI>Kk3r+7S(0&qSx5% zXHM2=!NvY0yv*|Q4G_`ut{>{16ZXiWBHyV98S=cZDj%yg*oW=(gYx-H83_%C774~J zlf@40op@epB||RTSA6F&#Qagw*GwipJ5Vwb2)R9|%FU){qTMj^4zXbr%@ZOzW%Xop z{&`QA?BAWi)MohX@T$VrKJEqYv;kU~po7 zcMei~vNbqXu>7kD0y-r=znHIj;?^18Y@$wI{U3#&q1C;rI@LDu{}0&kkC4RXC3U}+bHi=AG<->8TP-)R!m9Zcl( z#-uLv6nPGJcfouOw&Oi?Jj_JDM(4^i{4Lb-DZDR5%|UCBr|UJIE*@$Sa4-{b^_L0* z*boA`P81%eMZ2#Op}I?Tj8sJW*)|2&DR-`&Ds<+U%&}O5H-bLs<6m#p zxfKI?6BwOJqv6E&N0P@nSiC%NzzY-AgX6Sp?%Ny2oW@u21w&cZruOaH&NkBnJB7rsOODC(iYvp=3|sXm(72)oYsx0^o=57@%K&wgZp zGhhAe2%{v5v-9karCIvElW;(C$ zbX7HqVCLW7uwrkAnhBD#lYH;F&Shi+*TjfqC*OZBwb8H97?4&Ke3wqq!$X?GIsnI# zPbfqKN%mCKQ)B%$D=~|QOzG_tK}W9OL(6;MJ>Bv63O;0Ee`-ad$qHWNh>6yHGsrGIvH+P*s4he1veL84jVR;~DhLah}g@%7Z$q$`p zP3|t(mkpP=De;PuIFBSB6CzO)daIqE$ZI8weWQbctf+}9%omj=oi9>(91x>LId;SA z6n;s<8EF*{(kW-Ly4fgIZ%a?_p;Kf~m`U(2^I9D?3EW%uOlbaM!wJJM#F5`&mN5hzABfC`BCYi%l(#10~vSf8BAi~aeJUe>PT?p@U7LKx_QLw&%8On z%VX7ET8or6x;@6@-^3U9Jk-%b4uPBLH|NoMhpLHsa!iT$%bH?=5tcrxI+m-47arQ5 zC5vlxC~bMn1f`?Zsmc}`>aMUd*b~b)enhzy#n?BBIrAu?#x})=hh7TQ)!4{v6;5W0D$5* zp*C2-0#&?rLm#$d3#mC+r!z$#3tk$3+gMO~(5u11P4Li>CVAt%edxYpn#l(F3?!m( zgU8G|@F92NpLjm;g>lpN+yu$mKD^_K4I@(8bWpoSfS%3o?IqUB_`7vGb6jooGibNb zb`+|dmHI^ae8ic`O@@Eg?zJ^jWHPC|iEcJ!wGLZ>yX@1wFHa30!tiE8^7`~`3jk%c z_(I6JP}%;|Rh^u87GT0#IgoXV%wz%fweJfAM7v2M;)ZRNpxtSXPFflrVqaYym98iw zt=0lMn);ojIb=A}aN~-P%=ta-lHg;<|D4+GqLe0uzN{6zx$j=mgy9LesQtZQUJDetu7~b@ncshsiZzTK?UlmZyFbD)+>?(uR)c zEn6DMhu=!bT{*VUXegZ`?VhJN52k=sDp-&u39mFk6AHp`_OD3@iRt%@s9+Mr!!$Y8 zMXI}7I%bG&^xnW}MBMS)?LJ`!I3-!^+Lz9p#Rj$yCa9BWS_wt`@i#B_d$yik{F@DA z)|yKv4Hd3^&pHU%*L7_Mv-BGxOj7FSO&)YqMtfH0)=nrcU~+2yn2t9*GlJ(XPw9DO z-tzg8rse!2Wm1lMV9Pp6yI8Fqn&Y2s=qYnipYO*3fKyNod6e&LdtKR6sywyVQuMRv zS#1vIB{zsE(w+a5EfLaSUhNzKhA0ixoebgbS*cU%1eIC>~u%^KG65}NmsZUQ3 z*-UP}DZQ7RCchQj>m$qg`xTH_x~+#fguY=xu~JwNl9!nS?eyD&u_xh{|=vASyi?L2oAnfYxF6*7@b?1_ILF*I07 zMT+-meWg7BKdV$w83e(cA{CtCWOFW3ApJ_36R^MwPXPnRMd}1JukBzj2{-^e^N&^O zT&%U0?l4h8Au(vAdzTGx!HZ*6u#;SWE2zt`Zy+7YDke_=+S?=zV}X>LNW;1uWjVEH z(QD@Oi*)7w5+{F%oUI_prKkGof=c% zazYnJmgz6WDJkA`sgo57gl?19(tP~<{EUo1Ar@2EV)r24$=*FPy-t@1kSePK!n*xJ zr=OSCFegWnhGB^u@sH~F&;;)EAfE(Up(j%hid4bI>+@&ong=kt?H@nV-+q;`J9sFe zN&BaV$SEb@5E|=;Uj)d0dm+%qU-dx|B>>9|)39rm&>;O92gqjZX`}vfsRMH1-W}RS z(UZQ<_uzV1Q6Lq7nDBy3+##4=)=~d`!oZAYMOW;TIZ2M#vI3>|n$Dey&|P&K7h>lW z^;_9ya#{eOP0pB3%+dL85=u&aiX6!yPpv<@h-~}dsG)@8(V2cE#hA}mJ`219nFTwO z@+3FZYW?IFsubIyYKoyX9#{UhOoeadxBwPbf#mMHj)>DK&g4mC=lad3F+Z(Z7PYKe z40uPSr937~Kw19IWhTNqCWPU@v8#Jv&B>UTo)Qbz+w18EZKr;0GjN6^SVXi*9%trfP%a-5e}<@6b`5^u3TycbB)ugU_7tvzB;-> zg5p6GI>8xi)mz*^mfOnhsUXJd3xQei=|j~r!3qBt%7Ex&WRwBuPa|Wj@pC#UVk+q6 z+1Xj}jlXY4BSVkBy|WU_zjL1UP>u}sid8?NljbbEzf$N1L(-2QzOTMybb9YZ;Us4< zDvy6)Cyg9wgbK?lKk)TQGQeFM?=DAkf9TF0)rF8iKEnV{h72kO7?+W&pWvv?yxfdCFPB{mbj z-mEACwbPMHfye``C0Prb8#UN3$UdFe3B>(axlb%@Xmhmoas!+@`AJ|b*BBv8S;Er! zNJ6dFhCw*nUyfRs498i&*6F7>Q~8gQ?OPoT8hu0+>o9?Fc+8-B$N0+rF6(_*Bancs z_0+Tq+@;by#O@IF*-HcxUEQc_dF;&+>fuA8_&>4X7UZkMYV4bxz#iM1{p8+3;Zuix zIkAWNEHr=2F=ty-Oa17a*dF%mRuz~rI>!syO3z$I^=fVF#JOCG8L5Aj_P^dY${+mYRnE{(+iN0BsW#)F+a66Gh?KzqA!@OK=v@LInC^JuJ~H^2kGQR z>sBIkWuKDB^Cf@_QnYVt7yAnK)YqAp`bY;Sa5Wduw-Wyy4c$(ZRM+lSBE$69@A9a$Y zR7}Wh1Yh@X@9?o%HDO4Ch@1O2v_*&Qo$3pVThP?>A2mXq!BebKvIsgy!27hQu&acO zy5T)uO<0%+wQP|76BNY64y@w&R7Q%pPjTvB8?a0i%8OBS7L+quJ=80j-&?>yC z{fR=XAetjuUriUB(VrKQ07kpT3orECFWsdr`Ttux1%++AO}toL@VO8tHQC?%cu7EH zX&zyT&TN<(UAtBEz;Tb?XI0O&J_}Az`K*@I?bcKJ#!9HMiNXbcT~IzB4i-QsE;|?D zYh_h#q*D`4on=A+XYa2V*X_KVR7me6Pd%oxERGBrEUB- z7@2b3JwTfS7PUYTVlZ(QdhLFy7)YynN&Jzy%dDCsbnfqe z4?%TuU*5uM$s=)&t1iRw&Rui9(a6+Lz2z!CGla$zFlo zOe`qilINRCN(6SPd1Aej)F-a!Ti_RqJArE6F*Xc;MbJbCyuXVe@e`49 z`|mzajrmEaPU75z?2(a^%6?U zCcVK|sbxApr!zv42q55Qy5FTUIkz6m=uXGp9VHcJ0P(yzx&!Mhgnje>RRTOw<3K=8~%P`H-J-S&F_rBxYL#Y>Y=@pbx~;1)w@t)QaC%(t}MRHXyFI2oA>Q4-41 zX+Dde5t1%N*SDD#20aZ1+tA(Mo_GlOO7daH)$qCB)O|_T&Y3L9v!3wywcckH2)l-l z>NU}m@!Lvm=zm-_Y!5q7(zl#Jzzx!QDv^e0y%t8EmQ}w&`-d$u_bopD!4$E(EiuV< zdgt1ZODS><0K3`M;0Yxs?Oc0;lH6Sr6)~;yMxQLE{-j15(9&yxc0TgZ!cHPLxi_zI zdl)X6R=610Eo)!QGtP{H?-7m6^}VWb zovpOUd4{kGO*re*`o=S*!|>5lU-Qyzkm;19UO2P7IzChybyQwfLo#I;I?ub%?|$%F zL`9KD`(yvhmqqU!=vHIKX2+H0Y(^-gF>#E(%BV9Ao6}Of2t%lnxyS8U9N)$F@4H@Cq uYhMv@Ay#34U*%VW%NmkMEyl^k{6}@d%2zM<#rK>D{OD>LYP6`KG5-g@*uoV6 literal 0 HcmV?d00001 diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz new file mode 100644 index 0000000..2988d81 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_L.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphL(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Label = 1, + .Pred = 0, + .distance = 800, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 800, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 1, + .distance = 800, + .bearing = 0.0, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 2, + .distance = 800, + .bearing = 1.57, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 4, + .distance = 800, + .bearing = 1.57, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz new file mode 100644 index 0000000..068e5a2 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_O.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphO(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Label = 1, + .Pred = 0, + .distance = 2000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 2000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 1, + .distance = 2000, + .bearing = 1.57, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 1, + .distance = 1414, + .bearing = 3.93, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 2, + .distance = 1414, + .bearing = 0.785, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz new file mode 100644 index 0000000..14e183b --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_P.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphP(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 1, + .distance = 707, + .bearing = 0.79, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 2, + .distance = 1000, + .bearing = 0.0, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz new file mode 100644 index 0000000..171c693 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_Y.bzz @@ -0,0 +1,59 @@ +#Table of the nodes in the graph +m_vecNodes={} + +function Read_GraphY(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 707, + .bearing = 2.36, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 707, + .bearing = 0.79, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 2, + .distance = 707, + .bearing = 2.36, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 3, + .distance = 707, + .bearing = 0.79, + .height = 14000, + .State="UNASSIGNED", + .StateAge=0 +} +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz new file mode 100644 index 0000000..a924696 --- /dev/null +++ b/buzz_scripts/include/taskallocate/graphs/shapes_square.bzz @@ -0,0 +1,60 @@ +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} +function Read_Graph(){ +m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing + .Label = 0, # Label of the point + .Pred = -1, # Label of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Label = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Label = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Label = 3, + .Pred = 0, + .distance = 1000, + .bearing = 3.14, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Label = 4, + .Pred = 0, + .distance = 707, + .bearing = 3.93, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[5] = { + .Label = 5, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} + +} \ No newline at end of file diff --git a/buzz_scripts/include/taskallocate/waypointlist.csv b/buzz_scripts/include/taskallocate/waypointlist.csv new file mode 100644 index 0000000..e0a2c42 --- /dev/null +++ b/buzz_scripts/include/taskallocate/waypointlist.csv @@ -0,0 +1,89 @@ +0,-73.1531935978886,45.5084960903092,15,15 +1,-73.1530989420915,45.5085624255498,15,15 +2,-73.1530042862771,45.5086287608025,15,15 +3,-73.1529096304626,45.5086950960552,15,15 +4,-73.1529458247399,45.5087204611798,15,15 +5,-73.1530404805543,45.5086541259271,15,15 +6,-73.1531351363515,45.5085877906865,15,15 +7,-73.1532297921486,45.508521455446,15,15 +8,-73.1533244479457,45.5084551202054,15,15 +9,-73.1533606422273,45.508480485333,15,15 +10,-73.1532659864302,45.5085468205736,15,15 +11,-73.153171330633,45.5086131558142,15,15 +12,-73.1530766748359,45.5086794910548,15,15 +13,-73.1529820190215,45.5087458263075,15,15 +14,-73.1530182132901,45.5087711914261,15,15 +15,-73.1531128691045,45.5087048561733,15,15 +16,-73.1532075249016,45.5086385209327,15,15 +17,-73.1533021806988,45.5085721856922,15,15 +18,-73.1533968364959,45.5085058504516,15,15 +19,-73.1534330307645,45.5085312155701,15,15 +20,-73.1533383749674,45.5085975508107,15,15 +21,-73.1532437191702,45.5086638860513,15,15 +22,-73.1531490633731,45.5087302212919,15,15 +23,-73.1530544075587,45.5087965565446,15,15 +24,-73.1530906018273,45.5088219216632,15,15 +25,-73.1531852576417,45.5087555864105,15,15 +26,-73.1532799134389,45.5086892511699,15,15 +27,-73.153374569236,45.5086229159293,15,15 +28,-73.1534692250331,45.5085565806887,15,15 +29,-73.1535054193017,45.5085819458072,15,15 +30,-73.1534107635046,45.5086482810478,15,15 +31,-73.1533161077075,45.5087146162884,15,15 +32,-73.1532214519103,45.508780951529,15,15 +33,-73.1531267960959,45.5088472867817,15,15 +34,-73.1531629903645,45.5088726519003,15,15 +35,-73.1532576461789,45.5088063166476,15,15 +36,-73.1533523019761,45.508739981407,15,15 +37,-73.1534469577732,45.5086736461664,15,15 +38,-73.1535416135703,45.5086073109258,15,15 +39,-73.1535778078389,45.5086326760444,15,15 +40,-73.1534831520418,45.5086990112849,15,15 +41,-73.1533884962447,45.5087653465255,15,15 +42,-73.1532938404476,45.5088316817661,15,15 +43,-73.1531991846331,45.5088980170188,15,15 +44,-73.1532353789017,45.5089233821374,15,15 +45,-73.1533300347162,45.5088570468847,15,15 +46,-73.1534246905133,45.5087907116441,15,15 +47,-73.1535193463104,45.5087243764035,15,15 +48,-73.1536140021075,45.5086580411629,15,15 +49,-73.1536501963762,45.5086834062815,15,15 +50,-73.153555540579,45.508749741522,15,15 +51,-73.1534608847819,45.5088160767626,15,15 +52,-73.1533662289848,45.5088824120032,15,15 +53,-73.1532715731703,45.508948747256,15,15 +54,-73.1533077674389,45.5089741123745,15,15 +55,-73.1534024232534,45.5089077771218,15,15 +56,-73.1534970790505,45.5088414418812,15,15 +57,-73.1535917348476,45.5087751066406,15,15 +58,-73.1536863906448,45.5087087714,15,15 +59,-73.1537225849134,45.5087341365186,15,15 +60,-73.1536279291163,45.5088004717592,15,15 +61,-73.1535332733191,45.5088668069997,15,15 +62,-73.153438617522,45.5089331422403,15,15 +63,-73.1533439617076,45.5089994774931,15,15 +64,-73.1533801559762,45.5090248426116,15,15 +65,-73.1534748117906,45.5089585073589,15,15 +66,-73.1535694675877,45.5088921721183,15,15 +67,-73.1536641233849,45.5088258368777,15,15 +68,-73.153758779182,45.5087595016371,15,15 +69,-73.1537949734506,45.5087848667557,15,15 +70,-73.1537003176535,45.5088512019963,15,15 +71,-73.1536056618563,45.5089175372369,15,15 +72,-73.1535110060592,45.5089838724775,15,15 +73,-73.1534163502448,45.5090502077302,15,15 +74,-73.1534525445134,45.5090755728487,15,15 +75,-73.1535472003278,45.509009237596,15,15 +76,-73.1536418561249,45.5089429023554,15,15 +77,-73.1537365119221,45.5088765671148,15,15 +78,-73.1538311677192,45.5088102318742,15,15 +79,-73.1538673619878,45.5088355969928,15,15 +80,-73.1537727061907,45.5089019322334,15,15 +81,-73.1536780503936,45.508968267474,15,15 +82,-73.1535833945964,45.5090346027146,15,15 +83,-73.153488738782,45.5091009379673,15,15 +84,-73.1535249330852,45.5091263031101,15,15 +85,-73.1536195888996,45.5090599678574,15,15 +86,-73.1537142446968,45.5089936326168,15,15 +87,-73.1538089004939,45.5089272973762,15,15 +88,-73.153903556291,45.5088609621356,15,15 diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..8b933ee --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,28 @@ +TIME_TO_SYNC = 100 +sync_timer = 0 +timesync_old_state = 0 +timesync_itr = 0 +timesync_state = 0 + +# Function to sync. algo +function check_time_sync(){ + # sync_timer = sync_timer + 1 + # if(sync_timer < TIME_TO_SYNC){ + # log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + # timesync_state = 1 + # } + # else{ + # timesync_state = 0 + # } + # if(timesync_old_state == 0 and timesync_state == 1){ + # timesync_itr = timesync_itr + 1 + # timesync_old_state = 0 + # } + # timesync_old_state = timesync_state +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz new file mode 100644 index 0000000..4a5c699 --- /dev/null +++ b/buzz_scripts/include/update.bzz @@ -0,0 +1,10 @@ +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +updates_active = 1 +function updated_no_bct(){ + neighbors.broadcast(updated, update_no) +} \ No newline at end of file diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz new file mode 100644 index 0000000..8cef004 --- /dev/null +++ b/buzz_scripts/include/utils/conversions.bzz @@ -0,0 +1,69 @@ +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 +} + +function LimitSpeed(vel_vec, factor){ + if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor) + vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec)) + return vel_vec +} + +# TODO: add other conversions.... +function convert_path(P) { + var pathR={} + if(V_TYPE == 0) { + var n = 1 + while(n <= size(P)){ + pathR[n] = {} + var tmpgoal = gps_from_vec(math.vec2.sub(getvec(P,n),cur_cell)) + pathR[n][1]=tmpgoal.latitude + pathR[n][2]=tmpgoal.longitude + n = n + 1 + } + } + return pathR +} + +# TODO: add other conversions.... +function convert_pt(in) { + if(V_TYPE == 0) + return vec_from_gps(in.x, in.y, 0) +} + +function vec_from_gps(lat, lon, home_ref) { + d_lon = lon - pose.position.longitude + d_lat = lat - pose.position.latitude + if(home_ref == 1) { + d_lon = lon - homegps.long + d_lat = lat - homegps.lat + } + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) +} + +function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing, pose.position.latitude, pose.position.longitude) + latR = pose.position.latitude*math.pi/180.0; + lonR = pose.position.longitude*math.pi/180.0; + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + pose.position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + pose.position.longitude; + + return Lgoal +} diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz new file mode 100644 index 0000000..91ceb63 --- /dev/null +++ b/buzz_scripts/include/utils/string.bzz @@ -0,0 +1,92 @@ +# +# Returns the string character at the given position. +# PARAM s: The string +# PARAM n: The position of the wanted character +# RETURN The character at the wanted position, or nil +# +string.charat = function(s, n) { + return string.sub(s, n, n+1) +} + +# +# Returns the index of the first occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the first match, or nil +# +string.indexoffirst = function(s, m) { + var las = string.length(s) + var lm = string.length(m) + var i = 0 + while(i < las-lm+1) { + if(string.sub(s, i, i+lm) == m) return i + i = i + 1 + } + return nil +} + +# +# Returns the index of the last occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the last match, or nil +# +string.indexoflast = function(s, m) { + var las = string.length(s) + var lm = string.length(m) + var i = las - lm + 1 + while(i >= 0) { + if(string.sub(s, i, i+lm) == m) return i + i = i - 1 + } + return nil +} + +# Splits a string s using the delimiters in d. The string list is +# returned in a table indexed by value (starting at 0). +# PARAM s: The string +# PARAM d: A string containing the delimiters +# RETURN: A table containing the tokens +string.split = function(s, d) { + var i1 = 0 # index to move along s (token start) + var i2 = 0 # index to move along s (token end) + var c = 0 # token count + var t = {} # token list + var las = string.length(s) + var ld = string.length(d) + # Go through string s + while(i2 < las) { + # Try every delimiter + var j = 0 # index to move along d + var f = nil # whether the delimiter was found or not + while(j < ld and (not f)) { + if(string.charat(s, i2) == string.charat(d, j)) { + # Delimiter found + f = 1 + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + c = c + 1 + } + # Start new token + i1 = i2 + 1 + } + else { + # Next delimiter + j = j + 1 + } + } + # Next string character + i2 = i2 + 1 + } + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + } + # Return token list + return t; +} diff --git a/buzz_scripts/include/utils/vec2.bzz b/buzz_scripts/include/utils/vec2.bzz new file mode 100644 index 0000000..ebfce96 --- /dev/null +++ b/buzz_scripts/include/utils/vec2.bzz @@ -0,0 +1,107 @@ +# +# Create a new namespace for vector2 functions +# +math.vec2 = {} + +# +# Creates a new vector2. +# PARAM x: The x coordinate. +# PARAM y: The y coordinate. +# RETURN: A new vector2. +# +math.vec2.new = function(x, y) { + return { .x = x, .y = y } +} + +# +# Creates a new vector2 from polar coordinates. +# PARAM l: The length of the vector2. +# PARAM a: The angle of the vector2. +# RETURN: A new vector2. +# +math.vec2.newp = function(l, a) { + return { + .x = l * math.cos(a), + .y = l * math.sin(a) + } +} + +# +# Calculates the length of the given vector2. +# PARAM v: The vector2. +# RETURN: The length of the vector. +# +math.vec2.length = function(v) { + return math.sqrt(v.x * v.x + v.y * v.y) +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +math.vec2.angle = function(v) { + return math.atan(v.y, v.x) +} + +# +# Returns the normalized form of a vector2. +# PARAM v: The vector2. +# RETURN: The normalized form. +# +math.vec2.norm = function(v) { + var l = math.length(v) + return { + .x = v.x / l, + .y = v.y / l + } +} + +# +# Calculates v1 + v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.add = function(v1, v2) { + return { + .x = v1.x + v2.x, + .y = v1.y + v2.y + } +} + +# +# Calculates v1 - v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.sub = function(v1, v2) { + return { + .x = v1.x - v2.x, + .y = v1.y - v2.y + } +} + +# +# Scales a vector by a numeric constant. +# PARAM v: A vector2. +# PARAM s: A number (float or int). +# RETURN: s * v +# +math.vec2.scale = function(v, s) { + return { + .x = v.x * s, + .y = v.y * s + } +} + +# +# Calculates v1 . v2 (the dot product) +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 . v2 +# +math.vec2.dot = function(v1, v2) { + return v1.x * v2.x + v1.y * v2.y +} diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz new file mode 100644 index 0000000..4c0e06b --- /dev/null +++ b/buzz_scripts/include/vstigenv.bzz @@ -0,0 +1,131 @@ +######################################## +# +# FLEET V.STIGMERGY-RELATED FUNCTIONS +# +######################################## +# +# Constants +# +STATUS_VSTIG = 20 +GROUND_VSTIG = 21 +HIGHEST_ROBOTID = 14 +WAIT4STEP = 10 + +# +# Init var +# +var v_status = {} +var v_ground = {} +b_updating = 0 +vstig_counter = WAIT4STEP + + +function init_swarm() { + s = swarm.create(1) + s.join() +} + +function init_stig() { + v_status = stigmergy.create(STATUS_VSTIG) + v_ground = stigmergy.create(GROUND_VSTIG) +} + +function uav_updatestig() { + # TODO: Push values on update only. + if(vstig_counter<=0) { + b_updating=1 + #var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status} + ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status + log("Pushing ", ls, "on vstig with id:", id) + v_status.put(id, ls) + vstig_counter=WAIT4STEP + } else { + b_updating=0 + vstig_counter=vstig_counter-1 + } +} + +function unpackstatus(recv_value){ + gps=(recv_value-recv_value%1000000)/1000000 + recv_value=recv_value-gps*1000000 + batt=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-batt*1000 + xbee=(recv_value-recv_value%10)/10 + recv_value=recv_value-xbee*10 + fc=recv_value + log("- GPS ", gps) + log("- Battery ", batt) + log("- Xbee ", xbee) + log("- Status ", fc) +} + +function checkusers() { + # Read a value from the structure + if(size(users)>0) + log("Got a user!") + +# log(users) +# users_print(users.dataG) +# if(size(users.dataG)>0) +# vt.put("p", users.dataG) + + # Get the number of keys in the structure +# log("The vstig has ", vt.size(), " elements") +# users_save(vt.get("p")) +# table_print(users.dataL) +} + +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +# printing the contents of a table: a custom function +function usertab_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +function stattab_print() { + if(v_status.size()>0) { + if(b_updating==0) { + u=0 + while(u0) { + if(b_updating==0) { + u=0 + while(u outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + +# initGraph() + + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m + + loop = 1 + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +# Executed at each time step. +function step() { + + # check time sync algorithm state + check_time_sync() + + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="CUSFUN") + statef=cusfun + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE + statef=goinghome + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="AGGREGATE") + statef=aggregate + else if(BVMSTATE=="FORMATION") + statef=formation + else if(BVMSTATE=="PURSUIT") + statef=pursuit + else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ? + statef=resetGraph + else if(BVMSTATE=="GRAPH_FREE") + statef=DoFree + else if(BVMSTATE=="GRAPH_ASKING") + statef=DoAsking + else if(BVMSTATE=="GRAPH_JOINING") + statef=DoJoining + else if(BVMSTATE=="GRAPH_JOINED") + statef=DoJoined + else if(BVMSTATE=="GRAPH_TRANSTOLOCK") + statef=TransitionToLock + else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list + statef=DoLock + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/mainRRT.bzz b/buzz_scripts/mainRRT.bzz new file mode 100644 index 0000000..2563c3e --- /dev/null +++ b/buzz_scripts/mainRRT.bzz @@ -0,0 +1,80 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "plan/rrtstar.bzz" +include "vstigenv.bzz" + +#State launched after takeoff +AUTO_LAUNCH_STATE = "IDLE" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +goal_list = { + .0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5} +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + # nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup. + # BVMSTATE = "TURNEDOFF" + BVMSTATE = "LAUNCHED" +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # graph state machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure + statef=makegraph # or bidding + else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar + statef=rrtstar + else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar + statef=navigate + else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure + statef=follow + else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure + statef=take_picture + + statef() + + log("Current state: ", BVMSTATE) + +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz new file mode 100644 index 0000000..a49329b --- /dev/null +++ b/buzz_scripts/minimal.bzz @@ -0,0 +1,63 @@ +include "update.bzz" +include "act/states.bzz" +include "vstigenv.bzz" + +# State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" + +##### +# Vehicule type: +# 0 -> outdoor flying vehicle +# 1 -> indoor flying vehicle +# 2 -> outdoor wheeled vehicle +# 3 -> indoor wheeled vehicle +V_TYPE = 0 + +# Executed once at init time. +function init() { + init_swarm() + + TARGET_ALTITUDE = 25.0 # m + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +function action() { + BVMSTATE = "ACTION" + # do some actions.... +} + +# Executed at each time step. +function step() { + # listen to Remote Controller + rc_cmd_listen() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action + + statef() + log("Current state: ", BVMSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz new file mode 100644 index 0000000..c605089 --- /dev/null +++ b/buzz_scripts/stand_by.bzz @@ -0,0 +1,38 @@ +include "act/states.bzz" +include "vstigenv.bzz" + +updated="update_ack" +update_no=0 +BVMSTATE = "UPDATESTANDBY" + +# Executed once at init time. +function init(){ + barrier = stigmergy.create(101) + barrier.put(id,1) + barrier_val=0 + barrier.onconflict(function (k,l,r) { + if(r. data < l. data or (r. data == l. data )) return l + else return r + }) + init_swarm() + # start the swarm command listener + nei_cmd_listen() +} + +function stand_by(){ + barrier.get(id) + barrier_val = barrier.size() + + neighbors.listen(updated, + function(vid, value, rid) { + if(value==update_no) barrier.put(rid,1) + } +) + +} + +# Executed at each time step. +function step() { + stand_by() + +} diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100755 index 0000000..5240618 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -0,0 +1,72 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +V_TYPE = 0 + +#State launched after takeoff +AUTO_LAUNCH_STATE = "FORMATION" + + +TARGET = 8.0 +EPSILON = 3.0 +GOTO_MAXVEL = 2.5 # m/steps + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # start the swarm command listener + nei_cmd_listen() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" + TAKEOFF_COUNTER = 20 +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="FORMATION") + statef=formation + + statef() + + log("Current state: ", BVMSTATE) + + # Auto-takeoff (delayed for simulator boot) + if(id == 0) { + if(TAKEOFF_COUNTER>0) + TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1 + else if(TAKEOFF_COUNTER == 0) { + BVMSTATE="LAUNCH" + TAKEOFF_COUNTER = -1 + } + } +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz new file mode 100644 index 0000000..70531f4 --- /dev/null +++ b/buzz_scripts/testaloneWP.bzz @@ -0,0 +1,57 @@ +include "update.bzz" +# don't use a stigmergy id=11 with this header, for barrier +# it requires an 'action' function to be defined here. +include "act/states.bzz" +include "vstigenv.bzz" + +#State launched after takeoff +AUTO_LAUNCH_STATE = "ACTION" + +function action() { + BVMSTATE = "ACTION" + uav_storegoal(-1.0,-1.0,-1.0) + goto_gps(picture) +} + +# Executed once at init time. +function init() { + init_stig() + init_swarm() + + # Starting state: TURNEDOFF to wait for user input. + BVMSTATE = "TURNEDOFF" +} + +# Executed at each time step. +function step() { + rc_cmd_listen() + + # update the vstig (status/net/batt/...) + # uav_updatestig() + + # + # State machine + # + if(BVMSTATE=="TURNEDOFF") + statef=turnedoff + else if(BVMSTATE=="STOP") # ends on turnedoff + statef=stop + else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE + statef=launch + else if(BVMSTATE=="IDLE") + statef=idle + else if(BVMSTATE=="ACTION") + statef=action + + statef() + + log("Current state: ", BVMSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/include/buzz_update.h b/include/buzz_update.h new file mode 100644 index 0000000..4b43e86 --- /dev/null +++ b/include/buzz_update.h @@ -0,0 +1,171 @@ +#ifndef BUZZ_UPDATE_H +#define BUZZ_UPDATE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define delete_p(p) \ + do \ + { \ + free(p); \ + p = NULL; \ + } while (0) +namespace buzz_update +{ + static const uint16_t CODE_REQUEST_PADDING = 250; + static const uint16_t MIN_UPDATE_PACKET = 251; + static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; + static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; + /*********************/ + /* Updater states */ + /********************/ + + typedef enum { + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update + } code_states_e; + + /*********************/ + /*Message types */ + /********************/ + + typedef enum { + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request + } code_message_e; + + /*************************/ + /*Updater message queue */ + /*************************/ + + struct updater_msgqueue_s + { + uint8_t* queue; + uint8_t* size; + }; + typedef struct updater_msgqueue_s* updater_msgqueue_t; + + struct updater_code_s + { + uint8_t* bcode; + uint8_t* bcode_size; + }; + typedef struct updater_code_s* updater_code_t; + + /**************************/ + /*Updater data*/ + /**************************/ + + struct buzz_updater_elem_s + { + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; + }; + typedef struct buzz_updater_elem_s* buzz_updater_elem_t; + + /**************************************************************************/ + /*Updater routine from msg processing to file checks to be called from main*/ + /**************************************************************************/ + void update_routine(); + + /************************************************/ + /*Initalizes the updater */ + /************************************************/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); + + /*********************************************************/ + /*Appends buffer of given size to in msg queue of updater*/ + /*********************************************************/ + + void code_message_inqueue_append(uint8_t* msg, uint16_t size); + + /*********************************************************/ + /*Processes messages inside the queue of the updater*/ + /*********************************************************/ + + void code_message_inqueue_process(); + + /*****************************************************/ + /*Obtains messages from out msgs queue of the updater*/ + /*******************************************************/ + uint8_t* getupdater_out_msg(); + + /******************************************************/ + /*Obtains out msg queue size*/ + /*****************************************************/ + uint8_t* getupdate_out_msg_size(); + + /**************************************************/ + /*Destroys the out msg queue*/ + /*************************************************/ + void destroy_out_msg_queue(); + + // buzz_updater_elem_t get_updater(); + /***************************************************/ + /*Sets bzz file name*/ + /***************************************************/ + void set_bzz_file(const char* in_bzz_file, bool dbg); + + /****************************************************/ + /*Tests the code from a buffer*/ + /***************************************************/ + + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); + + /****************************************************/ + /*Destroys the updater*/ + /***************************************************/ + + void destroy_updater(); + + /****************************************************/ + /*Checks for updater message*/ + /***************************************************/ + + int is_msg_present(); + + /****************************************************/ + /*Compiles a bzz script to bo and bdbg*/ + /***************************************************/ + + int compile_bzz(std::string bzz_file); + + /****************************************************/ + /*Set number of robots in the updater*/ + /***************************************************/ + + void updates_set_robots(int robots); +} +#endif diff --git a/include/buzz_utility.h b/include/buzz_utility.h new file mode 100644 index 0000000..50132fe --- /dev/null +++ b/include/buzz_utility.h @@ -0,0 +1,108 @@ +#pragma once +#include +#include "buzz_utility.h" +#include "buzzuav_closures.h" +#include "buzz_update.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +namespace buzz_utility +{ +struct pos_struct +{ + double x, y, z; + pos_struct(double x, double y, double z) : x(x), y(y), z(z){}; + pos_struct() + { + } +}; +typedef struct pos_struct Pos_struct; +struct rb_struct +{ + double r, b, latitude, longitude, altitude; + rb_struct(double la, double lo, double al, double r, double b) + : latitude(la), longitude(lo), altitude(al), r(r), b(b){}; + rb_struct() + { + } +}; +typedef struct rb_struct RB_struct; + +struct neiStatus +{ + uint gps_strenght = 0; + uint batt_lvl = 0; + uint xbee = 0; + uint flight_status = 0; +}; +typedef struct neiStatus neighbors_status; + +struct neitime +{ + uint64_t nei_hardware_time; + uint64_t nei_logical_time; + uint64_t node_hardware_time; + uint64_t node_logical_time; + double nei_rate; + double relative_rate; + int age; + neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) + : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), + nei_rate(nr), relative_rate(mr) {}; + neitime() + { + } +}; +typedef struct neitime neighbor_time; + +uint16_t* u64_cvt_u16(uint64_t u64); + +int buzz_listen(const char* type, int msg_size); +int make_table(buzzobj_t* t); +int buzzusers_reset(); +int create_stig_tables(); + +void in_msg_append(uint64_t* payload); + +uint64_t* obt_out_msg(); + +void update_sensors(); + +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); + +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); + +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); + +void buzz_script_step(); + +void buzz_script_destroy(); + +int buzz_script_done(); + +int update_step_test(); + +int get_robotid(); + +int get_swarmsize(); + +buzzvm_t get_vm(); + +void set_robot_var(int ROBOTS); + +int get_inmsg_size(); + +std::vector get_inmsg_vector(); + +std::string get_bvmstate(); + +int get_timesync_state(); + +int get_timesync_itr(); +} diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h new file mode 100644 index 0000000..71b99f1 --- /dev/null +++ b/include/buzzuav_closures.h @@ -0,0 +1,183 @@ +#pragma once + +#include +#include +#include "mavros_msgs/CommandCode.h" +#include "mavros_msgs/Mavlink.h" +#include "ros/ros.h" +#include "buzz_utility.h" +#include "rosbuzz/mavrosCC.h" + +#define EARTH_RADIUS (double)6371000.0 +#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) +#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI))) + +namespace buzzuav_closures +{ +/* + * prextern int() function in Buzz + * This function is used to print data from buzz + * The command to use in Buzz is buzzros_print takes any available datatype in Buzz + */ +int buzzros_print(buzzvm_t vm); +void setWPlist(std::string path); + +/* + * closure to move following a vector + */ +int buzzuav_moveto(buzzvm_t vm); +/* + * closure to store a new GPS goal + */ +int buzzuav_storegoal(buzzvm_t vm); +/* + * closure to control the gimbal + */ +int buzzuav_setgimbal(buzzvm_t vm); +/* + * parse a csv list of waypoints + */ +void parse_gpslist(); +/* + * closure to export a 2D map + */ +int buzz_exportmap(buzzvm_t vm); +/* + * closure to take a picture + */ +int buzzuav_takepicture(buzzvm_t vm); +/* + * Returns the current command from local variable + */ +int getcmd(); +/* + * Sets goto position from rc client + */ +void rc_set_goto(int id, double latitude, double longitude, double altitude); +/* + *Sets gimbal orientation from rc client + */ +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); +/* + * sets rc requested command + */ +void rc_call(int rc_cmd); +/* + * sets the battery state + */ +void set_battery(float voltage, float current, float remaining); +/* + * sets the xbee network status + */ +void set_deque_full(bool state); +void set_rssi(float value); +void set_raw_packet_loss(float value); +void set_filtered_packet_loss(float value); +// void set_api_rssi(float value); +/* + * sets current position + */ + +void set_currentNEDpos(double x, double y); + +void set_currentpos(double latitude, double longitude, float altitude, float yaw); +/* + * returns the current go to position + */ +double* getgoto(); +/* + * returns the current grid + */ +std::map> getgrid(); + + +/* + * returns the gimbal commands + */ +float* getgimbal(); +/* + *updates flight status + */ +void flight_status_update(uint8_t state); +/* + *Update neighbors table + */ +void neighbour_pos_callback(int id, float range, float bearing, float elevation); +/* + * update neighbors from in msgs + */ +void update_neighbors(buzzvm_t vm); +/* + *Clear neighbours struct + */ +void clear_neighbours_pos(); +/* + * closure to add a neighbor status + */ +int buzzuav_addNeiStatus(buzzvm_t vm); +/* + * returns the current array of neighbors status + */ +mavros_msgs::Mavlink get_status(); + +/* + *Flight status + */ +void set_obstacle_dist(float dist[]); + +/* + * Commands the UAV to takeoff + */ +int buzzuav_takeoff(buzzvm_t vm); +/* + * Arm command from Buzz + */ +int buzzuav_arm(buzzvm_t vm); +/* + * Disarm from buzz + */ +int buzzuav_disarm(buzzvm_t vm); +/* Commands the UAV to land + */ +int buzzuav_land(buzzvm_t vm); + +/* + * Command the UAV to go to home location + */ +int buzzuav_gohome(buzzvm_t vm); + +/* + * Updates battery information in Buzz + */ +int buzzuav_update_battery(buzzvm_t vm); +/* + * Updates xbee_status information in Buzz + */ +int buzzuav_update_xbee_status(buzzvm_t vm); +/* + * Updates current position in Buzz + */ +int buzzuav_update_currentpos(buzzvm_t vm); +/* + * add new target in the BVM + */ +int buzzuav_addtargetRB(buzzvm_t vm); +/* + * Updates flight status and rc command in Buzz, put it in a tabel to acess it + * use flight.status for flight status + * use flight.rc_cmd for current rc cmd + */ +int buzzuav_update_flight_status(buzzvm_t vm); + +/* + * Updates IR information in Buzz + * Proximity and ground sensors to do !!!! + */ +int buzzuav_update_prox(buzzvm_t vm); +/* + * returns the current FC command + */ +int bzz_cmd(); + +int dummy_closure(buzzvm_t vm); +} diff --git a/include/rosbuzz/mavrosCC.h b/include/rosbuzz/mavrosCC.h new file mode 100644 index 0000000..7917bac --- /dev/null +++ b/include/rosbuzz/mavrosCC.h @@ -0,0 +1,24 @@ +#pragma once + +#if MAVROSKINETIC + +const short MISSION_START = mavros_msgs::CommandCode::MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; + +#else + +const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START; +const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; +const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM; + +#endif + +const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF; +const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND; +const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; +const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT; +const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT; +const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE; +const short CMD_REQUEST_UPDATE = 666; +const short CMD_SYNC_CLOCK = 777; diff --git a/include/roscontroller.h b/include/roscontroller.h new file mode 100644 index 0000000..d7bbbc5 --- /dev/null +++ b/include/roscontroller.h @@ -0,0 +1,309 @@ +#pragma once +#include +#include +#include +#include +#include +#include "mavros_msgs/GlobalPositionTarget.h" +#include "mavros_msgs/CommandCode.h" +#include "mavros_msgs/CommandLong.h" +#include "mavros_msgs/CommandBool.h" +#include "mavros_msgs/ExtendedState.h" +#include "mavros_msgs/SetMode.h" +#include "mavros_msgs/State.h" +#include "mavros_msgs/BatteryStatus.h" +#include "sensor_msgs/BatteryState.h" +#include "mavros_msgs/Mavlink.h" +#include "mavros_msgs/PositionTarget.h" +#include "sensor_msgs/NavSatStatus.h" +#include "mavros_msgs/WaypointPush.h" +#include "mavros_msgs/Waypoint.h" +#include "mavros_msgs/PositionTarget.h" +#include "mavros_msgs/StreamRate.h" +#include "mavros_msgs/ParamGet.h" +#include "geometry_msgs/PoseStamped.h" +#include "std_msgs/Float64.h" +#include "std_msgs/String.h" +#include +#include +#include +#include +#include "buzz_utility.h" +#include +#include +#include +#include +#include +#include +#include "buzzuav_closures.h" +#include "rosbuzz/mavrosCC.h" + +/* +* ROSBuzz message types +*/ +typedef enum { + ROS_BUZZ_MSG_NIL = 0, // dummy msg + UPDATER_MESSAGE, // Update msg + 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 MOVING_AVERAGE_ALPHA 0.1 +#define MAX_NUMBER_OF_ROBOTS 10 + +#define TIMEOUT 60 +#define BUZZRATE 10 + +using namespace std; + +namespace rosbuzz_node +{ +class roscontroller +{ +public: + roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); + ~roscontroller(); + void RosControllerRun(); + + static const string CAPTURE_SRV_DEFAULT_NAME; + +private: + struct num_robot_count + { + uint8_t history[10]; + uint8_t index = 0; + uint8_t current = 0; + num_robot_count() + { + } + }; + typedef struct num_robot_count Num_robot_count; + + Num_robot_count count_robots; + + struct POSE + { + double longitude = 0.0; + double latitude = 0.0; + float altitude = 0.0; + // NED coordinates + float x = 0.0; + float y = 0.0; + float z = 0.0; + float yaw = 0.0; + }; + typedef struct POSE ros_pose; + + ros_pose target, home, cur_pos; + + struct MsgData + { + int msgid; + uint16_t nid; + uint16_t size; + double sent_time; + uint64_t received_time; + 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; + + uint64_t payload; + std::map neighbours_pos_map; + std::map raw_neighbours_pos_map; + std::map neighbours_time_map; + int timer_step = 0; + int robot_id = 0; + ros::Time logical_clock; + ros::Time previous_step_time; + std::vector inmsgdata; + uint64_t out_msg_time; + double logical_time_rate; + bool time_sync_jumped; + std::string robot_name = ""; + + int rc_cmd; + float fcu_timeout; + int armstate; + int barrier; + int update; + int message_number = 0; + uint8_t no_of_robots = 0; + bool rcclient; + bool xbeeplugged = false; + 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; + std::string modeclient; + std::string rcservice_name; + std::string bcfname, dbgfname; + std::string out_payload; + std::string in_payload; + std::string stand_by; + std::string xbeesrv_name; + std::string capture_srv_name; + std::string setpoint_name; + std::string stream_client_name; + std::string setpoint_nonraw; + + // ROS service, publishers and subscribers + ros::ServiceClient mav_client; + ros::ServiceClient xbeestatus_srv; + ros::ServiceClient capture_srv; + ros::ServiceClient stream_client; + ros::Publisher payload_pub; + ros::Publisher MPpayload_pub; + ros::Publisher neigh_pos_pub; + ros::Publisher bvmstate_pub; + ros::Publisher grid_pub; + ros::Publisher localsetpoint_nonraw_pub; + ros::ServiceServer service; + ros::Subscriber current_position_sub; + ros::Subscriber users_sub; + ros::Subscriber battery_sub; + ros::Subscriber payload_sub; + ros::Subscriber flight_estatus_sub; + ros::Subscriber flight_status_sub; + ros::Subscriber obstacle_sub; + ros::Subscriber Robot_id_sub; + ros::Subscriber relative_altitude_sub; + ros::Subscriber local_pos_sub; + + std::map m_smTopic_infos; + + int setpoint_counter; + + std::ofstream log; + + // Commands for flight controller + mavros_msgs::CommandLong cmd_srv; + + mavros_msgs::CommandBool m_cmdBool; + ros::ServiceClient arm_client; + + mavros_msgs::SetMode m_cmdSetMode; + ros::ServiceClient mode_client; + + // Initialize publisher and subscriber, done in the constructor + void Initialize_pub_sub(ros::NodeHandle& n_c); + + std::string current_mode; + + /*Obtain data from ros parameter server*/ + void Rosparameters_get(ros::NodeHandle& n_c_priv); + + /*compiles buzz script from the specified .bzz file*/ + std::string Compile_bzz(std::string bzzfile_name); + + /*Flight controller service call*/ + void flight_controller_service_call(); + + /*Neighbours pos publisher*/ + void neighbours_pos_publisher(); + + /*UAVState publisher*/ + void state_publisher(); + + /*Grid publisher*/ + void grid_publisher(); + + /*BVM message payload publisher*/ + void send_MPpayload(); + + /*Prepare messages and publish*/ + void prepare_msg_and_publish(); + + /*Refresh neighbours Position for every ten step*/ + void maintain_pos(int tim_step); + + /*Puts neighbours position inside neigbours_pos_map*/ + void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); + + /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ + void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr); + + /*Set the current position of the robot callback*/ + void set_cur_pos(double latitude, double longitude, double altitude); + + /*convert from spherical to cartesian coordinate system callback */ + float constrainAngle(float x); + void gps_rb(POSE nei_pos, double out[]); + void gps_ned_cur(float& ned_x, float& ned_y, POSE t); + void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, + double gps_r_lat); + + /*battery status callback */ + void battery(const sensor_msgs::BatteryState::ConstPtr& msg); + + /*flight extended status callback*/ + void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); + + /*flight status callback*/ + void flight_status_update(const mavros_msgs::State::ConstPtr& msg); + + /*current position callback*/ + void global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg); + + /*current relative altitude callback*/ + void rel_alt_callback(const std_msgs::Float64::ConstPtr& msg); + + /*payload callback callback*/ + void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); + + /* RC commands service */ + bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res); + + /*robot id sub callback*/ + void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); + + /*Obstacle distance table callback*/ + void obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg); + + /*Get publisher and subscriber from YML file*/ + void GetSubscriptionParameters(ros::NodeHandle& node_handle); + + /*Arm/disarm method that can be called from buzz*/ + void Arm(); + + /*set mode like guided for solo*/ + void SetMode(std::string mode, int delay_miliseconds); + + /*Robot independent subscribers*/ + void Subscribe(ros::NodeHandle& n_c); + + void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); + + void fc_command_setup(); + + void SetLocalPosition(float x, float y, float z, float yaw); + + void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + + void SetStreamRate(int id, int rate, int on_off); + + void get_number_of_robots(); + + // functions related to Xbee modules information + void GetRobotId(); + bool GetDequeFull(bool& result); + bool GetRssi(float& result); + bool TriggerAPIRssi(const uint8_t short_id); + bool GetAPIRssi(const uint8_t short_id, float& result); + bool GetRawPacketLoss(const uint8_t short_id, float& result); + bool GetFilteredPacketLoss(const uint8_t short_id, float& result); + void get_xbee_status(); + + void time_sync_step(); + void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); + uint64_t get_logical_time(); + void set_logical_time_correction(uint64_t cor); +}; +} diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml new file mode 100644 index 0000000..c18075b --- /dev/null +++ b/launch/launch_config/solo.yaml @@ -0,0 +1,12 @@ +topics: + gps : mavros/global_position/global + battery : mavros/battery + status : mavros/state + fcclient: mavros/cmd/command + setpoint: mavros/setpoint_position/local + armclient: mavros/cmd/arming + modeclient: mavros/set_mode + localpos: mavros/local_position/pose + stream: mavros/set_stream_rate + altitude: mavros/global_position/rel_alt + diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml new file mode 100644 index 0000000..9e33d37 --- /dev/null +++ b/launch/launch_config/topics.yaml @@ -0,0 +1,13 @@ +topics: + gps : global_position/global + battery : battery + status : state + estatus : extended_state + fcclient: cmd/command + setpoint: setpoint_position/local + armclient: cmd/arming + modeclient: set_mode + localpos: local_position/pose + stream: set_stream_rate + altitude: global_position/rel_alt + obstacles: obstacles diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch new file mode 100644 index 0000000..63c7906 --- /dev/null +++ b/launch/rosbuzz.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch new file mode 100644 index 0000000..36fa979 --- /dev/null +++ b/launch/rosbuzzd.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh new file mode 100644 index 0000000..bea3e11 --- /dev/null +++ b/misc/cmdlinectr.sh @@ -0,0 +1,44 @@ +#! /bin/bash +function takeoff { + rosservice call $1/buzzcmd 0 22 0 0 0 0 0 0 0 0 +} +function land { + rosservice call $1/buzzcmd 0 21 0 0 0 0 0 0 0 0 +} +function arm { + rosservice call $1/buzzcmd 0 400 0 1 0 0 0 0 0 0 +} +function disarm { + rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 +} +function timesync { + rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0 +} +function testWP { + rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 +} +function record { + rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info + +} +function clean { + sudo rm /var/log/upstart/robot* + sudo rm /var/log/upstart/dji* + sudo rm /var/log/upstart/x3s* +} +function startrobot { + sudo service dji start +} +function stoprobot { + sudo service dji stop +} +function updaterobot { +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch + rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXHeavenbuzzy.launch +# rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXxbeebuzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch +} +function uavstate { + let "a = $1 + 900" + rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0 +} diff --git a/msg/neigh_pos.msg b/msg/neigh_pos.msg new file mode 100644 index 0000000..8db4257 --- /dev/null +++ b/msg/neigh_pos.msg @@ -0,0 +1,2 @@ +Header header +sensor_msgs/NavSatFix[] pos_neigh diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..0fa6866 --- /dev/null +++ b/package.xml @@ -0,0 +1,61 @@ + + + rosbuzz + 0.1.0 + The rosbuzz package + + + + + vivek + + + + + + MIT + + + + + + + + + + + + vivek + + + + + + + + + + + + + + catkin + roscpp + roscpp + std_msgs + std_msgs + mavros_msgs + mavros_msgs + sensor_msgs + sensor_msgs + nav_msgs + nav_msgs + message_generation + message_runtime + + + + + + + diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..d61cdb4 --- /dev/null +++ b/readme.md @@ -0,0 +1,111 @@ +ROS Implemenation of Buzz +========================= + +What is Buzz? +============= + +Buzz is a novel programming language for heterogeneous robots swarms. + +Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion. + +Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations. + +Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm. + +Self-organization results from the fact that the Buzz run-time platform is purely distributed. + +The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS. + +More information is available at http://the.swarming.buzz/wiki/doku.php?id=start. + +Description: +============ + +Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS. + + +Downloading ROS Package +======================= + + $ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz + +Requirements +============ + +* Buzz : + +You can download the development sources through git: + + $ git clone https://github.com/MISTLab/Buzz.git buzz + +* ROS binary distribution (Indigo or higher) with catkin (could be used with older versions of ROS with catkin but not tested) + + +You need the following package: + +* mavros_msgs : + +You can install using apt-get: + + $ sudo apt-get install ros--mavros ros--mavros-extras + +Compilation +=========== + +To compile the ros package, execute the following: + + $ cd catkin_ws + $ catkin_make + $ source devel/setup.bash + +Run +=== +To run the ROSBuzz package using the launch file, execute the following: + + $ roslaunch rosbuzz rosbuzz.launch + +Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch). + +* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script. + +Publisher +========= + +* Messages from Buzz (BVM): +The package publishes mavros_msgs/Mavlink message "outMavlink". + +* Command to the flight controller: +The package publishes geometry_msgs/PoseStamped message "setpoint_position/local". + +Subscribers +----------- + +* Current position of the Robot: +The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose". + +* Messages to Buzz (BVM): +The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink". + +* Status: +The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state". + +Service +------- + +* Remote Controller: +The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line. + +References +------ +* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 + +* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. + +Visual Studio Code +-------------------- +To activate highlights of the code in Visual Studio Code or Roboware add the following to settings.json: +``` + "files.associations": { + "*.bzz":"python" + } +``` diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp new file mode 100644 index 0000000..8624c3a --- /dev/null +++ b/src/buzz_update.cpp @@ -0,0 +1,724 @@ +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + +#include "buzz_update.h" + +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; + + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; + + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + if(debug) ROS_INFO("Initializing file monitor..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) + { + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; + + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } + } + /*Check for .bzz file chages*/ + int check_update() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /*Update index to start of next event*/ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file and call bsdiff to patch*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + std::stringstream patch_exsisting; + patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 + << "tmp_patch.bo"; + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + if(debug){ + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + if(debug) { + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter); + } + + void code_message_inqueue_append(uint8_t* msg, uint16_t size) + { + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + if(debug) { + ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode)); + ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + if(debug) ROS_WARN("Received patch PASSED tests!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } + } + } + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) + { + if(debug) ROS_WARN("Patch rebroadcast request received."); + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + { + size += sizeof(uint16_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + { + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + + void create_update_patch() + { + std::stringstream genpatch; + + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + /*Launch bsdiff and create a patch*/ + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) + { + ROS_ERROR("Error in compiling script, resuming old script."); + } + else + { + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + if(debug) ROS_INFO("Broadcasting patch ..."); + code_message_outqueue_append(); + } + delete_p(BO_BUF); + } + } + } + + else + { + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) + { + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, rolling back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; + } + } + } + + uint8_t* getupdater_out_msg() + { + return (uint8_t*)updater->outmsg_queue->queue; + } + + uint8_t* getupdate_out_msg_size() + { + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; + } + + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) + { + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) + { + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) + { + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + if(debug) ROS_WARN("Step test passed"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; + } + /*Unable to step something wrong*/ + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + } + else + { + /*You will never reach here*/ + ROS_ERROR("Step test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + } + return 0; + } + } + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("Initialization test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); + } + else + { + /*You will never reach here*/ + ROS_ERROR("Initialization test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + } + return 0; + } + } + + void destroy_out_msg_queue() + { + // fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready = 0; + } + + int is_msg_present() + { + return updater_msg_ready; + } + /*buzz_updater_elem_t get_updater() + { + return updater; + }*/ + void destroy_updater() + { + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); + } + + void set_bzz_file(const char* in_bzz_file, bool dbg) + { + debug=dbg; + bzz_file = in_bzz_file; + } + + void updates_set_robots(int robots) + { + no_of_robot = robots; + } + + /*-------------------------------------------------------- + / Create Buzz bytecode from the bzz script input + /-------------------------------------------------------*/ + int compile_bzz(std::string bzz_file) + { + /*Compile the buzz code .bzz to .bo*/ + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + 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/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp new file mode 100644 index 0000000..42b5f57 --- /dev/null +++ b/src/buzz_utility.cpp @@ -0,0 +1,626 @@ +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + +#include "buzz_utility.h" + +namespace buzz_utility +{ +/****************************************/ +/****************************************/ + +static buzzvm_t VM = 0; +static char* BO_FNAME = 0; +static uint8_t* BO_BUF = 0; +static buzzdebug_t DBG_INFO = 0; +static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets +static uint8_t Robot_id = 0; +static std::vector IN_MSG; +std::map users_map; + +/****************************************/ + +uint16_t* u64_cvt_u16(uint64_t u64) +/* +/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user +------------------------------------------------------------------------*/ +{ + uint16_t* out = new uint16_t[4]; + uint32_t int32_1 = u64 & 0xFFFFFFFF; + uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000)) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000)) >> 16; + // DEBUG + // cout << " values " < 0 && unMsgSize <= size - tot) + { + buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize)); + tot += unMsgSize; + } + } while (size - tot > sizeof(uint16_t) && unMsgSize > 0); + free(first_INmsg); + IN_MSG.erase(IN_MSG.begin()); + } + // Process messages VM call* + buzzvm_process_inmsgs(VM); +} + +uint64_t* obt_out_msg() +/* +/ Obtains messages from buzz out message Queue +-------------------------------------------------*/ +{ + // Process out messages + buzzvm_process_outmsgs(VM); + uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); + memset(buff_send, 0, MAX_MSG_SIZE); + // Taking into consideration the sizes included at the end + ssize_t tot = sizeof(uint16_t); + // Send robot id + *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; + tot += sizeof(uint16_t); + // Send messages from FIFO + do + { + // Are there more messages? + if (buzzoutmsg_queue_isempty(VM)) + break; + // Get first message + buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); + // Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes + // DEBUG + // ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); + if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) + { + buzzmsg_payload_destroy(&m); + break; + } + + // Add message length to data buffer + *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); + tot += sizeof(uint16_t); + + // Add payload to data buffer + memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); + tot += buzzmsg_payload_size(m); + + // Get rid of message + buzzoutmsg_queue_next(VM); + buzzmsg_payload_destroy(&m); + } while (1); + + uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t))); + *(uint16_t*)buff_send = (uint16_t)total_size; + + uint64_t* payload_64 = new uint64_t[total_size]; + + memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); + free(buff_send); + // DEBUG + // for(int i=0;ipc); + char* msg; + if (dbg != NULL) + { + asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname, + dbg->line, dbg->col, VM->errormsg); + } + else + { + asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg); + } + return msg; +} + +static int buzz_register_hooks() +/* +/ Buzz hooks that can be used inside .bzz file +------------------------------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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, "storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); + buzzvm_gstore(VM); + 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, "takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); + buzzvm_gstore(VM); + 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, "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, "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)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap)); + buzzvm_gstore(VM); + + return VM->state; +} + +static int testing_buzz_register_hooks() +/* +/ Register dummy Buzz hooks for test during update +---------------------------------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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, "storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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, "takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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, "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, "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)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + + return VM->state; +} + +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) +/* +/ Sets the .bzz and .bdbg file +---------------------------------*/ +{ + ROS_INFO(" Robot ID: %i", robot_id); + Robot_id = robot_id; + // Read bytecode from file and fill the bo buffer + FILE* fd = fopen(bo_filename, "rb"); + if (!fd) + { + perror(bo_filename); + return 0; + } + fseek(fd, 0, SEEK_END); + size_t bcode_size = ftell(fd); + rewind(fd); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size) + { + perror(bo_filename); + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + fclose(fd); + return 0; + } + fclose(fd); + + // Save bytecode file name + BO_FNAME = strdup(bo_filename); + + return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); +} + +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Sets a new update +-----------------------*/ +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); + return 0; + } + // Register hook functions + if (buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // Execute the global part of the script + if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE) + { + ROS_ERROR("Error executing global part, VM state : %i", VM->state); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Performs a initialization test +-----------------------------------*/ +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); + return 0; + } + // Register hook functions + if (testing_buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // Execute the global part of the script + if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE) + { + ROS_ERROR("Error executing global part, VM state : %i", VM->state); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +struct buzzswarm_elem_s +/* +/ Swarm struct +----------------*/ +{ + buzzdarray_t swarms; + uint16_t age; +}; +typedef struct buzzswarm_elem_s* buzzswarm_elem_t; + +void update_sensors() +/* +/ Update from all external inputs +-------------------------------*/ +{ + // Update sensors + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_xbee_status(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); +} + +void buzz_script_step() +/* +/ Step through the buzz script +-------------------------------*/ +{ + // Process available messages + in_message_process(); + // Update sensors + update_sensors(); + // Call Buzz step() function + if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } +} + +void buzz_script_destroy() +/* +/ Destroy the bvm and other related ressources +-------------------------------------*/ +{ + if (VM) + { + if (VM->state != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } + buzzvm_function_call(VM, "destroy", 0); + buzzvm_destroy(&VM); + free(BO_FNAME); + buzzdebug_destroy(&DBG_INFO); + } + ROS_INFO("Script execution stopped."); +} + +int buzz_script_done() +/* +/ Check if the BVM execution terminated +---------------------------------------*/ +{ + return VM->state != BUZZVM_STATE_READY; +} + +int update_step_test() +/* +/ Step test for the update mechanism +------------------------------------*/ +{ + // Process available messages + in_message_process(); + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + + int a = buzzvm_function_call(VM, "step", 0); + + if (a != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info()); + fprintf(stdout, "step test VM state %i\n", a); + } + + return a == BUZZVM_STATE_READY; +} + +void set_robot_var(int ROBOTS) +/* +/ set swarm size in the BVM +-----------------------------*/ +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); +} + +int get_inmsg_size() +/* +/ get the incoming msgs size +------------------------------*/ +{ + return IN_MSG.size(); +} + +std::vector get_inmsg_vector(){ + return IN_MSG; +} + +buzzvm_t get_vm() +/* +/ return the BVM +----------------*/ +{ + return VM; +} + +string get_bvmstate() +/* +/ return current BVM state +---------------------------------------*/ +{ + std::string uav_state = "Unknown"; + if(VM && VM->strings){ + 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) + 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 +--------------------------------------*/ +{ + int timesync_state = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value; + buzzvm_pop(VM); + } + return timesync_state; +} +int get_timesync_itr() +/* +/ return time sync iteration from bzz script +--------------------------------------*/ +{ + int timesync_itr = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value; + buzzvm_pop(VM); + } + return timesync_itr; +} + +} diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp new file mode 100644 index 0000000..4c8b2a5 --- /dev/null +++ b/src/buzzuav_closures.cpp @@ -0,0 +1,859 @@ +/** @file buzzuav_closures.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + +#include "buzzuav_closures.h" +#include "math.h" + +namespace buzzuav_closures +{ +// TODO: Minimize the required global variables and put them in the header +// static const rosbzz_node::roscontroller* roscontroller_ptr; +static double goto_pos[4]; +static double rc_goto_pos[3]; +static float rc_gimbal[4]; +static float batt[3]; +static float obst[5] = { 0, 0, 0, 0, 0 }; +static double cur_pos[4]; +static double cur_NEDpos[2]; +static uint8_t status; +static int cur_cmd = 0; +static int rc_cmd = 0; +static int rc_id = -1; +static int buzz_cmd = 0; +static float height = 0; +static bool deque_full = false; +static int rssi = 0; +static float raw_packet_loss = 0.0; +static int filtered_packet_loss = 0; +static float api_rssi = 0.0; +string WPlistname = ""; + +std::map targets_map; +std::map wplist_map; +std::map neighbors_map; +std::map neighbors_status_map; +std::map> grid; + +/****************************************/ +/****************************************/ + +int buzzros_print(buzzvm_t vm) +/* +/ Buzz closure to print out +----------------------------------------------------------- */ +{ + std::ostringstream buffer(std::ostringstream::ate); + buffer << "[" << buzz_utility::get_robotid() << "] "; + for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) + { + buzzvm_lload(vm, index); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch (o->o.type) + { + case BUZZTYPE_NIL: + buffer << " BUZZ - [nil]"; + break; + case BUZZTYPE_INT: + buffer << " " << o->i.value; + break; + case BUZZTYPE_FLOAT: + buffer << " " << o->f.value; + break; + case BUZZTYPE_TABLE: + buffer << " [table with " << buzzdict_size(o->t.value) << " elems]"; + break; + case BUZZTYPE_CLOSURE: + if (o->c.value.isnative) + { + buffer << " [n-closure @" << o->c.value.ref << "]"; + } + else + { + buffer << " [c-closure @" << o->c.value.ref << "]"; + } + break; + case BUZZTYPE_STRING: + buffer << " " << o->s.value.str; + break; + case BUZZTYPE_USERDATA: + buffer << " [userdata @" << o->u.value << "]"; + break; + default: + break; + } + } + ROS_INFO("%s", buffer.str().c_str()); + return buzzvm_ret0(vm); +} + +void setWPlist(string path) +/* +/ set the absolute path for a csv list of waypoints +----------------------------------------------------------- */ +{ + WPlistname = path + "include/taskallocate/waypointlist.csv"; +} + +float constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ +{ + x = fmod(x + M_PI, 2 * M_PI); + if (x < 0.0) + x += 2 * M_PI; + return x - M_PI; +} + +void rb_from_gps(double nei[], double out[], double cur[]) +/* +/ Compute Range and Bearing from 2 GPS set of coordinates +/----------------------------------------*/ +{ + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); + out[1] = constrainAngle(atan2(ned_y, ned_x)); + out[2] = 0.0; +} + +void parse_gpslist() +/* +/ parse a csv of GPS targets +/----------------------------------------*/ +{ + // Open file: + ROS_INFO("WP list file: %s", WPlistname.c_str()); + std::ifstream fin(WPlistname.c_str()); // Open in text-mode. + + // Opening may fail, always check. + if (!fin) + { + ROS_ERROR("GPS list parser, could not open file."); + return; + } + + // Prepare a C-string buffer to be used when reading lines. + const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. + char buffer[MAX_LINE_LENGTH] = {}; + const char* DELIMS = "\t ,"; // Tab, space or comma. + + // Read the file and load the data: + buzz_utility::RB_struct RB_arr; + // Read one line at a time. + while (fin.getline(buffer, MAX_LINE_LENGTH)) + { + // Extract the tokens: + int tid = atoi(strtok(buffer, DELIMS)); + double lon = atof(strtok(NULL, DELIMS)); + double lat = atof(strtok(NULL, DELIMS)); + int alt = atoi(strtok(NULL, DELIMS)); + // int tilt = atoi(strtok(NULL, DELIMS)); + // DEBUG + // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); + RB_arr.latitude = lat; + RB_arr.longitude = lon; + RB_arr.altitude = alt; + // Insert elements. + map::iterator it = wplist_map.find(tid); + if (it != wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); + + // Close the file: + fin.close(); +} + +int buzz_exportmap(buzzvm_t vm) +/* +/ Buzz closure to export a 2D map +/----------------------------------------*/ +{ + grid.clear(); + buzzvm_lnum_assert(vm, 1); + // Get the parameter + buzzvm_lload(vm, 1); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary + buzzobj_t t = buzzvm_stack_at(vm, 1); + for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) { + buzzvm_dup(vm); + buzzvm_pushi(vm, i); + buzzvm_tget(vm); + std::map row; + for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) { + buzzvm_dup(vm); + buzzvm_pushi(vm, j); + buzzvm_tget(vm); + row.insert(std::pair(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0))); + buzzvm_pop(vm); + } + grid.insert(std::pair>(i,row)); + buzzvm_pop(vm); + } + // DEBUG + // ROS_INFO("----- Recorded a grid of %i(%i)", grid.size(), buzzdict_size(t->t.value)); + return buzzvm_ret0(vm); +} + +int buzzuav_moveto(buzzvm_t vm) +/* +/ Buzz closure to move following a 3D vector + Yaw +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // dx + buzzvm_lload(vm, 2); // dy + buzzvm_lload(vm, 3); // dheight + buzzvm_lload(vm, 4); // dyaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dyaw = buzzvm_stack_at(vm, 1)->f.value; + float dh = buzzvm_stack_at(vm, 2)->f.value; + float dy = buzzvm_stack_at(vm, 3)->f.value; + float dx = buzzvm_stack_at(vm, 4)->f.value; + goto_pos[0] = dx; + goto_pos[1] = dy; + 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]); + buzz_cmd = NAV_SPLINE_WAYPOINT; // TODO: standard mavros? + return buzzvm_ret0(vm); +} + +int buzzuav_addtargetRB(buzzvm_t vm) +/* +/ Buzz closure to add a target (goal) GPS +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // longitude + buzzvm_lload(vm, 2); // latitude + buzzvm_lload(vm, 3); // id + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double tmp[3]; + tmp[0] = buzzvm_stack_at(vm, 2)->f.value; + tmp[1] = buzzvm_stack_at(vm, 1)->f.value; + tmp[2] = 0.0; + int uid = buzzvm_stack_at(vm, 3)->i.value; + double rb[3]; + + rb_from_gps(tmp, rb, cur_pos); + if (fabs(rb[0]) < 100.0) + { + buzz_utility::RB_struct RB_arr; + RB_arr.latitude = tmp[0]; + RB_arr.longitude = tmp[1]; + RB_arr.altitude = tmp[2]; + RB_arr.r = rb[0]; + RB_arr.b = rb[1]; + map::iterator it = targets_map.find(uid); + if (it != targets_map.end()) + targets_map.erase(it); + targets_map.insert(make_pair(uid, RB_arr)); + // DEBUG + // ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + return vm->state; + } + else + ROS_WARN(" ---------- Target too far %f", rb[0]); + + return 0; +} + +int buzzuav_addNeiStatus(buzzvm_t vm) +/* +/ closure to add neighbors status to the BVM +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 5); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzz_utility::neighbors_status newRS; + uint8_t id = buzzvm_stack_at(vm, 5)->i.value; + newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee = buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; + map::iterator it = neighbors_status_map.find(id); + if (it != neighbors_status_map.end()) + neighbors_status_map.erase(it); + neighbors_status_map.insert(make_pair(id, newRS)); + return vm->state; +} + +mavros_msgs::Mavlink get_status() +/* +/ return neighbors status from BVM +/----------------------------------------*/ +{ + mavros_msgs::Mavlink payload_out; + map::iterator it; + for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it) + { + payload_out.payload64.push_back(it->first); + payload_out.payload64.push_back(it->second.gps_strenght); + payload_out.payload64.push_back(it->second.batt_lvl); + payload_out.payload64.push_back(it->second.xbee); + payload_out.payload64.push_back(it->second.flight_status); + } + // Add Robot id and message number to the published message + payload_out.sysid = (uint8_t)neighbors_status_map.size(); + + return payload_out; +} + +int buzzuav_takepicture(buzzvm_t vm) +/* +/ Buzz closure to take a picture here. +/----------------------------------------*/ +{ + buzz_cmd = IMAGE_START_CAPTURE; + return buzzvm_ret0(vm); +} + +int buzzuav_setgimbal(buzzvm_t vm) +/* +/ Buzz closure to change locally the gimbal orientation +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // time + buzzvm_lload(vm, 2); // pitch + buzzvm_lload(vm, 3); // roll + buzzvm_lload(vm, 4); // yaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; + rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; + rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; + rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; + + ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); + buzz_cmd = DO_MOUNT_CONTROL; + return buzzvm_ret0(vm); +} + +int buzzuav_storegoal(buzzvm_t vm) +/* +/ Buzz closure to store locally a GPS destination from the fleet +/----------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // altitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // latitude + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double goal[3]; + goal[0] = buzzvm_stack_at(vm, 3)->f.value; + goal[1] = buzzvm_stack_at(vm, 2)->f.value; + goal[2] = buzzvm_stack_at(vm, 1)->f.value; + if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1) + { + if (wplist_map.size() <= 0) + parse_gpslist(); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_map.begin()->first); + } + + double rb[3]; + + rb_from_gps(goal, rb, cur_pos); + 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]); + } + return buzzvm_ret0(vm); +} + +int buzzuav_arm(buzzvm_t vm) +/* +/ Buzz closure to arm +/---------------------------------------*/ +{ + cur_cmd = COMPONENT_ARM_DISARM; + printf(" Buzz requested Arm \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_disarm(buzzvm_t vm) +/* +/ Buzz closure to disarm +/---------------------------------------*/ +{ + cur_cmd = COMPONENT_ARM_DISARM + 1; + printf(" Buzz requested Disarm \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_takeoff(buzzvm_t vm) +/* +/ Buzz closure to takeoff +/---------------------------------------*/ +{ + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* Altitude */ + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value; + height = goto_pos[2]; + cur_cmd = NAV_TAKEOFF; + printf(" Buzz requested Take off !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_land(buzzvm_t vm) +/* +/ Buzz closure to land +/-------------------------------------------------------------*/ +{ + cur_cmd = NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +int buzzuav_gohome(buzzvm_t vm) +/* +/ Buzz closure to return Home +/-------------------------------------------------------------*/ +{ + cur_cmd = NAV_RETURN_TO_LAUNCH; + printf(" Buzz requested gohome !!! \n"); + buzz_cmd = cur_cmd; + return buzzvm_ret0(vm); +} + +double* getgoto() +/* +/ return the GPS goal +/-------------------------------------------------------------*/ +{ + return goto_pos; +} + +std::map> getgrid() +/* +/ return the grid +/-------------------------------------------------------------*/ +{ + return grid; +} + +float* getgimbal() +/* +/ return current gimbal commands +---------------------------------------*/ +{ + return rc_gimbal; +} + +int getcmd() +/* +/ return current mavros command to the FC +---------------------------------------*/ +{ + return cur_cmd; +} + +int bzz_cmd() +/* +/ return and clean the custom command from Buzz to the FC +----------------------------------------------------------*/ +{ + int cmd = buzz_cmd; + buzz_cmd = 0; + return cmd; +} + +void rc_set_goto(int id, double latitude, double longitude, double altitude) +/* +/ update interface RC GPS goal input +-----------------------------------*/ +{ + rc_id = id; + rc_goto_pos[0] = latitude; + rc_goto_pos[1] = longitude; + rc_goto_pos[2] = altitude; +} + +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) +/* +/ update interface RC gimbal control input +-----------------------------------*/ +{ + rc_id = id; + rc_gimbal[0] = yaw; + rc_gimbal[1] = roll; + rc_gimbal[2] = pitch; + rc_gimbal[3] = t; +} + +void rc_call(int rc_cmd_in) +/* +/ update interface RC command input +-----------------------------------*/ +{ + rc_cmd = rc_cmd_in; +} + +void set_obstacle_dist(float dist[]) +/* +/ update interface proximity value array +-----------------------------------*/ +{ + for (int i = 0; i < 5; i++) + obst[i] = dist[i]; +} + +void set_battery(float voltage, float current, float remaining) +/* +/ update interface battery value array +-----------------------------------*/ +{ + batt[0] = voltage; + batt[1] = current; + batt[2] = remaining; +} + +int buzzuav_update_battery(buzzvm_t vm) +/* +/ update BVM battery table +-----------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); + buzzvm_pushf(vm, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushi(vm, (int)batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +/* +/ Set of function to update interface variable of xbee network status +----------------------------------------------------------------------*/ +void set_deque_full(bool state) +{ + deque_full = state; +} + +void set_rssi(float value) +{ + rssi = round(value); +} + +void set_raw_packet_loss(float value) +{ + raw_packet_loss = value; +} + +void set_filtered_packet_loss(float value) +{ + filtered_packet_loss = round(100 * value); +} + +/*void set_api_rssi(float value) +{ + api_rssi = value; +}*/ + +int buzzuav_update_xbee_status(buzzvm_t vm) +/* +/ update BVM xbee_status table +-----------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1)); + buzzvm_pushi(vm, static_cast(deque_full)); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); + buzzvm_pushi(vm, rssi); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); + buzzvm_pushf(vm, raw_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); + buzzvm_pushi(vm, filtered_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1)); + buzzvm_pushf(vm, api_rssi); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +void set_currentNEDpos(double x, double y) +/* +/ update interface position array +-----------------------------------*/ +{ + cur_NEDpos[0] = x; + cur_NEDpos[1] = y; +} + +void set_currentpos(double latitude, double longitude, float altitude, float yaw) +/* +/ update interface position array +-----------------------------------*/ +{ + cur_pos[0] = latitude; + cur_pos[1] = longitude; + cur_pos[2] = altitude; + cur_pos[3] = yaw; +} +// adds neighbours position +void neighbour_pos_callback(int id, float range, float bearing, float elevation) +{ + buzz_utility::Pos_struct pos_arr; + pos_arr.x = range; + pos_arr.y = bearing; + pos_arr.z = elevation; + map::iterator it = neighbors_map.find(id); + if (it != neighbors_map.end()) + neighbors_map.erase(it); + neighbors_map.insert(make_pair(id, pos_arr)); +} + +// update at each step the VM table +void update_neighbors(buzzvm_t vm) +{ + // Reset neighbor information + buzzneighbors_reset(vm); + // Get robot id and update neighbor information + map::iterator it; + for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it) + { + buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z); + } +} + +// Clear neighbours pos +void clear_neighbours_pos(){ + neighbors_map.clear(); +} + +int buzzuav_update_currentpos(buzzvm_t vm) +/* +/ Update the BVM position table +/------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "pose", 1)); + buzzvm_pusht(vm); + buzzobj_t tPoseTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tPosition = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 0)); + buzzvm_pushf(vm, cur_pos[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 0)); + buzzvm_pushf(vm, cur_pos[1]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0)); + buzzvm_pushf(vm, cur_pos[2]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0)); + buzzvm_pushf(vm, cur_NEDpos[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tPosition); + buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0)); + buzzvm_pushf(vm, cur_NEDpos[1]); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); + buzzvm_push(vm, tPosition); + buzzvm_tput(vm); + + // Create table for i-th read + buzzvm_pusht(vm); + buzzobj_t tOrientation = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tOrientation); + buzzvm_pushs(vm, buzzvm_string_register(vm, "yaw", 0)); + buzzvm_pushf(vm, cur_pos[3]); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tPoseTable); + buzzvm_pushs(vm, buzzvm_string_register(vm, "orientation", 0)); + buzzvm_push(vm, tOrientation); + buzzvm_tput(vm); + + return vm->state; +} + +void flight_status_update(uint8_t state) +/* +/ Update the interface status variable +/------------------------------------------------------*/ +{ + status = state; +} + +int buzzuav_update_flight_status(buzzvm_t vm) +/* +/ Create the generic robot table with status, remote controller current comand and destination +/ and current position of the robot +/------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); + buzzvm_pushi(vm, rc_cmd); + buzzvm_tput(vm); + buzzvm_dup(vm); + rc_cmd = 0; + buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); + buzzvm_pushi(vm, status); + buzzvm_tput(vm); + buzzvm_gstore(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +int buzzuav_update_prox(buzzvm_t vm) +/* +/ Create an obstacle Buzz table from proximity sensors +/ Acessing proximity in buzz script +/ proximity[0].angle and proximity[0].value - front +/ "" "" "" - right and back +/ proximity[3].angle and proximity[3].value - left +/ proximity[4].angle = -1 and proximity[4].value -bottom +-------------------------------------------------------------*/ +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); + buzzvm_pusht(vm); + buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + // Fill into the proximity table + buzzobj_t tProxRead; + float angle = 0; + for (size_t i = 0; i < 4; ++i) + { + // Create table for i-th read + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[i + 1]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, i); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + angle += 1.5708; + } + // Create table for bottom read + angle = -1; + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + // Fill in the read + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + // Store read table in the proximity table + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, 4); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + return vm->state; +} + +int dummy_closure(buzzvm_t vm) +/* +/ Dummy closure for use during update testing +----------------------------------------------------*/ +{ + return buzzvm_ret0(vm); +} +} diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp new file mode 100644 index 0000000..3816f55 --- /dev/null +++ b/src/rosbuzz.cpp @@ -0,0 +1,24 @@ +/** @file rosbuzz.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + +#include "roscontroller.h" + +int main(int argc, char** argv) +/* + / This program implements Buzz in a ROS node using mavros_msgs. + -----------------------------------------------------------------*/ +{ + // Initialize rosBuzz node + ros::init(argc, argv, "rosBuzz"); + ros::NodeHandle nh_priv("~"); + ros::NodeHandle nh; + rosbuzz_node::roscontroller RosController(nh, nh_priv); + + RosController.RosControllerRun(); + return 0; +} diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp new file mode 100644 index 0000000..4e1f2b8 --- /dev/null +++ b/src/roscontroller.cpp @@ -0,0 +1,1517 @@ +/** @file roscontroller.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + +#include "roscontroller.h" +#include +namespace rosbuzz_node +{ +const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; + +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +logical_clock(ros::Time()), previous_step_time(ros::Time()) +/* +/ roscontroller class Constructor +---------------*/ +{ + ROS_INFO("Buzz_node"); + // Obtain parameters from ros parameter server + Rosparameters_get(n_c_priv); + // Initialize publishers, subscribers and client + Initialize_pub_sub(n_c); + // Compile the .bzz file to .basm, .bo and .bdbg + std::string fname = Compile_bzz(bzzfile_name); + bcfname = fname + ".bo"; + dbgfname = fname + ".bdb"; + buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); + buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); + // Initialize variables + SetMode("LOITER", 0); + armstate = 0; + multi_msg = true; + setpoint_counter = 0; + fcu_timeout = TIMEOUT; + cur_pos.longitude = 0; + cur_pos.latitude = 0; + cur_pos.altitude = 0; + + // set stream rate - wait for the FC to be started + SetStreamRate(0, 10, 1); + + // Get Home position - wait for none-zero value + while (cur_pos.latitude == 0.0f) + { + ROS_INFO("Waiting for GPS. "); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } + + // Get Robot Id - wait for Xbee to be started + // or else parse it from the launch file parameter + if (xbeeplugged) + { + GetRobotId(); + } + else + { + robot_id = strtol(robot_name.c_str() + 5, NULL, 10); + } + // time sync algo. parameter intialization + previous_step_time.fromNSec(ros::Time::now().toNSec()); + logical_clock.fromSec(0); + logical_time_rate = 0; + time_sync_jumped = false; + out_msg_time=0; + // Create log dir and open log file + std::string path = + bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; + std::string folder_check="mkdir -p "+path; + system(folder_check.c_str()); + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; + log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); +} + +roscontroller::~roscontroller() +/* +/ roscontroller class Destructor +---------------------*/ +{ + // Destroy the BVM + buzz_utility::buzz_script_destroy(); + // Close the data logging file + log.close(); +} + +void roscontroller::GetRobotId() +/* +/ Get robot ID from the Xbee service +---------------------*/ +{ + mavros_msgs::ParamGet::Request robot_id_srv_request; + robot_id_srv_request.param_id = "id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response)) + { + ros::Duration(0.1).sleep(); + ROS_WARN("ROSBUZZ is waiting for Xbee device ID"); + } + + robot_id = robot_id_srv_response.value.integer; +} + +void roscontroller::send_MPpayload() +{ + MPpayload_pub.publish(buzzuav_closures::get_status()); +} + +void roscontroller::RosControllerRun() +/* +/rosbuzz_node main loop method +/--------------------------------------------------*/ +{ + // Set the Buzz bytecode + if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) + { + ROS_INFO("[%i] INIT DONE!!!", robot_id); + int packet_loss_tmp, time_step = 0; + double cur_packet_loss = 0; + ROS_INFO("[%i] Bytecode file found and set", robot_id); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + // Intialize the update monitor + update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + // set ROS loop rate + ros::Rate loop_rate(BUZZRATE); + // check for BVMSTATE variable + 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(); + state_publisher(); + grid_publisher(); + send_MPpayload(); + // Check updater state and step code + if(update) buzz_update::update_routine(); + if (time_step == BUZZRATE) + { + time_step = 0; + cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); + packet_loss_tmp = 0; + if (cur_packet_loss < 0) + cur_packet_loss = 0; + else if (cur_packet_loss > 1) + cur_packet_loss = 1; + } + else + { + packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); + time_step++; + } + if (debug) + ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + + // log data + // hardware time now + log< 0)log<<","; + map::iterator it = + neighbours_pos_map.begin(); + for (; it != neighbours_pos_map.end(); ++it) + { + log<<","<< it->first<<","; + log<< (double)it->second.x << "," << (double)it->second.y + << "," << (double)it->second.z; + } + for (std::vector::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ + log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<sent_time + <<","<received_time; + } + inmsgdata.clear(); + log<<(int)inmsgdata.size()<<","<< message_number<<","; + log<< out_msg_time<<","; + log < ros::Duration(1.0 / (float)BUZZRATE)) + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, + loop_rate.cycleTime().toSec()); + // Safety land if the data coming from the flight controller are too old + if (fcu_timeout <= 0) + buzzuav_closures::rc_call(NAV_LAND); + else + fcu_timeout -= 1 / BUZZRATE; + timer_step += 1; + // Force a refresh on neighbors array once in a while + maintain_pos(timer_step); + // DEBUG + // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; + } + } +} + +void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) +/* +/ Get all required parameters from the ROS launch file +/-------------------------------------------------------*/ +{ + // Obtain .bzz file name from launch file parameter + if (n_c.getParam("bzzfile_name", bzzfile_name)) + ; + else + { + ROS_ERROR("Provide a .bzz file to run in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain debug mode from launch file parameter + if (n_c.getParam("debug", debug)) + ; + else + { + ROS_ERROR("Provide a debug mode in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain rc service option from parameter server + if (n_c.getParam("rcclient", rcclient)) + { + if (rcclient == true) + { + if (!n_c.getParam("rcservice_name", rcservice_name)) + { + ROS_ERROR("Provide a name topic name for rc service in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else if (rcclient == false) + { + ROS_INFO("RC service is disabled"); + } + } + else + { + ROS_ERROR("Provide a rc client option: yes or no in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain out payload name + n_c.getParam("out_payload", out_payload); + // Obtain in payload name + n_c.getParam("in_payload", in_payload); + // Obtain standby script to run during update + n_c.getParam("stand_by", stand_by); + + if (n_c.getParam("xbee_plugged", xbeeplugged)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (!xbeeplugged) + { + if (n_c.getParam("name", robot_name)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else + n_c.getParam("xbee_status_srv", xbeesrv_name); + + if (!n_c.getParam("capture_image_srv", capture_srv_name)) + { + capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; + } + + GetSubscriptionParameters(n_c); +} + +void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) +/* +/Obtains publisher, subscriber and services from yml config file +/-----------------------------------------------------------------------------------*/ +{ + std::string topic; + if (node_handle.getParam("topics/gps", topic)) + ; + else + { + ROS_ERROR("Provide a gps topic in YAML file"); + system("rosnode kill rosbuzz_node"); + } + m_smTopic_infos.insert(pair(topic, "sensor_msgs/NavSatFix")); + if (node_handle.getParam("topics/localpos", topic)) + ; + else + { + ROS_ERROR("Provide a localpos name in YAML file"); + system("rosnode kill rosbuzz_node"); + } + m_smTopic_infos.insert(pair(topic, "geometry_msgs/PoseStamped")); + + node_handle.getParam("topics/obstacles", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/LaserScan")); + + node_handle.getParam("topics/battery", topic); + m_smTopic_infos.insert(pair(topic, "sensor_msgs/BatteryState")); + + node_handle.getParam("topics/status", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", topic); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/ExtendedState")); + + node_handle.getParam("topics/altitude", topic); + m_smTopic_infos.insert(pair(topic, "std_msgs/Float64")); + + // Obtain required topic and service names from the parameter server + if (node_handle.getParam("topics/fcclient", fcclient_name)) + ; + else + { + ROS_ERROR("Provide a fc client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/setpoint", setpoint_name)) + ; + else + { + ROS_ERROR("Provide a Set Point name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/armclient", armclient)) + ; + else + { + ROS_ERROR("Provide an arm client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/modeclient", modeclient)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/stream", stream_client_name)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } +} + +void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) +/* +/ Create all required subscribers, publishers and ROS service clients +/-------------------------------------------------------*/ +{ + // subscribers + + Subscribe(n_c); + + payload_sub = n_c.subscribe(in_payload, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this); + + // publishers + 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); + bvmstate_pub = n_c.advertise("bvmstate", 5); + grid_pub = n_c.advertise("grid", 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + + // Service Clients + arm_client = n_c.serviceClient(armclient); + mode_client = n_c.serviceClient(modeclient); + mav_client = n_c.serviceClient(fcclient_name); + if (rcclient == true) + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + ROS_INFO("Ready to receive Mav Commands from RC client"); + xbeestatus_srv = n_c.serviceClient(xbeesrv_name); + capture_srv = n_c.serviceClient(capture_srv_name); + stream_client = n_c.serviceClient(stream_client_name); + + multi_msg = true; +} + +void roscontroller::Subscribe(ros::NodeHandle& n_c) +/* +/ Robot independent subscribers +/--------------------------------------*/ +{ + for (std::map::iterator it = 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); + } + else if (it->second == "mavros_msgs/State") + { + flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); + } + else if (it->second == "sensor_msgs/BatteryState") + { + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); + } + else if (it->second == "sensor_msgs/NavSatFix") + { + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::global_gps_callback, this); + } + else if (it->second == "std_msgs/Float64") + { + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::rel_alt_callback, this); + } + else if (it->second == "geometry_msgs/PoseStamped") + { + local_pos_sub = n_c.subscribe(it->first, 5, &roscontroller::local_pos_callback, this); + } + else if (it->second == "sensor_msgs/LaserScan") + { + obstacle_sub = n_c.subscribe(it->first, 5, &roscontroller::obstacle_dist_callback, this); + } + + std::cout << "Subscribed to: " << it->first << endl; + } +} + +std::string roscontroller::Compile_bzz(std::string bzzfile_name) +/* +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +{ + // Compile the buzz code .bzz to .bo + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + 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/"; + bzzfile_in_compile << " -b " << path << name << ".bo"; + bzzfile_in_compile << " -d " << path << name << ".bdb "; + bzzfile_in_compile << bzzfile_name; + + ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str()); + + system(bzzfile_in_compile.str().c_str()); + + return path + name; +} + +void roscontroller::neighbours_pos_publisher() +/* +/ Publish neighbours pos and id in neighbours pos topic +/----------------------------------------------------*/ +{ + auto current_time = ros::Time::now(); + map::iterator it; + rosbuzz::neigh_pos neigh_pos_array; + neigh_pos_array.header.frame_id = "/world"; + neigh_pos_array.header.stamp = current_time; + for (it = raw_neighbours_pos_map.begin(); it != raw_neighbours_pos_map.end(); ++it) + { + sensor_msgs::NavSatFix neigh_tmp; + neigh_tmp.header.stamp = current_time; + neigh_tmp.header.frame_id = "/world"; + neigh_tmp.position_covariance_type = it->first; // custom robot id storage + neigh_tmp.latitude = (it->second).x; + neigh_tmp.longitude = (it->second).y; + neigh_tmp.altitude = (it->second).z; + neigh_pos_array.pos_neigh.push_back(neigh_tmp); + // DEBUG + // cout<<"iterator it val: "<< it-> first << " After convertion: " + // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<> grid = buzzuav_closures::getgrid(); + std::map>::iterator itr = grid.begin(); + int g_w = itr->second.size(); + int g_h = grid.size(); + + if(g_w!=0 && g_h!=0) { + // DEBUG + //ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w); + auto current_time = ros::Time::now(); + nav_msgs::OccupancyGrid grid_msg; + grid_msg.header.frame_id = "/world"; + grid_msg.header.stamp = current_time; + grid_msg.info.map_load_time = current_time; // Same as header stamp as we do not load the map. + grid_msg.info.resolution = 0.01;//gridMap.getResolution(); + grid_msg.info.width = g_w; + grid_msg.info.height = g_h; + grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution; + grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution; + grid_msg.info.origin.position.z = 0.0; + grid_msg.info.origin.orientation.x = 0.0; + grid_msg.info.origin.orientation.y = 0.0; + grid_msg.info.origin.orientation.z = 0.0; + grid_msg.info.origin.orientation.w = 1.0; + grid_msg.data.resize(g_w * g_h); + + for (itr=grid.begin(); itr!=grid.end(); ++itr) { + std::map::iterator itc = itr->second.begin(); + for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) { + grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second; + // DEBUG + //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]); + } + } + grid_pub.publish(grid_msg); + } +} + + +void roscontroller::Arm() +/* +/ Functions handling the local MAV ROS flight controller +/-------------------------------------------------------*/ +{ + mavros_msgs::CommandBool arming_message; + arming_message.request.value = armstate; + if (arm_client.call(arming_message)) + { + if (arming_message.response.success == 1) + ROS_WARN("FC Arm Service called!"); + else + ROS_WARN("FC Arm Service call failed!"); + } + else + { + ROS_WARN("FC Arm Service call failed!"); + } +} + +void roscontroller::prepare_msg_and_publish() +/* +/Prepare Buzz messages payload for each step and publish +/----------------------------------------------------------------------------------------------------- +/ Message format of payload (Each slot is uint64_t) +/ +/ +/| | | | +| | / +/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs +with size......... | / +/|_____|_____|_____|________________________________________________|______________________________| +/----------------------------------------------------------------------------------------------------*/ +{ + // get the payload to be sent + uint64_t* payload_out_ptr = buzz_utility::obt_out_msg(); + uint64_t position[3]; + // Appened current position to message + double tmp[3]; + tmp[0] = cur_pos.latitude; + tmp[1] = cur_pos.longitude; + tmp[2] = cur_pos.altitude; + memcpy(position, tmp, 3 * sizeof(uint64_t)); + mavros_msgs::Mavlink payload_out; + // Add Robot id and message number to the published message + if (message_number < 0) + message_number = 0; + else + message_number++; + + //header variables + uint16_t tmphead[4]; + tmphead[1] = (uint16_t)message_number; + get_logical_time(); + uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000); + memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t)); + uint64_t rheader[1]; + 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; + memcpy(rheader, tmphead, sizeof(uint64_t)); + // push header into the buffer + payload_out.payload64.push_back(rheader[0]); + payload_out.payload64.push_back(position[0]); + 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 + payload_out.payload64.push_back(ros::Time::now().toNSec()); + payload_out.payload64.push_back(logical_clock.toNSec()); + //uint64_t ltrate64 = 0; + //memcpy((void*)<rate64, (void*)&logical_time_rate, sizeof(uint64_t)); + //payload_out.payload64.push_back(ltrate64); + } + else{ + // prepare rosbuzz msg header + 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]); + payload_out.payload64.push_back(position[0]); + payload_out.payload64.push_back(position[1]); + payload_out.payload64.push_back(position[2]); + } + // Append Buzz message + uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); + for (int i = 0; i < out[0]; i++) + { + payload_out.payload64.push_back(payload_out_ptr[i]); + } + //Store out msg time + get_logical_time(); + out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec(); + // publish prepared messages in respective topic + payload_pub.publish(payload_out); + delete[] out; + delete[] payload_out_ptr; + /// Check for updater message if present send + if (update && buzz_update::is_msg_present()) + { + uint8_t* buff_send = 0; + uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size()); + ; + int tot = 0; + mavros_msgs::Mavlink update_packets; + fprintf(stdout, "Appending code into message ...\n"); + fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); + // allocate mem and clear it + buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize); + memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); + // Append updater msg size + *(uint16_t*)(buff_send + tot) = updater_msgSize; + tot += sizeof(uint16_t); + // Append updater msgs + memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize); + tot += updater_msgSize; + // Destroy the updater out msg queue + buzz_update::destroy_out_msg_queue(); + uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); + uint64_t* payload_64 = new uint64_t[total_size]; + memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); + free(buff_send); + // Send a constant number to differenciate updater msgs + update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE); + 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); + delete[] payload_64; + } +} + +void roscontroller::flight_controller_service_call() +/* +/Flight controller service call every step if there is a command set from bzz +script +/-------------------------------------------------------------------------------*/ +{ + // flight controller client call if requested from Buzz + double* goto_pos; + float* gimbal; + switch (buzzuav_closures::bzz_cmd()) + { + case NAV_TAKEOFF: + goto_pos = buzzuav_closures::getgoto(); + cmd_srv.request.param7 = goto_pos[2]; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + // Registering HOME POINT. + home = cur_pos; + } + if (current_mode != "GUIDED") + SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case NAV_LAND: + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (current_mode != "LAND") + { + SetMode("LAND", 0); + armstate = 0; + Arm(); + } + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + { + ROS_ERROR("Failed to call service from flight controller"); + } + armstate = 0; + break; + + case NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! + cmd_srv.request.param5 = home.latitude; + cmd_srv.request.param6 = home.longitude; + cmd_srv.request.param7 = home.altitude; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case COMPONENT_ARM_DISARM: + if (!armstate) + { + SetMode("LOITER", 0); + armstate = 1; + Arm(); + } + break; + + case COMPONENT_ARM_DISARM+1: + if (armstate) + { + armstate = 0; + SetMode("LOITER", 0); + Arm(); + } + break; + + case NAV_SPLINE_WAYPOINT: + goto_pos = buzzuav_closures::getgoto(); + roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); + break; + + case DO_MOUNT_CONTROL: + gimbal = buzzuav_closures::getgimbal(); + cmd_srv.request.param1 = gimbal[0]; + cmd_srv.request.param2 = gimbal[1]; + cmd_srv.request.param3 = gimbal[2]; + cmd_srv.request.param4 = gimbal[3]; + cmd_srv.request.command = DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; + + case IMAGE_START_CAPTURE: + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + if (capture_srv.call(capture_command)) + { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } + else + ROS_ERROR("Failed to call service from camera streamer"); + break; + } +} + +void roscontroller::maintain_pos(int tim_step) +/* +/Refresh neighbours Position for every ten step +/---------------------------------------------*/ +{ + if (timer_step >= BUZZRATE) + { + neighbours_pos_map.clear(); + buzzuav_closures::clear_neighbours_pos(); + // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but + // have to clear ! + timer_step = 0; + } +} + +void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts neighbours position into local struct storage that is cleared every 10 +step +/---------------------------------------------------------------------------------*/ +{ + map::iterator it = neighbours_pos_map.find(id); + if (it != neighbours_pos_map.end()) + neighbours_pos_map.erase(it); + neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts raw neighbours position into local storage for neighbours pos publisher +/-----------------------------------------------------------------------------------*/ +{ + map::iterator it = raw_neighbours_pos_map.find(id); + if (it != raw_neighbours_pos_map.end()) + raw_neighbours_pos_map.erase(it); + raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +void roscontroller::set_cur_pos(double latitude, double longitude, double altitude) +/* +/Set the current position of the robot callback +/--------------------------------------------------------*/ +{ + cur_pos.latitude = latitude; + cur_pos.longitude = longitude; + cur_pos.altitude = altitude; +} + +float roscontroller::constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ +{ + x = fmod(x + M_PI, 2 * M_PI); + if (x < 0.0) + x += 2 * M_PI; + return x - M_PI; +} + +void roscontroller::gps_rb(POSE nei_pos, double out[]) +/* +/ Compute Range and Bearing of a neighbor in a local reference frame +/ from GPS coordinates +----------------------------------------------------------- */ +{ + 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[1] = atan2(ned_y, ned_x); + out[2] = 0.0; +} + +void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t) +/* +/ Get GPS from NED and a reference GPS point (struct input) +----------------------------------------------------------- */ +{ + gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude); +} + +void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, + double gps_r_lat) +/* +/ Get GPS from NED and a reference GPS point +----------------------------------------------------------- */ +{ + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); +}; + +void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg) +/* +/ Update battery status into BVM from subscriber +/------------------------------------------------------*/ +{ + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); + // DEBUG + // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, + // msg->current, msg ->remaining); +} + +void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber for solos +/---------------------------------------------------------------------*/ +{ + // http://wiki.ros.org/mavros/CustomModes + std::cout << "Message: " << msg->mode << std::endl; + if (msg->mode == "GUIDED") + buzzuav_closures::flight_status_update(2); + else if (msg->mode == "LAND") + buzzuav_closures::flight_status_update(1); + else + buzzuav_closures::flight_status_update(7); // default to fit mavros::extended_state +} + +void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber +------------------------------------------------------------*/ +{ + buzzuav_closures::flight_status_update(msg->landed_state); +} + +void roscontroller::global_gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) +/* +/ Update current GPS position into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // reset timeout counter + fcu_timeout = TIMEOUT; + set_cur_pos(msg->latitude, msg->longitude, cur_pos.z); + buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_pos.z, cur_pos.yaw); +} + +void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) +/* +/ Update current position for flight controller NED offset +/-------------------------------------------------------------*/ +{ + 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( + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z, + msg->pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + cur_pos.yaw = yaw; +} + +void roscontroller::rel_alt_callback(const std_msgs::Float64::ConstPtr& msg) +/* +/ Update altitude into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // DEBUG + // ROS_INFO("Altitude in: %f", msg->data); + cur_pos.z = (double)msg->data; +} + +void roscontroller::obstacle_dist_callback(const sensor_msgs::LaserScan::ConstPtr& msg) +/* +/Set obstacle Obstacle distance table into BVM from subscriber +/-------------------------------------------------------------*/ +{ + float obst[5]; + for (int i = 0; i < 5; i++) + obst[i] = msg->ranges[i]; + buzzuav_closures::set_obstacle_dist(obst); +} + +void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) +/* +/ Publisher to send the flight controller navigation commands in local coordinates +/-------------------------------------------------------------*/ +{ + // http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html + // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned + + geometry_msgs::PoseStamped moveMsg; + moveMsg.header.stamp = ros::Time::now(); + moveMsg.header.seq = setpoint_counter++; + moveMsg.header.frame_id = 1; + + // DEBUG + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f, %.3f", cur_pos.x, cur_pos.y, x, y, yaw); + moveMsg.pose.position.x = cur_pos.x + y; + moveMsg.pose.position.y = cur_pos.y + x; + moveMsg.pose.position.z = z; + + tf::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + moveMsg.pose.orientation.x = q[0]; + moveMsg.pose.orientation.y = q[1]; + moveMsg.pose.orientation.z = q[2]; + moveMsg.pose.orientation.w = q[3]; + + // To prevent drifting from stable position, uncomment + if(fabs(x)>0.005 || fabs(y)>0.005) { + localsetpoint_nonraw_pub.publish(moveMsg); + } +} + +void roscontroller::SetMode(std::string mode, int delay_miliseconds) +/* +/ Use setmode service of the flight controller +/-------------------------------------------------------------*/ +{ + // wait if necessary + if (delay_miliseconds != 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); + } + // set mode + mavros_msgs::SetMode set_mode_message; + set_mode_message.request.base_mode = 0; + set_mode_message.request.custom_mode = mode; + current_mode = mode; + if (mode_client.call(set_mode_message)) + { + ; // DEBUG + // ROS_INFO("Set Mode Service call successful!"); + } + else + { + ROS_INFO("Set Mode Service call failed!"); + } +} + +void roscontroller::SetStreamRate(int id, int rate, int on_off) +/* +/ Set the streamrate for mavros publications +/-------------------------------------------------------------*/ +{ + mavros_msgs::StreamRate message; + message.request.stream_id = id; + message.request.message_rate = rate; + message.request.on_off = on_off; + + while (!stream_client.call(message)) + { + ROS_INFO("Set stream rate call failed!, trying again..."); + ros::Duration(0.1).sleep(); + } + ROS_WARN("Set stream rate call successful"); +} + +void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) +/* +/Push payload into BVM FIFO +/---------------------------------------------------------------------------------------- +/ Message format of payload (Each slot is uint64_t) +/ _________________________________________________________________________________________________ +/| | | | | + * | +/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs + * with size......... | +/|_____|_____|_____|________________________________________________|______________________________| + +-----------------------------------------------------------------------------------------------------*/ +{ + // decode msg header + uint64_t rhead = msg->payload64[0]; + uint16_t r16head[4]; + memcpy(r16head,&rhead, sizeof(uint64_t)); + uint16_t mtype = r16head[0]; + uint16_t mid = r16head[1]; + uint32_t temptime=0; + 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 + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE) + { + uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); + uint64_t message_obt[obt_msg_size]; + // Go throught the obtained payload + for (int i = 0; i < (int)msg->payload64.size(); i++) + { + message_obt[i] = (uint64_t)msg->payload64[i]; + } + + uint8_t* pl = (uint8_t*)malloc(obt_msg_size); + memset(pl, 0, obt_msg_size); + // Copy packet into temporary buffer neglecting update constant + memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); + uint16_t unMsgSize = *(uint16_t*)(pl); + fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); + if (unMsgSize > 0) + { + buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + buzz_update::code_message_inqueue_process(); + } + free(pl); + } + // BVM FIFO message + else if ((int)mtype == (int)BUZZ_MESSAGE_TIME || + (int)mtype == (int)BUZZ_MESSAGE_NO_TIME) + { + uint64_t message_obt[msg->payload64.size() - 1]; + // Go throught the obtained payload + for (int i = 1; i < (int)msg->payload64.size(); i++) + { + message_obt[i - 1] = (uint64_t)msg->payload64[i]; + } + // Extract neighbours position from payload + double neighbours_pos_payload[3]; + memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); + buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1], + neighbours_pos_payload[2]); + POSE nei_pos; + nei_pos.latitude = neighbours_pos_payload[0]; + nei_pos.longitude = neighbours_pos_payload[1]; + nei_pos.altitude = neighbours_pos_payload[2]; + double cvt_neighbours_pos_payload[3]; + gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5; + // Extract robot id of the neighbour + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); + //get_logical_time(); + // store in msg data for data log + msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec()); + inmsgdata.push_back(inm); + + if (debug) + ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); + // Pass neighbour position to local maintaner + buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], + cvt_neighbours_pos_payload[2]); + // Put RID and pos + raw_neighbours_pos_put((int)out[1], raw_neigh_pos); + // TODO: remove roscontroller local map array for neighbors + neighbours_pos_put((int)out[1], n_pos); + buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); + if((int)mtype == (int)BUZZ_MESSAGE_TIME){ + // update time struct for sync algo + double nr = 0; + push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } + else{ + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } + } +} + +bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) +/* +/ Catch the ROS service call from a custom remote controller (Mission Planner) +/ and send the requested commands to Buzz +----------------------------------------------------------- */ +{ + int rc_cmd; + switch (req.command) + { + case NAV_TAKEOFF: + ROS_INFO("RC_call: TAKE OFF!!!!"); + rc_cmd = NAV_TAKEOFF; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case NAV_LAND: + ROS_INFO("RC_Call: LAND!!!!"); + rc_cmd = NAV_LAND; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case COMPONENT_ARM_DISARM: + rc_cmd = COMPONENT_ARM_DISARM; + armstate = req.param1; + if (armstate) + { + ROS_INFO("RC_Call: ARM!!!!"); + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + } + else + { + ROS_INFO("RC_Call: DISARM!!!!"); + buzzuav_closures::rc_call(rc_cmd + 1); + res.success = true; + } + break; + case NAV_RETURN_TO_LAUNCH: + ROS_INFO("RC_Call: GO HOME!!!!"); + rc_cmd = NAV_RETURN_TO_LAUNCH; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case NAV_WAYPOINT: + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); + rc_cmd = NAV_WAYPOINT; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); + rc_cmd = DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: + rc_cmd = CMD_REQUEST_UPDATE; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_SYNC_CLOCK: + rc_cmd = CMD_SYNC_CLOCK; + buzzuav_closures::rc_call(rc_cmd); + ROS_INFO("<---------------- Time Sync requested ----------->"); + res.success = true; + break; + default: + buzzuav_closures::rc_call(req.command); + ROS_ERROR("----> Received unregistered command: ", req.command); + res.success = true; + break; + } + return true; +} + +void roscontroller::get_number_of_robots() +/* +/ Garbage collector for the number of robots in the swarm +--------------------------------------------------------------------------*/ +{ + int cur_robots = buzz_utility::get_swarmsize(); + if (no_of_robots == 0) + { + no_of_robots = cur_robots; + } + else + { + if (no_of_robots != cur_robots && no_cnt == 0) + { + no_cnt++; + old_val = cur_robots; + } + else if (no_cnt != 0 && old_val == cur_robots) + { + no_cnt++; + if (no_cnt >= 150 || cur_robots > no_of_robots) + { + no_of_robots = cur_robots; + no_cnt = 0; + } + } + else + { + no_cnt = 0; + } + } +} + +/* +/ Set of functions to grab network quality data from Zbee service +--------------------------------------------------------------------------*/ +bool roscontroller::GetDequeFull(bool& result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "deque_full"; + mavros_msgs::ParamGet::Response srv_response; + + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = (srv_response.value.integer == 1) ? true : false; + return srv_response.success; +} + +bool roscontroller::GetRssi(float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "rssi"; + mavros_msgs::ParamGet::Response srv_response; + + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::TriggerAPIRssi(const uint8_t short_id) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "trig_rssi_api_avg"; + } + else + { + srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + return srv_response.success; +} + +bool roscontroller::GetAPIRssi(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "get_rssi_api_avg"; + } + else + { + srv_request.param_id = "get_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "pl_raw_avg"; + } + else + { + srv_request.param_id = "pl_raw_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float& result) +{ + mavros_msgs::ParamGet::Request srv_request; + if (short_id == 0xFF) + { + srv_request.param_id = "pl_filtered_avg"; + } + else + { + srv_request.param_id = "pl_filtered_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } + + result = srv_response.value.real; + return srv_response.success; +} + +void roscontroller::get_xbee_status() +/* + * Call all the xbee node services and update the xbee status + ------------------------------------------------------------------ */ +{ + bool result_bool; + float result_float; + const uint8_t all_ids = 0xFF; + if (GetDequeFull(result_bool)) + { + buzzuav_closures::set_deque_full(result_bool); + } + if (GetRssi(result_float)) + { + buzzuav_closures::set_rssi(result_float); + } + if (GetRawPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_raw_packet_loss(result_float); + } + if (GetFilteredPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_filtered_packet_loss(result_float); + } + // This part needs testing since it can overload the xbee module + /* + * if(GetAPIRssi(all_ids, result_float)) + * { + * buzzuav_closures::set_api_rssi(result_float); + * } + * TriggerAPIRssi(all_ids); + */ +} + +void roscontroller::time_sync_step() +/* + * Steps the time syncronization algorithm + ------------------------------------------------------------------ */ +{ + //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + double avgRate = logical_time_rate; + double avgOffset = 0; + int offsetCount = 0; + map::iterator it; + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ) + { + 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; + avgOffset = avgOffset + offset; + offsetCount++; + if((it->second).age > BUZZRATE){ + neighbours_time_map.erase(it++); + } + else{ + (it->second).age++; + ++it; + } + ROS_INFO("Size of nei time %i",neighbours_time_map.size()); + } + avgRate = avgRate/(neighbours_time_map.size()+1); + if(offsetCount>0 && !time_sync_jumped){ + int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); + if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){ + set_logical_time_correction(correction); + } + } + get_logical_time(); // just to update clock + time_sync_jumped = false; + //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 + ------------------------------------------------------------------ */ +{ + map::iterator it = neighbours_time_map.find(nid); + double relativeRate =1; + if (it != neighbours_time_map.end()){ + int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + 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) + + (1- MOVING_AVERAGE_ALPHA)*currentRate; + + 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); + } + int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); + if(offset > TIME_SYNC_JUMP_THR){ + set_logical_time_correction(offset);// + time_diff * logical_time_rate ); + time_sync_jumped = true; + } + buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), + logical_clock.toNSec(), nr, relativeRate); + nt.age=0; + neighbours_time_map.insert(make_pair(nid, nt)); +} + + uint64_t roscontroller::get_logical_time(){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff);// + time_diff * logical_time_rate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + return logical_clock.toNSec(); + } + void roscontroller::set_logical_time_correction(uint64_t cor){ + uint64_t l_time_now = get_logical_time(); + logical_clock.fromNSec(l_time_now + cor); + } +}