From ed4c201fab3cf54d22c0c36eb8b53d577096d138 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Fri, 9 Feb 2024 17:51:44 -0330 Subject: [PATCH 1/8] changed to use pixhawk wp reached and only send single setpoints --- src/nav_channel_ctrl.cpp | 84 +++++++++++++++++++++++++--------------- 1 file changed, 53 insertions(+), 31 deletions(-) diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index c809e49..faee560 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -10,6 +10,7 @@ #include #include #include +#include class NavChannel { public: @@ -35,6 +36,8 @@ class NavChannel { global_pos_ = nh_.subscribe(local_pose_topic_, 10, &NavChannel::localPositionCallback, this); + wp_reached_sub_ = nh_.subscribe("/mavros/mission/reached", 10, &NavChannel::missionReachedCallback, this); + // ROS publishers // publishes task progress to /task_status @@ -65,6 +68,7 @@ class NavChannel { ros::Subscriber task_to_exec_; ros::Subscriber prop_map_; ros::Subscriber global_pos_; + ros::Subscriber wp_reached_sub_; ros::Publisher pub_task_status_; ros::Publisher task_goal_position_; task_master::TaskStatus task_status_; @@ -86,6 +90,7 @@ class NavChannel { double dist_past_second_gate; double dist_to_est_gate_2; double dist_from_gate; + double is_reached_; std::string red_marker_str; std::string green_marker_str; @@ -278,22 +283,22 @@ class NavChannel { return midpnt; } - bool isReached() { - //ROS_DEBUG_STREAM(TAG << "isReached() called"); - - // check to see it we are at the goal (within a set amount of error) - bool atDestination = false; - ROS_DEBUG_STREAM(TAG << "wp_error_tolerance: " << wp_error_tolerance); - ROS_DEBUG_STREAM(TAG << "isReached current x: " << current_pos_.pose.position.x << " goal x: " << goal_pos_.point.x << "current y:" << current_pos_.pose.position.y << " goal y: " << goal_pos_.point.y); - if (current_pos_.pose.position.x < goal_pos_.point.x+wp_error_tolerance & current_pos_.pose.position.x > goal_pos_.point.x-wp_error_tolerance) { - if (current_pos_.pose.position.y < goal_pos_.point.y+wp_error_tolerance & current_pos_.pose.position.y > goal_pos_.point.y-wp_error_tolerance) { - atDestination = true; - ROS_INFO_STREAM(TAG << "Goal position reached"); - } - } - - return atDestination; - } + //bool isReached() { + // //ROS_DEBUG_STREAM(TAG << "isReached() called"); +// + // // check to see it we are at the goal (within a set amount of error) + // bool atDestination = false; + // ROS_DEBUG_STREAM(TAG << "wp_error_tolerance: " << wp_error_tolerance); + // ROS_DEBUG_STREAM(TAG << "isReached current x: " << current_pos_.pose.position.x << " goal x: " << goal_pos_.point.x << "current y:" << current_pos_.pose.position.y << " goal y: " << goal_pos_.point.y); + // if (current_pos_.pose.position.x < goal_pos_.point.x+wp_error_tolerance & current_pos_.pose.position.x > goal_pos_.point.x-wp_error_tolerance) { + // if (current_pos_.pose.position.y < goal_pos_.point.y+wp_error_tolerance & current_pos_.pose.position.y > goal_pos_.point.y-wp_error_tolerance) { + // atDestination = true; + // ROS_INFO_STREAM(TAG << "Goal position reached"); + // } + // } +// + // return atDestination; + //} void holdPose() { @@ -301,7 +306,13 @@ class NavChannel { // set goal pose to current pose goal_pos_.point = current_pos_.pose.position; ROS_DEBUG_STREAM(TAG << "Holding position, goal pos set to x: " << current_pos_.pose.position.x << ", y: " << current_pos_.pose.position.y ); - setDestination(); + //setDestination(); + return; + } + + void missionReachedCallback(const mavros_msgs::WaypointReached msg) + { + is_reached_ = true; return; } @@ -320,6 +331,7 @@ class NavChannel { { ROS_INFO_STREAM(TAG << "At least 2 markers detected, moving on to find gate 1"); status = States::FIND_GATE1; + is_reached_ = false; } break; } @@ -328,7 +340,7 @@ class NavChannel { ROS_INFO_STREAM(TAG << "Looking for the 1st gate"); - holdPose(); // maintain current position + //holdPose(); // maintain current position prop_mapper::Prop green_marker; // TODO change to Prop ID prop_mapper::Prop red_marker; // TODO change to Prop ID @@ -344,24 +356,27 @@ class NavChannel { status = States::MOVE_BEFORE_GATE1; goal_pos_.point = before_gate_; ROS_INFO_STREAM(TAG << "Moving to point before first gate at x: " << goal_pos_.point.x << " ,y: " << goal_pos_.point.y); //TODO add angle to the midpoint + setDestination(); } } break; case States::MOVE_BEFORE_GATE1: { - if(!isReached()) + if(!is_reached_) { ROS_DEBUG_STREAM(TAG << "Point before 1st gate not reached yet"); - setDestination(); + ros::Rate rate(10); rate.sleep(); } - if (isReached()) { + if (is_reached_) { + is_reached_ = false; status = States::MOVE_AFTER_GATE1; ROS_INFO_STREAM(TAG << "Point before 1st gate reached"); ROS_INFO_STREAM(TAG << "Moving past gate 1"); goal_pos_.point = after_gate_; + setDestination(); }; } @@ -369,18 +384,20 @@ class NavChannel { case States::MOVE_AFTER_GATE1: { - if(!isReached()) + if(!is_reached_) { ROS_DEBUG_STREAM(TAG << "Point after gate 1 not reached yet"); - setDestination(); + ros::Rate rate(10); rate.sleep(); } - if (isReached()) { + if (is_reached_) { + is_reached_ = false; status = States::FIND_GATE2; ROS_INFO_STREAM(TAG << "Gate 1 passed"); ROS_INFO_STREAM(TAG << "Moving forwards " << dist_to_est_gate_2 << " m until gate 2 identified"); + setDestination(); }; } @@ -391,7 +408,7 @@ class NavChannel { // move forwards until we find second gate goal_pos_.point = est_gate_2_; - setDestination(); + prop_mapper::Prop green_marker; // TODO change to Prop ID prop_mapper::Prop red_marker; // TODO change to Prop ID @@ -405,42 +422,47 @@ class NavChannel { goal_pos_.point = before_gate_;//findEndpoint(green_marker.point, midpnt, dist_to_est_gate_2); // extends the midpoint to actually pass through the gate ROS_INFO_STREAM(TAG << "Moving before 2nd gate at x: " << goal_pos_.point.x << " ,y: " << goal_pos_.point.y); status = States::MOVE_BEFORE_GATE2; + setDestination(); } } break; case States::MOVE_BEFORE_GATE2: { - if(!isReached()) + if(!is_reached_) { ROS_DEBUG_STREAM(TAG << "Point before gate 2 not reached yet"); - setDestination(); + ros::Rate rate(10); rate.sleep(); } - if (isReached()) { + if (is_reached_) { + is_reached_ = false; goal_pos_.point = after_gate_; status = States::MOVE_AFTER_GATE2; ROS_DEBUG_STREAM(TAG << "Point before gate 2 reached"); ROS_INFO_STREAM(TAG << "Moving past gate 2"); + setDestination(); }; } break; case States::MOVE_AFTER_GATE2: { - if(!isReached()) + if(!is_reached_) { ROS_DEBUG_STREAM(TAG << "Point past gate 2 not reached yet"); - setDestination(); + ros::Rate rate(10); rate.sleep(); } - if (isReached()) { + if (is_reached_) { + is_reached_ = false; status = States::COMPLETE; ROS_DEBUG_STREAM(TAG << "Passed gate 2"); ROS_INFO_STREAM(TAG << "Navigation Channel Complete"); + }; } break; From b966110ad4322356ce390ae00874abec40424d1d Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sat, 10 Feb 2024 05:19:25 -0330 Subject: [PATCH 2/8] Add send low frequency setpoints --- config/params.yaml | 6 ++ config/sim_params.yaml | 5 ++ src/nav_channel_ctrl.cpp | 140 +++++++++++++++++++++++++++++---------- 3 files changed, 116 insertions(+), 35 deletions(-) diff --git a/config/params.yaml b/config/params.yaml index 54c7aaa..222ef27 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -6,5 +6,11 @@ dist_past_second_gate: 5 # how far the asv will travel past the second gate for dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate +# Switching Setpoint Modes +use_pixawk_reached: true +freq_disc_mode: true +freq_disc_count: 6 +freq: 2 # in hertz + red_marker: "red_marker" green_marker: "green_marker" \ No newline at end of file diff --git a/config/sim_params.yaml b/config/sim_params.yaml index e08fc7e..9fc3dff 100644 --- a/config/sim_params.yaml +++ b/config/sim_params.yaml @@ -6,6 +6,11 @@ dist_past_second_gate: 5 # how far the asv will travel past the second gate for dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate +# Switching Setpoint Modes +use_pixawk_reached: true +freq_disc_mode: false +freq_disc_count: 1 +freq: 1 red_marker: "red_marker" green_marker: "green_marker" \ No newline at end of file diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index faee560..2b4fe34 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -11,6 +11,7 @@ #include #include #include +#include class NavChannel { public: @@ -23,6 +24,15 @@ class NavChannel { private_nh_.param("dist_past_second_gate", dist_past_second_gate, 2.0); private_nh_.param("dist_to_est_gate_2", dist_to_est_gate_2, 2.0); private_nh_.param("dist_from_gate", dist_from_gate, 1.0); + + private_nh_.param("use_pixawk_reached", use_pixawk_reached_p, 2.0); + private_nh_.param("freq_disc_mode", freq_disc_mode_p, 2.0); + private_nh_.param("freq_disc_count", freq_disc_count_p, 1.0); + private_nh_.param("freq", freq_p, 1.0); + + count_ = freq_disc_count_p; //start with timer disabled + + private_nh_.param("red_marker", red_marker_str, "red_marker"); private_nh_.param("green_marker", green_marker_str, "green_marker"); @@ -38,6 +48,8 @@ class NavChannel { wp_reached_sub_ = nh_.subscribe("/mavros/mission/reached", 10, &NavChannel::missionReachedCallback, this); + state_sub_ = nh_.subscribe("/mavros/state", 10, &NavChannel::state_cb, this); + // ROS publishers // publishes task progress to /task_status @@ -45,6 +57,9 @@ class NavChannel { // publishes desired positions to /task_goal_position to move asv task_goal_position_ = nh_.advertise("task_goal_position", 10); + + // Set publishing rate + publish_timer_ = nh_.createTimer(ros::Duration(1.0 /freq_p), &NavChannel::publishTimerCallback, this); } void spin() { @@ -68,9 +83,12 @@ class NavChannel { ros::Subscriber task_to_exec_; ros::Subscriber prop_map_; ros::Subscriber global_pos_; + ros::Subscriber state_sub_; ros::Subscriber wp_reached_sub_; ros::Publisher pub_task_status_; ros::Publisher task_goal_position_; + ros::Timer publish_timer_; + task_master::TaskStatus task_status_; std::string local_pose_topic_; @@ -84,12 +102,18 @@ class NavChannel { std::string TAG = "NAV_CHANNEL_CTRL: "; + mavros_msgs::State current_state_; double wp_error_tolerance; double gate_max_dist; double gate_max_width; double dist_past_second_gate; double dist_to_est_gate_2; double dist_from_gate; + bool use_pixawk_reached_p; + bool freq_disc_mode_p; + int freq_disc_count_p; + double freq_p; + int count_; double is_reached_; std::string red_marker_str; std::string green_marker_str; @@ -109,6 +133,27 @@ class NavChannel { States status = States::NOT_STARTED; + // for controlled frequency + void publishTimerCallback(const ros::TimerEvent&) { + if (freq_disc_mode_p && count_ < freq_disc_count_p && current_state_.mode == "GUIDED"){ + + // Publish setpoint + setDestination(); + count_++; + } + } + + void resetTimer() + { + count_ = 0; + } + + void stopTimer() + { + count_ = freq_disc_count_p; + } + + void setDestination() { ROS_DEBUG_STREAM(TAG << "setDestination() called"); @@ -219,6 +264,8 @@ class NavChannel { red_marker = props_.props[red_idx]; green_marker = props_.props[green_idx]; gate_found = true; + + ROS_INFO_STREAM(TAG << "Gate found with marker IDs " << red_marker.id << ", " << green_marker.id); } } i++; @@ -283,22 +330,30 @@ class NavChannel { return midpnt; } - //bool isReached() { - // //ROS_DEBUG_STREAM(TAG << "isReached() called"); -// - // // check to see it we are at the goal (within a set amount of error) - // bool atDestination = false; - // ROS_DEBUG_STREAM(TAG << "wp_error_tolerance: " << wp_error_tolerance); - // ROS_DEBUG_STREAM(TAG << "isReached current x: " << current_pos_.pose.position.x << " goal x: " << goal_pos_.point.x << "current y:" << current_pos_.pose.position.y << " goal y: " << goal_pos_.point.y); - // if (current_pos_.pose.position.x < goal_pos_.point.x+wp_error_tolerance & current_pos_.pose.position.x > goal_pos_.point.x-wp_error_tolerance) { - // if (current_pos_.pose.position.y < goal_pos_.point.y+wp_error_tolerance & current_pos_.pose.position.y > goal_pos_.point.y-wp_error_tolerance) { - // atDestination = true; - // ROS_INFO_STREAM(TAG << "Goal position reached"); - // } - // } -// - // return atDestination; - //} + bool isReached() { + //ROS_DEBUG_STREAM(TAG << "isReached() called"); + bool atDestination = false; + if(use_pixawk_reached_p) + { + ROS_DEBUG_STREAM(TAG << "Using pixhawk for waypoint reached signal"); + atDestination = is_reached_; + } + else + { + ROS_DEBUG_STREAM(TAG << "Using current local pose for waypoint reached signal"); + // check to see it we are at the goal (within a set amount of error) + + ROS_DEBUG_STREAM(TAG << "wp_error_tolerance: " << wp_error_tolerance); + ROS_DEBUG_STREAM(TAG << "isReached current x: " << current_pos_.pose.position.x << " goal x: " << goal_pos_.point.x << "current y:" << current_pos_.pose.position.y << " goal y: " << goal_pos_.point.y); + if (current_pos_.pose.position.x < goal_pos_.point.x+wp_error_tolerance & current_pos_.pose.position.x > goal_pos_.point.x-wp_error_tolerance) { + if (current_pos_.pose.position.y < goal_pos_.point.y+wp_error_tolerance & current_pos_.pose.position.y > goal_pos_.point.y-wp_error_tolerance) { + atDestination = true; + ROS_INFO_STREAM(TAG << "Goal position reached"); + } + } + } + return atDestination; + } void holdPose() { @@ -310,10 +365,20 @@ class NavChannel { return; } + void state_cb(const mavros_msgs::State::ConstPtr& msg) + { + current_state_ = *msg; + } + void missionReachedCallback(const mavros_msgs::WaypointReached msg) { - is_reached_ = true; - return; + if(current_state_.mode == "GUIDED") + { + ROS_INFO_STREAM(TAG << "Setpoint reached!"); + is_reached_ = true; + return; + } + } void navChannelCallback(const task_master::Task msg) { @@ -356,27 +421,28 @@ class NavChannel { status = States::MOVE_BEFORE_GATE1; goal_pos_.point = before_gate_; ROS_INFO_STREAM(TAG << "Moving to point before first gate at x: " << goal_pos_.point.x << " ,y: " << goal_pos_.point.y); //TODO add angle to the midpoint - setDestination(); + if ( freq_disc_mode_p ) { resetTimer();} + } } break; case States::MOVE_BEFORE_GATE1: { - if(!is_reached_) + if(!isReached()) { ROS_DEBUG_STREAM(TAG << "Point before 1st gate not reached yet"); - + if (! freq_disc_mode_p) {setDestination();} ros::Rate rate(10); rate.sleep(); } - if (is_reached_) { + if (isReached()) { is_reached_ = false; + if ( freq_disc_mode_p ) { resetTimer();} status = States::MOVE_AFTER_GATE1; ROS_INFO_STREAM(TAG << "Point before 1st gate reached"); ROS_INFO_STREAM(TAG << "Moving past gate 1"); goal_pos_.point = after_gate_; - setDestination(); }; } @@ -384,20 +450,20 @@ class NavChannel { case States::MOVE_AFTER_GATE1: { - if(!is_reached_) + if(!isReached()) { ROS_DEBUG_STREAM(TAG << "Point after gate 1 not reached yet"); - + if (! freq_disc_mode_p) {setDestination();} ros::Rate rate(10); rate.sleep(); } - if (is_reached_) { + if (isReached()) { is_reached_ = false; status = States::FIND_GATE2; ROS_INFO_STREAM(TAG << "Gate 1 passed"); ROS_INFO_STREAM(TAG << "Moving forwards " << dist_to_est_gate_2 << " m until gate 2 identified"); - setDestination(); + }; } @@ -422,46 +488,50 @@ class NavChannel { goal_pos_.point = before_gate_;//findEndpoint(green_marker.point, midpnt, dist_to_est_gate_2); // extends the midpoint to actually pass through the gate ROS_INFO_STREAM(TAG << "Moving before 2nd gate at x: " << goal_pos_.point.x << " ,y: " << goal_pos_.point.y); status = States::MOVE_BEFORE_GATE2; - setDestination(); + if ( freq_disc_mode_p ) { resetTimer();} + } } break; case States::MOVE_BEFORE_GATE2: { - if(!is_reached_) + if(!isReached()) { ROS_DEBUG_STREAM(TAG << "Point before gate 2 not reached yet"); - + if ( !freq_disc_mode_p ) { setDestination();} + ros::Rate rate(10); rate.sleep(); } - if (is_reached_) { + if (isReached()) { is_reached_ = false; goal_pos_.point = after_gate_; status = States::MOVE_AFTER_GATE2; ROS_DEBUG_STREAM(TAG << "Point before gate 2 reached"); ROS_INFO_STREAM(TAG << "Moving past gate 2"); - setDestination(); + if ( freq_disc_mode_p ) { resetTimer();} + }; } break; case States::MOVE_AFTER_GATE2: { - if(!is_reached_) + if(!isReached()) { ROS_DEBUG_STREAM(TAG << "Point past gate 2 not reached yet"); - + if (!freq_disc_mode_p) {setDestination();} ros::Rate rate(10); rate.sleep(); } - if (is_reached_) { + if (isReached()) { is_reached_ = false; status = States::COMPLETE; ROS_DEBUG_STREAM(TAG << "Passed gate 2"); ROS_INFO_STREAM(TAG << "Navigation Channel Complete"); + if(freq_disc_mode_p) {stopTimer();} }; } From 4dcea1a843520690036f090b20d58ed79c7225c4 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sat, 10 Feb 2024 10:05:03 -0330 Subject: [PATCH 3/8] colourblind --- config/params.yaml | 6 +-- src/nav_channel_ctrl.cpp | 114 +++++++++++++++++++++++++++++++++++++-- 2 files changed, 114 insertions(+), 6 deletions(-) diff --git a/config/params.yaml b/config/params.yaml index 222ef27..b2c73e6 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,11 +1,11 @@ # nav_channel wp_error_tolerance: 1.5 # acceptable distance for error when determining if we are at the goal, in meters. -gate_max_dist: 100.0 # max distance for gate 1, minimum distance for gate 2. -gate_max_width: 15.0 # max gap between buoys, accounts for diameter of buoys as well. +gate_max_dist: 12.0 # max distance for gate 1, minimum distance for gate 2. +gate_max_width: 8.0 # max gap between buoys, accounts for diameter of buoys as well. dist_past_second_gate: 5 # how far the asv will travel past the second gate for the task to be considered complete, in meters dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate - +colour_blind: true # Switching Setpoint Modes use_pixawk_reached: true freq_disc_mode: true diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index 2b4fe34..8986896 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -24,6 +24,7 @@ class NavChannel { private_nh_.param("dist_past_second_gate", dist_past_second_gate, 2.0); private_nh_.param("dist_to_est_gate_2", dist_to_est_gate_2, 2.0); private_nh_.param("dist_from_gate", dist_from_gate, 1.0); + private_nh_.param("color_blind", colour_blind_p, true); private_nh_.param("use_pixawk_reached", use_pixawk_reached_p, 2.0); private_nh_.param("freq_disc_mode", freq_disc_mode_p, 2.0); @@ -111,6 +112,7 @@ class NavChannel { double dist_from_gate; bool use_pixawk_reached_p; bool freq_disc_mode_p; + bool colour_blind_p; int freq_disc_count_p; double freq_p; int count_; @@ -229,7 +231,106 @@ class NavChannel { return angle; } - bool findGate(prop_mapper::Prop &green_marker, prop_mapper::Prop &red_marker) + double calcHypotenuse(prop_mapper::Prop prop) { + // Calculate the square of the lengths of the catheti + double x = prop.point.x; + double y = prop.point.y; + double x_squared = x * x; + double y_squared = y * y; + + // Calculate the square of the length of the hypotenuse + double hypotenuse_squared = x_squared + y_squared; + + // Calculate the length of the hypotenuse by taking the square root + double hypotenuse = std::sqrt(hypotenuse_squared); + + return hypotenuse; + } + + double calcDistance(geometry_msgs::Point pnt1, geometry_msgs::Point pnt2 ) { + double x1 = pnt1.x; + double y1 = pnt1.y; + double x2 = pnt2.x; + double y2 = pnt2.y; + double dx = x2 - x1; + double dy = y2 - y1; + + double distance = std::sqrt(dx * dx + dy * dy); + + return distance; + } + + bool findGateGreyScale(prop_mapper::Prop &green_marker, prop_mapper::Prop &red_marker) + { + ROS_DEBUG_STREAM(TAG << "findGate() called"); + + int green_idx; + int red_idx; + + //bool green_found = false; + //bool red_found = false; + bool gate_found = false; + + //int i = 0; + + std::vector marker_arr; + + // pull out markers + for (int n =0; n< props_.props.size(); n++) + { + if ( props_.props[n].prop_label == "red_marker" || props_.props[n].prop_label == "green_marker" || props_.props[n].prop_label == "marker" || props_.props[n].prop_label == "buoy") + { + ROS_DEBUG_STREAM(TAG << "marker added to marker array"); + marker_arr.push_back(props_.props[n]); + } + } + + // get closest marker + prop_mapper::Prop closest_marker; + closest_marker.point.x = 1000; + closest_marker.point.y = 1000; + int closest_idx = 0; + + for ( int i = 0; i < marker_arr.size(); i++) + { + if ( calcHypotenuse(marker_arr[i]) < calcHypotenuse(closest_marker)) + { + closest_marker = marker_arr[i]; + closest_idx = i; + + } + } + + marker_arr.erase(marker_arr.begin()+closest_idx); // remove closest element + + while(marker_arr.size() > 0 && !gate_found) + { + for ( int j = 0; j < marker_arr.size(); j++) + { + if ( calcDistance(closest_marker.point, marker_arr[j].point) < gate_max_width && calcDistance(closest_marker.point, current_pos_.pose.position) < gate_max_dist ) + { + //valid gate found + green_marker = closest_marker; + red_marker = marker_arr[j]; + gate_found = true; + closest_idx = j; + } + else + { + marker_arr.erase(marker_arr.begin()+closest_idx); // marker does not meet criteria - remove it + } + } + } + + if (gate_found) + { + return true; + } + return false; + + } + + bool findGateWithColours(prop_mapper::Prop &green_marker, prop_mapper::Prop &red_marker) { ROS_DEBUG_STREAM(TAG << "findGate() called"); @@ -264,8 +365,6 @@ class NavChannel { red_marker = props_.props[red_idx]; green_marker = props_.props[green_idx]; gate_found = true; - - ROS_INFO_STREAM(TAG << "Gate found with marker IDs " << red_marker.id << ", " << green_marker.id); } } i++; @@ -274,6 +373,15 @@ class NavChannel { return gate_found; } + bool findGate(prop_mapper::Prop &green_marker, prop_mapper::Prop &red_marker) + { + if (colour_blind_p) + { + return findGateGreyScale(green_marker, red_marker); + } + return findGateWithColours(green_marker, red_marker); + } + geometry_msgs::Point translatePoint(geometry_msgs::Point point, double x_trans, double y_trans ) { point.x = point.x + x_trans; From 77db987bea6e5ef53c61e125026949684691ae1f Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sat, 10 Feb 2024 12:20:54 -0330 Subject: [PATCH 4/8] Updates --- src/nav_channel_ctrl.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index 8986896..3eadb46 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -15,7 +15,7 @@ class NavChannel { public: - NavChannel(): nh_(""), private_nh_("~"), green_id_(-1), red_id_(-1) + NavChannel(): nh_(""), private_nh_("~"), green_id_(-1), red_id_(-1), reached_count_(0) { // ROS parameters private_nh_.param("error", wp_error_tolerance, 1.0); // Tolerance radius for determining if asv reached gate waypoint @@ -104,11 +104,13 @@ class NavChannel { std::string TAG = "NAV_CHANNEL_CTRL: "; mavros_msgs::State current_state_; + mavros_msgs::State prev_state_; double wp_error_tolerance; double gate_max_dist; double gate_max_width; double dist_past_second_gate; double dist_to_est_gate_2; + int reached_count_; double dist_from_gate; bool use_pixawk_reached_p; bool freq_disc_mode_p; @@ -278,7 +280,7 @@ class NavChannel { // pull out markers for (int n =0; n< props_.props.size(); n++) { - if ( props_.props[n].prop_label == "red_marker" || props_.props[n].prop_label == "green_marker" || props_.props[n].prop_label == "marker" || props_.props[n].prop_label == "buoy") + if ( props_.props[n].prop_label == "red_marker" || props_.props[n].prop_label == "green_marker" || props_.props[n].prop_label == "marker" && props_.props[n].id != green_id_ && props_.props[n].id != red_id_ ) { ROS_DEBUG_STREAM(TAG << "marker added to marker array"); marker_arr.push_back(props_.props[n]); @@ -312,6 +314,8 @@ class NavChannel { //valid gate found green_marker = closest_marker; red_marker = marker_arr[j]; + green_id_ = closest_marker.id; + red_id_ = marker_arr[j].id; gate_found = true; closest_idx = j; } @@ -475,18 +479,26 @@ class NavChannel { void state_cb(const mavros_msgs::State::ConstPtr& msg) { - current_state_ = *msg; + + current_state_ = *msg; + if (prev_state_.mode == "MANUAL" && current_state_.mode == "GUIDED") + { + reached_count_ = 0; + } + prev_state_ = current_state_; } void missionReachedCallback(const mavros_msgs::WaypointReached msg) { - if(current_state_.mode == "GUIDED") - { + ROS_INFO_STREAM(TAG << "reached count: " << reached_count_); + if(current_state_.mode == "GUIDED" )//&& reached_count_ == freq_disc_count_p-1) + { ROS_INFO_STREAM(TAG << "Setpoint reached!"); is_reached_ = true; + reached_count_ = 0; return; } - + reached_count_++; } void navChannelCallback(const task_master::Task msg) { From 14466f99112e83d1fc7f07770cba0f194c09e0e5 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sat, 10 Feb 2024 13:39:03 -0330 Subject: [PATCH 5/8] Updating params --- config/params.yaml | 4 ++-- config/sim_params.yaml | 14 +++++++------- src/nav_channel_ctrl.cpp | 5 +++-- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/config/params.yaml b/config/params.yaml index b2c73e6..33725d2 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -7,9 +7,9 @@ dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated se dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate colour_blind: true # Switching Setpoint Modes -use_pixawk_reached: true +use_pixawk_reached: false freq_disc_mode: true -freq_disc_count: 6 +freq_disc_count: 3 freq: 2 # in hertz red_marker: "red_marker" diff --git a/config/sim_params.yaml b/config/sim_params.yaml index 9fc3dff..e016a02 100644 --- a/config/sim_params.yaml +++ b/config/sim_params.yaml @@ -3,14 +3,14 @@ wp_error_tolerance: 1.5 # acceptable distance for error when determining if we a gate_max_dist: 100.0 # max distance for gate 1, minimum distance for gate 2. gate_max_width: 100.0 # max gap between buoys, accounts for diameter of buoys as well. dist_past_second_gate: 5 # how far the asv will travel past the second gate for the task to be considered complete, in meters -dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate +dist_to_est_gate_2: 10 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate - +colour_blind: true # Switching Setpoint Modes -use_pixawk_reached: true -freq_disc_mode: false -freq_disc_count: 1 -freq: 1 +use_pixawk_reached: false +freq_disc_mode: true +freq_disc_count: 3 +freq: 2 # in hertz red_marker: "red_marker" -green_marker: "green_marker" \ No newline at end of file +green_marker: "green_marker" diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index 3eadb46..a1eac68 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -303,7 +303,7 @@ class NavChannel { } } - marker_arr.erase(marker_arr.begin()+closest_idx); // remove closest element + //marker_arr.erase(marker_arr.begin()+closest_idx); // remove closest element while(marker_arr.size() > 0 && !gate_found) { @@ -321,7 +321,8 @@ class NavChannel { } else { - marker_arr.erase(marker_arr.begin()+closest_idx); // marker does not meet criteria - remove it + ROS_DEBUG_STREAM(TAG << "maker not in gate"); + //marker_arr.erase(marker_arr.begin()+closest_idx); // marker does not meet criteria - remove it } } } From 1bc640cb957d7a9dae0a586c06c964f071e90681 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sun, 11 Feb 2024 09:06:04 -0330 Subject: [PATCH 6/8] update nav channel params --- config/params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/params.yaml b/config/params.yaml index 33725d2..b357329 100644 --- a/config/params.yaml +++ b/config/params.yaml @@ -1,9 +1,9 @@ # nav_channel -wp_error_tolerance: 1.5 # acceptable distance for error when determining if we are at the goal, in meters. +wp_error_tolerance: 1.0 # acceptable distance for error when determining if we are at the goal, in meters. gate_max_dist: 12.0 # max distance for gate 1, minimum distance for gate 2. gate_max_width: 8.0 # max gap between buoys, accounts for diameter of buoys as well. dist_past_second_gate: 5 # how far the asv will travel past the second gate for the task to be considered complete, in meters -dist_to_est_gate_2: 5 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate +dist_to_est_gate_2: 20 # how far from the first gate will we set our estimated second gate, in meters. Boat will travel here until it detects second gate dist_from_gate: 2 # midpoint waypoints will be set this distance before and after the gate colour_blind: true # Switching Setpoint Modes From ea2f9c555c4f08e5c4bd28381b6a0796d3ad76b7 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sun, 11 Feb 2024 09:13:03 -0330 Subject: [PATCH 7/8] add timer resets --- src/nav_channel_ctrl.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index a1eac68..33ae488 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -266,6 +266,7 @@ class NavChannel { { ROS_DEBUG_STREAM(TAG << "findGate() called"); + int green_idx; int red_idx; @@ -280,6 +281,7 @@ class NavChannel { // pull out markers for (int n =0; n< props_.props.size(); n++) { + ROS_DEBUG_STREAM(TAG << "Pull out markers from array"); if ( props_.props[n].prop_label == "red_marker" || props_.props[n].prop_label == "green_marker" || props_.props[n].prop_label == "marker" && props_.props[n].id != green_id_ && props_.props[n].id != red_id_ ) { ROS_DEBUG_STREAM(TAG << "marker added to marker array"); @@ -292,6 +294,7 @@ class NavChannel { closest_marker.point.x = 1000; closest_marker.point.y = 1000; int closest_idx = 0; + bool can_erase = false; for ( int i = 0; i < marker_arr.size(); i++) { @@ -299,11 +302,16 @@ class NavChannel { { closest_marker = marker_arr[i]; closest_idx = i; + can_erase = true; } } - //marker_arr.erase(marker_arr.begin()+closest_idx); // remove closest element + //if (can_erase) + //{ + // marker_arr.erase(marker_arr.begin()+closest_idx); // remove closest element + //} + while(marker_arr.size() > 0 && !gate_found) { @@ -337,7 +345,7 @@ class NavChannel { bool findGateWithColours(prop_mapper::Prop &green_marker, prop_mapper::Prop &red_marker) { - ROS_DEBUG_STREAM(TAG << "findGate() called"); + ROS_DEBUG_STREAM(TAG << "findGate with colours called"); int green_idx; int red_idx; @@ -564,6 +572,7 @@ class NavChannel { ROS_INFO_STREAM(TAG << "Point before 1st gate reached"); ROS_INFO_STREAM(TAG << "Moving past gate 1"); goal_pos_.point = after_gate_; + if ( freq_disc_mode_p ) { resetTimer();} }; } From e4cdb311b52473a8e83d3b7e1e14bd7d358d4435 Mon Sep 17 00:00:00 2001 From: gpearcey Date: Sun, 11 Feb 2024 09:17:45 -0330 Subject: [PATCH 8/8] add missing timer rest --- src/nav_channel_ctrl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/nav_channel_ctrl.cpp b/src/nav_channel_ctrl.cpp index 33ae488..d0bbbd1 100644 --- a/src/nav_channel_ctrl.cpp +++ b/src/nav_channel_ctrl.cpp @@ -593,6 +593,8 @@ class NavChannel { status = States::FIND_GATE2; ROS_INFO_STREAM(TAG << "Gate 1 passed"); ROS_INFO_STREAM(TAG << "Moving forwards " << dist_to_est_gate_2 << " m until gate 2 identified"); + goal_pos_.point = est_gate_2_; + if ( freq_disc_mode_p ) { resetTimer();} };