From 8af7578bc6071148adcfe8b1cb736f4b0b8d7b36 Mon Sep 17 00:00:00 2001 From: Fang Wu Date: Thu, 18 Oct 2018 12:33:15 -0400 Subject: [PATCH 01/13] Add csv file, absolute path used, might need to fix in the future modified: src/rosbuzz/buzz_scripts/include/bidding/bidding.bzz new file: src/rosbuzz/buzz_scripts/include/bidding/waypoints_12.csv --- buzz_scripts/include/bidding/bidding.bzz | 4 ++-- buzz_scripts/include/bidding/waypoints_12.csv | 13 +++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) create mode 100755 buzz_scripts/include/bidding/waypoints_12.csv diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/bidding/bidding.bzz index 8f28c01..fd32bba 100644 --- a/buzz_scripts/include/bidding/bidding.bzz +++ b/buzz_scripts/include/bidding/bidding.bzz @@ -6,8 +6,8 @@ # ######################################## -CSV_FILENAME_AND_PATH = "/home/amber/rosbuzz-coverage/rosbuzz-coverage/waypoints/waypoints_15.csv" -OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv' +CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/bidding/waypoints_15.csv" +OUTPUT_FILENAME_AND_PATH = "/home/ubuntu/bidding_output/output-" # automatically completed with 'ID.csv' BID_WAIT = 40 PICTURE_WAIT = 40 BASE_ALTITUDE = 5.0 diff --git a/buzz_scripts/include/bidding/waypoints_12.csv b/buzz_scripts/include/bidding/waypoints_12.csv new file mode 100755 index 0000000..ec4cfdf --- /dev/null +++ b/buzz_scripts/include/bidding/waypoints_12.csv @@ -0,0 +1,13 @@ +0,0,45.510378000000000,-73.609516000000000,659.526,IMG_1102.JPG +1,0,45.510322000000000,-73.609484000000000,657.343,IMG_1142.JPG +2,0,45.510259000000000,-73.609444000000000,660.146,IMG_1056.JPG +3,0,45.510415000000000,-73.609412000000000,660.287,IMG_1049.JPG +4,0,45.510357000000000,-73.609377000000000,660.123,IMG_1072.JPG +5,0,45.510295000000000,-73.609336000000000,663.132,IMG_1078.JPG +6,0,45.510550000000000,-73.609176000000000,665.142,IMG_1092.JPG +7,0,45.510500000000000,-73.609142000000000,666.124,IMG_1095.JPG +8,0,45.510433000000000,-73.609104000000000,661.123,IMG_1098.JPG +9,0,45.510374000000000,-73.609069000000000,663.123,IMG_1100.JPG +10,0,45.510330000000000,-73.609041000000000,664.110,IMG_1101.JPG +11,0,45.510345000000000,-73.608986000000000,665.123,IMG_1103.JPG + From 389b8fca7ceb1ffcb83f4b3cbd1712f6ae6bce67 Mon Sep 17 00:00:00 2001 From: Fang Wu Date: Thu, 18 Oct 2018 16:57:47 -0400 Subject: [PATCH 02/13] Change the csv file name modified: src/rosbuzz/buzz_scripts/include/bidding/bidding.bzz --- buzz_scripts/include/bidding/bidding.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/bidding/bidding.bzz index fd32bba..521eb29 100644 --- a/buzz_scripts/include/bidding/bidding.bzz +++ b/buzz_scripts/include/bidding/bidding.bzz @@ -6,7 +6,7 @@ # ######################################## -CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/bidding/waypoints_15.csv" +CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/bidding/waypoints_12.csv" OUTPUT_FILENAME_AND_PATH = "/home/ubuntu/bidding_output/output-" # automatically completed with 'ID.csv' BID_WAIT = 40 PICTURE_WAIT = 40 From 3a0239b2ae16e034656b65b8e17224e6edd1e58c Mon Sep 17 00:00:00 2001 From: david Date: Thu, 18 Oct 2018 18:16:35 -0400 Subject: [PATCH 03/13] io.fwrite function in buzz was crashing the node, commented out for now. Crash reason undertermined. --- buzz_scripts/include/bidding/bidding.bzz | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/buzz_scripts/include/bidding/bidding.bzz b/buzz_scripts/include/bidding/bidding.bzz index 521eb29..3e27d46 100644 --- a/buzz_scripts/include/bidding/bidding.bzz +++ b/buzz_scripts/include/bidding/bidding.bzz @@ -230,13 +230,13 @@ function sc_cover_assigned_area() { var pursuing_wp = current_area_wp_order[current_area_wp_index] sc_move_wp(pursuing_wp) if (distance_from_gps(wp_lat(pursuing_wp), wp_lon(pursuing_wp))<0.1) { - io.fwrite(output_file, string.concat( string.tostring(wp_area(pursuing_wp)), ",", - string.tostring(wp_type(pursuing_wp)), ",", - string.tostring(wp_lat(pursuing_wp)), ",", - string.tostring(wp_lon(pursuing_wp)), ",", - string.tostring(wp_alt(pursuing_wp)), ",", - wp_filename(pursuing_wp), ",", - string.tostring(id) )) + # io.fwrite(output_file, string.concat( string.tostring(wp_area(pursuing_wp)), ",", + # string.tostring(wp_type(pursuing_wp)), ",", + # string.tostring(wp_lat(pursuing_wp)), ",", + # string.tostring(wp_lon(pursuing_wp)), ",", + # string.tostring(wp_alt(pursuing_wp)), ",", + # wp_filename(pursuing_wp), ",", + # string.tostring(id) )) if (current_area_wp_index < (current_area_wp_number - 1)) { current_area_wp_index = current_area_wp_index + 1 } else { From 9cac1eef656539f8287c7fe32dde5f46348cd185 Mon Sep 17 00:00:00 2001 From: Fang Wu Date: Mon, 22 Oct 2018 14:56:39 -0400 Subject: [PATCH 04/13] Changes to be committed: change the absolute path to drone workspace modified: src/rosbuzz/buzz_scripts/include/taskallocate/bidding.bzz --- buzz_scripts/include/taskallocate/bidding.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/taskallocate/bidding.bzz b/buzz_scripts/include/taskallocate/bidding.bzz index 01fd468..96416e9 100644 --- a/buzz_scripts/include/taskallocate/bidding.bzz +++ b/buzz_scripts/include/taskallocate/bidding.bzz @@ -6,8 +6,8 @@ # ######################################## -CSV_FILENAME_AND_PATH = "/home/amber/ROS_WS/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv" -OUTPUT_FILENAME_AND_PATH = "/home/amber/bidding_output/output-" # automatically completed with 'ID.csv' +CSV_FILENAME_AND_PATH = "/home/ubuntu/ROS_DRONES_WS2/src/rosbuzz/buzz_scripts/include/taskallocate/waypoints_12.csv" +OUTPUT_FILENAME_AND_PATH = "/home/ubuntu/bidding_output/output-" # automatically completed with 'ID.csv' BID_WAIT = 40 PICTURE_WAIT = 40 BASE_ALTITUDE = 5.0 From e36101f0de54c41e4323bfc0898196bb3f7ebdda Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 24 Oct 2018 14:40:01 -0400 Subject: [PATCH 05/13] Fix for solos set mode issue --- src/roscontroller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 362d477..20d955e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -768,7 +768,7 @@ script BClpose = true; } if (current_mode != "GUIDED" && setmode) - SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo + SetMode("GUIDED", 3000); // added for compatibility with 3DR Solo if(setmode) { if (mav_client.call(cmd_srv)) From a80fb083102f9e8ae93040263a8565faba104f5b Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 24 Oct 2018 16:45:42 -0400 Subject: [PATCH 06/13] Change the takeoff height for gizmo modified: src/rosbuzz/buzz_scripts/include/utils/takeoff_heights.bzz --- buzz_scripts/include/utils/takeoff_heights.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index 81081aa..dc539e1 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -6,9 +6,9 @@ takeoff_heights ={ .6 = 3.0, .9 = 15.0, .11 = 6.0, - .14 = 18.0, + .14 = 9.0, .16 = 9.0, .17 = 3.0, .18 = 6.0, .19 = 9.0 -} \ No newline at end of file +} From aeb622649f35801dc0884f0012bded504b94471c Mon Sep 17 00:00:00 2001 From: Wayne Date: Wed, 24 Oct 2018 18:02:54 -0400 Subject: [PATCH 07/13] Changed Centurion's takeoff height: modified: src/rosbuzz/buzz_scripts/include/utils/takeoff_heights.bzz --- buzz_scripts/include/utils/takeoff_heights.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index dc539e1..b43cf10 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -2,7 +2,7 @@ takeoff_heights ={ .1 = 21.0, .2 = 18.0, .3 = 12.0, - .5 = 12.0, + .5 = 6.0, .6 = 3.0, .9 = 15.0, .11 = 6.0, From aed31e22b82198610467d467fc81c681e1a81116 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 25 Oct 2018 14:07:18 -0400 Subject: [PATCH 08/13] Update takeoff_heights.bzz with new xbee sx heights --- buzz_scripts/include/utils/takeoff_heights.bzz | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/buzz_scripts/include/utils/takeoff_heights.bzz b/buzz_scripts/include/utils/takeoff_heights.bzz index b43cf10..a8fa27c 100644 --- a/buzz_scripts/include/utils/takeoff_heights.bzz +++ b/buzz_scripts/include/utils/takeoff_heights.bzz @@ -10,5 +10,8 @@ takeoff_heights ={ .16 = 9.0, .17 = 3.0, .18 = 6.0, - .19 = 9.0 + .19 = 9.0, + .20 = 3.0, + .21 = 6.0, + .22 = 9.0 } From b9966949768bda36c0771857158a3f97e328bb1f Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 18 Nov 2018 23:12:36 +0100 Subject: [PATCH 09/13] switched battery topic to mavros::batterystatus --- include/roscontroller.h | 4 ++-- src/roscontroller.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 0f2bc2f..75ac1f9 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -12,7 +12,7 @@ #include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" -#include "sensor_msgs/BatteryState.h" +//#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" @@ -240,7 +240,7 @@ private: double gps_r_lat); /*battery status callback */ - void battery(const sensor_msgs::BatteryState::ConstPtr& msg); + void battery(const mavros_msgs::BatteryStatus::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 071336b..d9aa603 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -352,7 +352,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, "sensor_msgs/BatteryState")); + m_smTopic_infos.insert(pair(topic, "mavros_msgs/BatteryStatus")); node_handle.getParam("topics/status", topic); m_smTopic_infos.insert(pair(topic, "mavros_msgs/State")); @@ -504,7 +504,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 == "sensor_msgs/BatteryState") + else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } @@ -1035,12 +1035,13 @@ 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 sensor_msgs::BatteryState::ConstPtr& msg) +void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) /* / Update battery status into BVM from subscriber /------------------------------------------------------*/ { - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); + // buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage); + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining*!00.0); // DEBUG // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // msg->current, msg ->remaining); From cffea220ae78a8a74a4a43effc1c6b8d4b7841ce Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 18 Nov 2018 23:24:55 +0100 Subject: [PATCH 10/13] reverted previous commit change of battery topic --- include/roscontroller.h | 6 +++--- src/roscontroller.cpp | 9 ++++----- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 75ac1f9..6edb678 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -11,8 +11,8 @@ #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/BatteryStatus.h" +#include "sensor_msgs/BatteryState.h" #include "mavros_msgs/Mavlink.h" #include "mavros_msgs/PositionTarget.h" #include "sensor_msgs/NavSatStatus.h" @@ -240,7 +240,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 d9aa603..50be762 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -352,7 +352,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")); @@ -504,7 +504,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); } @@ -1035,13 +1035,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->percentage); - buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining*!00.0); + 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 122994c3493f57b4c2b54e473705695bbd2bc47b Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 19 Nov 2018 11:16:10 +0100 Subject: [PATCH 11/13] change geofence --- buzz_scripts/include/act/naviguation.bzz | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 4c60583..6db477d 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -7,10 +7,10 @@ GPSlimit_CEPSUM = {.1={.lat=45.510400, .lng=-73.610421}, .3={.lat=45.510355, .lng=-73.608404}, .4={.lat=45.509840, .lng=-73.610072}} -GPSlimit_PANGEAE = {.1={.lat=29.020871, .lng=-13.712477}, - .2={.lat=29.019850, .lng=-13.712378}, - .3={.lat=29.019875, .lng=-13.710096}, - .4={.lat=29.021245, .lng=-13.710184}} +GPSlimit_PANGEAE = {.1={.lat=29.02045, .lng=-13.71234}, + .2={.lat=29.02131, .lng=-13.70961}, + .3={.lat=29.02061, .lng=-13.70933}, + .4={.lat=29.01966, .lng=-13.71203}} # Core naviguation function to travel to a GPS target location. function goto_gps(transf) { @@ -33,4 +33,4 @@ 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 -} \ No newline at end of file +} From 1223b3e18d0a9f0bda2e22dee280a232ae286732 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 20 Nov 2018 15:28:08 +0100 Subject: [PATCH 12/13] msg size to 200 --- src/buzz_utility.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 4686ffd..fe21f90 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ 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 = 210;//250; // Maximum Msg size for sending update packets +static uint32_t MAX_MSG_SIZE = 180;//250; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; std::map users_map; From 26b249625db4628bd2df1caf0cb550c2f64d7cd0 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 20 Nov 2018 16:04:55 +0100 Subject: [PATCH 13/13] increase barrier timeout --- buzz_scripts/include/act/barrier.bzz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index b21db79..d9d4543 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -7,7 +7,7 @@ # # Constants # -BARRIER_TIMEOUT = 200 # in steps +BARRIER_TIMEOUT = 400 # in steps BARRIER_VSTIG = 80 timeW = 1 barrier = nil @@ -108,4 +108,4 @@ function barrier_allgood(barrier, bc) { barriergood = 0 }) return barriergood -} \ No newline at end of file +}