update decision

66Lau · Nov 11, 2023 · 29fe731 · 29fe731
1 parent af461d8
commit 29fe731
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 3 deletions.
diff --git a/sentry_decision/src/random_deci.cpp b/sentry_decision/src/random_deci.cpp
@@ -39,9 +39,10 @@ void callback(const nav_msgs::Odometry& odom)
     // std::cout << imu.orientation.x << std::endl;
     // std::cout << imu.orientation.y << std::endl;
 
-    double distance = abs(odom.pose.pose.position.x-Goal.pose.position.x)+abs(odom.pose.pose.position.y-Goal.pose.position.y);
+    distance = abs(odom.pose.pose.position.x-Goal.pose.position.x)+abs(odom.pose.pose.position.y-Goal.pose.position.y);
+    publish_flag = distance < trigger_distance;
 
-    if (distance < trigger_distance){
+    if (publish_flag ){
         Goal.header.frame_id = "map";
         Goal.pose.position.x = generateRandomNumber(min_x_range, max_x_range);
         Goal.pose.position.y = generateRandomNumber(min_y_range, max_y_range);
@@ -80,7 +81,8 @@ int main (int argc, char** argv){
 
     while(ros::ok()){
         ros::spinOnce();
-        goal_pub.publish(Goal);
+        if (publish_flag ){
+            goal_pub.publish(Goal);}
         r.sleep();}
 
 }

diff --git a/sentry_decision/src/random_deci.h b/sentry_decision/src/random_deci.h
@@ -13,4 +13,7 @@ float min_y_range = -1.5;
 
 float trigger_distance = 0.8;
 
+double distance;
+bool publish_flag = true;
+
 #endif