From 123a0e23d506e1ec30afc577407d64445f5ec22a Mon Sep 17 00:00:00 2001 From: Sadanand Modak Date: Fri, 3 Nov 2023 02:55:04 -0500 Subject: [PATCH 1/2] adding reset nav and cleaning viz after robot stops --- src/navigation/navigation.cc | 7 +++++++ src/navigation/navigation.h | 1 + src/navigation/navigation_main.cc | 16 ++++++++++++++-- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc index 5667df2..7dd2c2a 100644 --- a/src/navigation/navigation.cc +++ b/src/navigation/navigation.cc @@ -189,6 +189,13 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) { plan_path_.clear(); } +void Navigation::ResetNavGoals() { + nav_state_ = NavigationState::kStopped; + nav_goal_loc_ = robot_loc_; + nav_goal_angle_ = robot_angle_; + local_target_.setZero(); + plan_path_.clear(); +} void Navigation::SetOverride(const Vector2f& loc, float angle) { nav_state_ = NavigationState::kOverride; diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h index af465e0..d1600d0 100644 --- a/src/navigation/navigation.h +++ b/src/navigation/navigation.h @@ -98,6 +98,7 @@ class Navigation { float* clearance, Eigen::Vector2f* obstruction); void SetNavGoal(const Eigen::Vector2f& loc, float angle); + void ResetNavGoals(); void SetOverride(const Eigen::Vector2f& loc, float angle); void Resume(); bool PlanStillValid(); diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 95ed9a3..c73486c 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -225,6 +225,11 @@ void GoToCallback(const geometry_msgs::PoseStamped& msg) { const Vector2f loc(msg.pose.position.x, msg.pose.position.y); const float angle = 2.0 * atan2(msg.pose.orientation.z, msg.pose.orientation.w); + if (loc.x() == 1111.0 && loc.y() == 1111.0) { + printf("Resetting all nav goals.\n"); + navigation_.ResetNavGoals(); + return; + } printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), angle); navigation_.SetNavGoal(loc, angle); navigation_.Resume(); @@ -232,6 +237,11 @@ void GoToCallback(const geometry_msgs::PoseStamped& msg) { void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) { const Vector2f loc(msg.pose.x, msg.pose.y); + if (loc.x() == 1111.0 && loc.y() == 1111.0 && msg.pose.theta == 1111.0) { + printf("Resetting all nav goals.\n"); + navigation_.ResetNavGoals(); + return; + } printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), msg.pose.theta); navigation_.SetNavGoal(loc, msg.pose.theta); navigation_.Resume(); @@ -870,8 +880,10 @@ int main(int argc, char** argv) { // Publish Visualizations PublishForwardPredictedPCL(navigation_.GetPredictedCloud()); DrawRobot(); - DrawTarget(); - DrawPathOptions(); + if (navigation_.GetNavStatusUint8() != static_cast(navigation::NavigationState::kStopped)) { + DrawTarget(); + DrawPathOptions(); + } PublishVisualizationMarkers(); PublishPath(); local_viz_msg_.header.stamp = ros::Time::now(); From ba69b9c3278328517bf2974460bccb75b494d59b Mon Sep 17 00:00:00 2001 From: Sadanand Modak Date: Fri, 3 Nov 2023 13:22:47 -0500 Subject: [PATCH 2/2] reset sub added --- src/navigation/navigation_main.cc | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index c73486c..9b427bc 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -65,6 +65,7 @@ #include "shared/util/helpers.h" #include "shared/ros/ros_helpers.h" #include "std_msgs/Bool.h" +#include "std_msgs/Empty.h" #include "tf/transform_broadcaster.h" #include "tf/transform_datatypes.h" #include "visualization/visualization.h" @@ -225,11 +226,6 @@ void GoToCallback(const geometry_msgs::PoseStamped& msg) { const Vector2f loc(msg.pose.position.x, msg.pose.position.y); const float angle = 2.0 * atan2(msg.pose.orientation.z, msg.pose.orientation.w); - if (loc.x() == 1111.0 && loc.y() == 1111.0) { - printf("Resetting all nav goals.\n"); - navigation_.ResetNavGoals(); - return; - } printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), angle); navigation_.SetNavGoal(loc, angle); navigation_.Resume(); @@ -237,16 +233,16 @@ void GoToCallback(const geometry_msgs::PoseStamped& msg) { void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) { const Vector2f loc(msg.pose.x, msg.pose.y); - if (loc.x() == 1111.0 && loc.y() == 1111.0 && msg.pose.theta == 1111.0) { - printf("Resetting all nav goals.\n"); - navigation_.ResetNavGoals(); - return; - } printf("Goal: (%f,%f) %f\u00b0\n", loc.x(), loc.y(), msg.pose.theta); navigation_.SetNavGoal(loc, msg.pose.theta); navigation_.Resume(); } +void ResetNavGoalsCallback(const std_msgs::Empty& msg) { + printf("Resetting all nav goals.\n"); + navigation_.ResetNavGoals(); +} + bool PlanServiceCb(graphNavSrv::Request &req, graphNavSrv::Response &res) { const Vector2f start(req.start.x, req.start.y); @@ -848,6 +844,8 @@ int main(int argc, char** argv) { n.subscribe("/move_base_simple/goal", 1, &GoToCallback); ros::Subscriber goto_amrl_sub = n.subscribe("/move_base_simple/goal_amrl", 1, &GoToCallbackAMRL); + ros::Subscriber reset_nav_goals_sub = + n.subscribe("/reset_nav_goals", 1, &ResetNavGoalsCallback); ros::Subscriber enabler_sub = n.subscribe(CONFIG_enable_topic, 1, &EnablerCallback); ros::Subscriber halt_sub =