From 22b4f208a29bcb7d277b59a820ecab215eb0ee2c Mon Sep 17 00:00:00 2001
From: jieshoudaxue <1641395022@qq.com>
Date: Wed, 13 Dec 2023 11:27:14 +0800
Subject: [PATCH] [fix] up it

---
 robot_hunt_maze/launch/go_maze.launch     |  7 +++
 robot_hunt_maze/scripts/exploring_maze.py | 41 +++++++++++++----
 robot_hunt_maze/scripts/maze_detect.py    |  2 +-
 robot_hunt_maze/scripts/move_to_target.py | 56 ++++++++++++++++++-----
 4 files changed, 86 insertions(+), 20 deletions(-)

diff --git a/robot_hunt_maze/launch/go_maze.launch b/robot_hunt_maze/launch/go_maze.launch
index 775d702..00c6309 100644
--- a/robot_hunt_maze/launch/go_maze.launch
+++ b/robot_hunt_maze/launch/go_maze.launch
@@ -14,6 +14,13 @@
       output="screen"
   /> -->
 
+  <node
+      pkg="robot_hunt_maze"
+      type="maze_detect.py"
+      name="maze_detect"
+      output="screen"
+  />
+
     <node
         pkg="robot_hunt_maze"
         type="move_to_target.py"
diff --git a/robot_hunt_maze/scripts/exploring_maze.py b/robot_hunt_maze/scripts/exploring_maze.py
index f121c49..afe0b01 100755
--- a/robot_hunt_maze/scripts/exploring_maze.py
+++ b/robot_hunt_maze/scripts/exploring_maze.py
@@ -5,24 +5,49 @@ import actionlib
 from actionlib_msgs.msg import *  
 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
+from std_msgs.msg import Int8
+from functools import partial
+
+STATUS_EXPLORING    = 0
+STATUS_CLOSE_TARGET = 1
+STATUS_GO_HOME      = 2
+
+target_list = []
+target_list.append(Pose(Point(0, 8, 0), Quaternion(0, 0, 0, 1.0)))
+target_list.append(Pose(Point(8, 8, 0), Quaternion(0, 0, 0, 1.0)))
+target_list.append(Pose(Point(8, 0, 0), Quaternion(0, 0, 0, 1.0)))
+target_list.append(Pose(Point(2.5, 4.8, 0), Quaternion(0, 0, 0, 1.0)))
+target_list.append(Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1.0)))
+
+def state_cb(state, move_base_client):
+  rospy.loginfo("receive state is %d" state.data)
+  
+  if state.data == STATUS_CLOSE_TARGET:
+    move_base_client.cancel_goal()
+    rospy.loginfo("stop exploring")
+  elif state.data == STATUS_GO_HOME:
+    goal = MoveBaseGoal()
+    goal.target_pose.pose = target_list[-1]
+    goal.target_pose.header.frame_id = 'map'
+    goal.target_pose.header.stamp = rospy.Time.now()    
+
+    move_base_client.send_goal(goal)
+    move_base_client.wait_for_result(rospy.Duration(300))
+    if move_base_client.get_state() == GoalStatus.SUCCEEDED:
+      rospy.loginfo("go home successed")
 
 
 def main():
   rospy.init_node("exploring_maze", anonymous=True)
-  
   move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
   
   rospy.loginfo("Waiting for move_base action server...")  
   
   while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
     rospy.loginfo("connected to move base server")
-  
-  target_list = []
-  target_list.append(Pose(Point(0, 8, 0), Quaternion(0, 0, 0, 1.0)))
-  target_list.append(Pose(Point(8, 8, 0), Quaternion(0, 0, 0, 1.0)))
-  target_list.append(Pose(Point(8, 0, 0), Quaternion(0, 0, 0, 1.0)))
-  target_list.append(Pose(Point(2.5, 4.8, 0), Quaternion(0, 0, 0, 1.0)))
-  target_list.append(Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1.0)))
+
+  bind_state_cb = partial(state_cb, move_base_client=move_base_client)
+  rospy.Subscriber("/state_cmd", Int8, bind_state_cb)
 
   for i, target in enumerate(target_list):
     start_time = rospy.Time.now()  
diff --git a/robot_hunt_maze/scripts/maze_detect.py b/robot_hunt_maze/scripts/maze_detect.py
index 764857a..a1b598e 100644
--- a/robot_hunt_maze/scripts/maze_detect.py
+++ b/robot_hunt_maze/scripts/maze_detect.py
@@ -73,7 +73,7 @@ def main():
     
     bind_image_cb = partial(image_cb, cv_bridge=bridge, image_pub=image_pub, maze_pose_pub=maze_pose_pub)
 
-    rospy.Subscriber("/usb_cam/image_raw", Image, bind_image_cb)
+    rospy.Subscriber("/camera/image_raw", Image, bind_image_cb)
 
     
     rospy.spin()
diff --git a/robot_hunt_maze/scripts/move_to_target.py b/robot_hunt_maze/scripts/move_to_target.py
index 75ba9b9..3fe2ced 100644
--- a/robot_hunt_maze/scripts/move_to_target.py
+++ b/robot_hunt_maze/scripts/move_to_target.py
@@ -8,31 +8,65 @@ import actionlib
 from actionlib_msgs.msg import *  
 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
+from std_msgs.msg import Int8
+
+STATUS_EXPLORING    = 0
+STATUS_CLOSE_TARGET = 1
+STATUS_GO_HOME      = 2
+GET_TARGET_SIZE     = 90000
+
+status_flag         = STATUS_EXPLORING
 
 
-def pose_cb(pose, cmd_pub):
+def pose_cb(pose, state_pub, cmd_pub, voice_client):
   rospy.loginfo("Target pose: x:%0.6f, y:%0.6f, z:%0.6f" %(pose.position.x, pose.position.y, pose.position.z))
+
+  if status_flag == STATUS_EXPLORING:
+    status_flag = STATUS_CLOSE_TARGET
+    
+    state_msg = Int8()
+    state_msg.data = status_flag
+    state_pub.publish(state_msg)
+
+    req = StringToVoiceRequest()
+    req.data = "发现宝藏了，跑过去拿"
+    resp = voice_client(req)
+    rospy.loginfo("send txt %s, resp %d" %(req.data, resp.success))    
+    
+  elif status_flag == STATUS_CLOSE_TARGET and pose.position.z > GET_TARGET_SIZE:
+    status_flag = STATUS_GO_HOME
+    
+    state_msg = Int8()
+    state_msg.data = status_flag
+    state_pub.publish(state_msg)
+
+    req = StringToVoiceRequest()
+    req.data = "拿到宝藏了，回家数数"
+    resp = voice_client(req)
+    rospy.loginfo("send txt %s, resp %d" %(req.data, resp.success))        
     
     
+  elif status_flag == STATUS_CLOSE_TARGET:
+    vel_msg = Twist()
+    vel_msg.linear.x = (100000 - pose.position.z) / 100000 * 0.3
+    vel_msg.angular.z = (640 - pose.position.x) / 640 * 0.3
+
+    cmd_pub.publish(vel_msg)
+  
 
 
 def main():
   rospy.init_node("move_to_target", anonymous=True)
 
-
-
   cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
-  bind_pose_cb = partial(pose_cb, cmd_pub=cmd_pub)
-  rospy.Subscriber("/maze_pose", Pose, bind_pose_cb)  
-
-
+  state_pub = rospy.Publisher("/state_cmd", Int8, queue_size=1)
+  
   voice_client = rospy.ServiceProxy("str2voice", StringToVoice)
   rospy.wait_for_service("str2voice")  
 
-  req = StringToVoiceRequest()
-  req.data = "找到宝藏了，现在回家"
-  resp = voice_client(req)
-  print("send txt %s, resp %d" %(req.data, resp.success))
+
+  bind_pose_cb = partial(pose_cb, state_pub=state_pub, cmd_pub=cmd_pub, voice_client=voice_client)
+  rospy.Subscriber("/maze_pose", Pose, bind_pose_cb)  
 
   rospy.spin()