add goal angle set support

orb_track.py

@@ -38,6 +38,7 @@
 from nav_msgs.msg import Odometry
 from std_msgs.msg import Bool, Int16, Int32
 from system_monitor.msg import Status
+from galileo_serial_server.msg import GalileoNativeCmds, GalileoStatus
 
 ORB_TRACK_THREAD = None
 ORB_INIT_FLAG = False
@@ -60,7 +61,11 @@
 
 ENABLE_MOVE_FLAG = True
 
+WORKING_FLAG = False
 
+time2_diff_value = 0.0
+
+ORB_INIT_FLAG2 = False
 class TrackTask(threading.Thread):
 
     def __init__(self):
@@ -120,52 +125,90 @@ def car_odom(odom):
 
 
 def camera_odom(campose):
-    global CAMERA_CURRENT_TIME, STATUS_LOCK, CAMERA_UPDATE_FLAG
+    global CAMERA_CURRENT_TIME, STATUS_LOCK, CAMERA_UPDATE_FLAG,ORB_INIT_FLAG2
     with STATUS_LOCK:
         CAMERA_CURRENT_TIME = rospy.Time.now()
         CAMERA_UPDATE_FLAG = True
-
+        ORB_INIT_FLAG2 = True
 
 def do_security():
     global STATUS_LOCK, CAR_UPDATE_FLAG, CAMERA_UPDATE_FLAG
     global CAMERA_CURRENT_TIME, CAR_CURRENT_TIME
     global GLOBAL_MOVE_PUB, VEL_PUB
     global NAV_FLAG_PUB, ENABLE_MOVE_FLAG, BAR_FLAG
+    global time2_diff_value,ORB_INIT_FLAG2
 
     time_now = rospy.Time.now()
 
     STATUS_LOCK.acquire()
 
-    if CAR_UPDATE_FLAG:
+    time1_diff = time_now - time_now
+
+    if not CAR_UPDATE_FLAG :
         time1_diff = time_now - CAR_CURRENT_TIME
 
-    if CAMERA_UPDATE_FLAG:
+    if not CAMERA_UPDATE_FLAG:
         time2_diff = time_now - CAMERA_CURRENT_TIME
 
+        if time2_diff.to_sec()>1.0 :
+            time2_diff_value = time2_diff_value + time2_diff.to_sec()
+            CAMERA_CURRENT_TIME = time_now
+    else:
+        time2_diff_value = 0.0
+
+    CAMERA_UPDATE_FLAG = False
+    CAR_UPDATE_FLAG = False
     # 视觉丢失超时保险
     # 里程计丢失超时保险
-    if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.:
+    if ORB_INIT_FLAG2 and ( time2_diff_value > 120 or time1_diff.to_sec() > 120.) and not BAR_FLAG and WORKING_FLAG:
         # 发布全局禁止flag
         global_move_flag = Bool()
         global_move_flag.data = False
         GLOBAL_MOVE_PUB.publish(global_move_flag)
         car_twist = Twist()
         VEL_PUB.publish(car_twist)
-        CAMERA_UPDATE_FLAG = False
-        CAR_UPDATE_FLAG = False
+        ORB_INIT_FLAG2 = False
+        time2_diff_value = 0.0
         CAMERA_CURRENT_TIME = rospy.Time.now()
         CAR_CURRENT_TIME = rospy.Time.now()
+        rospy.logwarn("oups3 %d %d %f %f",BAR_FLAG,WORKING_FLAG,time2_diff.to_sec(),time1_diff.to_sec())
     STATUS_LOCK.release()
 
 
 def deal_car_status(car_status):
-    global BAR_FLAG
-    status = car_status.data
-    if status == 2:
-        BAR_FLAG = True
-    else:
-        BAR_FLAG = False
+    global BAR_FLAG, STATUS_LOCK,CAMERA_CURRENT_TIME
+    with STATUS_LOCK:
+        status = car_status.data
+        if status == 2:
+            BAR_FLAG = True
+            CAMERA_CURRENT_TIME = rospy.Time.now()
+        else:
+            BAR_FLAG = False
+
+def deal_car_status(car_twist_msg):
+    global BAR_FLAG, STATUS_LOCK,CAMERA_CURRENT_TIME
+    with STATUS_LOCK:
+        vx_temp = car_twist_msg.linear.x
+        theta_temp = car_twist_msg.angular.z
+
+        if abs(vx_temp) < 0.05 and abs(theta_temp) <0.05:
+            BAR_FLAG = True
+            CAMERA_CURRENT_TIME = rospy.Time.now()
+        else:
+            BAR_FLAG = False
+            #rospy.logwarn("robot stopped")
+
+
+def deal_galileo_status(galileo_msg):
+    global WORKING_FLAG, STATUS_LOCK,CAR_CURRENT_TIME,CAMERA_CURRENT_TIME
+    with STATUS_LOCK:
 
+        if galileo_msg.targetStatus == 1:
+            WORKING_FLAG = True
+        else:
+            WORKING_FLAG = False
+            CAR_CURRENT_TIME = rospy.Time.now()
+            CAMERA_CURRENT_TIME = rospy.Time.now()
 
 def init():
     global ORB_INIT_FLAG, ORB_START_FLAG
@@ -183,7 +226,9 @@ def init():
     rospy.Subscriber("/system_monitor/report", Status, system_status_handler)
     rospy.Subscriber("/ORB_SLAM/Camera", Pose, camera_odom)
     rospy.Subscriber("/xqserial_server/Odom", Odometry, car_odom)
-    rospy.Subscriber("/xqserial_server/StatusFlag", Int32, deal_car_status)
+    #rospy.Subscriber("/xqserial_server/StatusFlag", Int32, deal_car_status)
+    rospy.Subscriber("/xqserial_server/Twist", Twist, deal_car_status)
+    rospy.Subscriber("/galileo/status", GalileoStatus, deal_galileo_status)
     GLOBAL_MOVE_PUB = rospy.Publisher('/global_move_flag', Bool, queue_size=1)
     NAV_FLAG_PUB = rospy.Publisher('/nav_setStop', Bool, queue_size=0)
     VEL_PUB = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

status_audio_reporter.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python
+# encoding=utf-8
+
+import rospy
+from std_msgs.msg import String
+from galileo_serial_server.msg import GalileoStatus
+import time
+import rosservice
+import subprocess
+import os
+
+PREVISOUS_STATUS = None
+BLOCK_TIME_COUNT = 0
+WORKING_TIME_COUNT = 0
+STOP_TIME_COUNT = 0
+PREVIOUS_GREETING_FLAG = False
+
+if __name__ == "__main__":
+    rospy.init_node("status_audio_reporter")
+    audio_pub = rospy.Publisher("/xiaoqiang_tts/text", String, queue_size=10)
+
+    def status_update_cb(status):
+        global PREVISOUS_STATUS, PREVIOUS_GREETING_FLAG, BLOCK_TIME_COUNT, STOP_TIME_COUNT, WORKING_TIME_COUNT
+        if PREVISOUS_STATUS == None:
+            PREVISOUS_STATUS = status
+            PREVIOUS_GREETING_FLAG = rospy.get_param(
+                "/xiaoqiang_greeting_node/is_enabled", False)
+            return
+        if PREVISOUS_STATUS.chargeStatus != 1 and status.chargeStatus == 1:
+            audio_pub.publish("开始充电")
+        if PREVISOUS_STATUS.chargeStatus != 2 and status.chargeStatus == 2:
+            audio_pub.publish("充电完成")
+        if PREVISOUS_STATUS.chargeStatus == 1 and status.chargeStatus == 0:
+            audio_pub.publish("停止充电")
+        if PREVISOUS_STATUS.mapStatus != 1 and status.mapStatus == 1:
+            audio_pub.publish("开始创建地图")
+        if PREVISOUS_STATUS.navStatus != 1 and status.navStatus == 1:
+            audio_pub.publish("开始导航")
+        if not PREVIOUS_GREETING_FLAG and rospy.get_param("/xiaoqiang_greeting_node/is_enabled", False):
+            audio_pub.publish("开启迎宾模式")
+        if PREVIOUS_GREETING_FLAG and not rospy.get_param("/xiaoqiang_greeting_node/is_enabled", False):
+            audio_pub.publish("关闭迎宾模式")
+        # 被挡住提示
+        if status.targetStatus == 1 and abs(status.currentSpeedX) < 0.01 and abs(status.currentSpeedTheta) < 0.01:
+            # 被人挡住了,在 WORKING 状态但是没有动
+            BLOCK_TIME_COUNT += (1000 / 30)
+        else:
+            BLOCK_TIME_COUNT = 0
+        if BLOCK_TIME_COUNT >= 5000:  # 等待5秒
+            BLOCK_TIME_COUNT = -10000  # 每15秒说一次
+            audio_pub.publish("您好，请让一下。赤兔机器人努力工作中")
+
+        # 巡航提示
+        if status.targetStatus == 1 and (abs(status.currentSpeedX) > 0.01 or abs(status.currentSpeedTheta) > 0.01):
+            # 被人挡住了,在 WORKING 状态但是没有动
+            WORKING_TIME_COUNT += (1000 / 30)
+        else:
+            WORKING_TIME_COUNT = 0
+        if WORKING_TIME_COUNT >= 4000:  # 等待2秒
+            WORKING_TIME_COUNT = 0  # 每2秒说一次
+            audio_pub.publish("巡 逻 中 ")
+
+        # 5min不动则关闭雷达
+        if abs(status.currentSpeedX) < 0.01 and abs(status.currentSpeedTheta) < 0.01:
+            STOP_TIME_COUNT += (1000 / 30)
+        else:
+            STOP_TIME_COUNT = 0
+        new_env = os.environ.copy()
+        if STOP_TIME_COUNT >= 5 * 60 * 1000:
+            if rospy.get_param("/rplidar_node_manager/keep_running", True):
+                rospy.set_param("/rplidar_node_manager/keep_running", False)
+                if rosservice.get_service_node("/stop_motor") is not None:
+                    cmd = "rosservice call /stop_motor"
+                    subprocess.Popen(
+                        cmd, shell=True, env=new_env)
+        else:
+            if not rospy.get_param("/rplidar_node_manager/keep_running", True):
+                rospy.set_param("/rplidar_node_manager/keep_running", True)
+                if rosservice.get_service_node("/start_motor") is not None:
+                    cmd = "rosservice call /start_motor"
+                    subprocess.Popen(
+                        cmd, shell=True, env=new_env)
+
+        PREVIOUS_GREETING_FLAG = rospy.get_param(
+            "/xiaoqiang_greeting_node/is_enabled", False)
+        PREVISOUS_STATUS = status
+
+    status_sub = rospy.Subscriber(
+        "/galileo/status", GalileoStatus, status_update_cb)
+
+    while not rospy.is_shutdown():
+        time.sleep(1)