From 74727e7c01391b29ff0f49055dc73d90540ba3cf Mon Sep 17 00:00:00 2001 From: xiaoqiang Date: Sat, 22 Dec 2018 11:35:32 +0800 Subject: [PATCH] fix galileo status to 30hz and refactor --- monitor.py | 6 +- orb_track.py | 7 +- remote_server.py | 334 +++++--------------------------- utils/galileo_status_service.py | 234 ++++++++++++++++++++++ utils/map_service.py | 14 +- utils/monitor_server.py | 77 +------- utils/nav_task.py | 18 +- utils/navigation_service.py | 14 +- utils/scale_orb.py | 10 +- utils/udp_status_service.py | 155 +++++++++++++++ 10 files changed, 484 insertions(+), 385 deletions(-) create mode 100644 utils/galileo_status_service.py create mode 100644 utils/udp_status_service.py diff --git a/monitor.py b/monitor.py index 7d7b565..1af79cf 100755 --- a/monitor.py +++ b/monitor.py @@ -36,7 +36,7 @@ import rospy from geometry_msgs.msg import Pose from nav_msgs.msg import Odometry -from std_msgs.msg import Bool, Float64, UInt32 +from std_msgs.msg import Bool, Float64, UInt32, Int32 from system_monitor.msg import Status from xiaoqiang_log.msg import LogRecord @@ -114,8 +114,8 @@ def monitor(): global REPORT_PUB rospy.init_node("monitor", anonymous=True) rospy.Subscriber("/usb_cam/brightness", UInt32, get_brightness) - rospy.Subscriber("/usb_cam/image_raw", rospy.AnyMsg, get_image) - rospy.Subscriber("/ORB_SLAM/Frame", rospy.AnyMsg, get_orb_start_status) + rospy.Subscriber("/usb_cam/camera_info", rospy.AnyMsg, get_image) + rospy.Subscriber("/ORB_SLAM/TrackingStatus", Int32, get_orb_start_status) rospy.Subscriber("/xqserial_server/Power", Float64, get_power) rospy.Subscriber("/xqserial_server/Odom", Odometry, get_odom) rospy.Subscriber("/ORB_SLAM/Camera", Pose, get_orb_track_flag) diff --git a/orb_track.py b/orb_track.py index 7063a97..3251f4b 100755 --- a/orb_track.py +++ b/orb_track.py @@ -145,12 +145,7 @@ def do_security(): # 视觉丢失超时保险 # 里程计丢失超时保险 if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.: - # 发布navFlag - if NAV_FLAG_PUB != None: - nav_flag = Bool() - nav_flag.data = True - NAV_FLAG_PUB.publish(nav_flag) - # 发布全局禁止ｆｌａｇ + # 发布全局禁止flag global_move_flag = Bool() global_move_flag.data = False GLOBAL_MOVE_PUB.publish(global_move_flag) diff --git a/remote_server.py b/remote_server.py index a255a29..c65b5ad 100755 --- a/remote_server.py +++ b/remote_server.py @@ -25,159 +25,51 @@ # Author: Randoms, Xiefusheng # +import os import struct +import subprocess import threading from socket import AF_INET, SO_BROADCAST, SOCK_DGRAM, SOL_SOCKET, socket import numpy as np +import psutil import rospy import tf -import os -import psutil -import subprocess from galileo_serial_server.msg import GalileoNativeCmds, GalileoStatus from geometry_msgs.msg import Pose, PoseStamped, Twist from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool, Float64, Int16, Int32, UInt32 from system_monitor.msg import Status +from tf.transformations import euler_from_quaternion -from utils.config import BROADCAST_PORT, TF_ROT, TF_TRANS,ROS_PACKAGE_PATH +from utils.config import BROADCAST_PORT, ROS_PACKAGE_PATH, TF_ROT, TF_TRANS +from utils.galileo_status_service import GalileoStatusService from utils.monitor_server import MonitorServer -from sensor_msgs.msg import LaserScan +from utils.udp_status_service import UDPStatusService +import rosservice -ROBOT_STATUS = Status() -ROBOT_STATUS.brightness = 0.0 -ROBOT_STATUS.imageStatus = False -ROBOT_STATUS.odomStatus = False -ROBOT_STATUS.orbStartStatus = False -ROBOT_STATUS.orbInitStatus = False -ROBOT_STATUS.power = 0.0 -ROBOT_STATUS.orbScaleStatus = False - -ROBOT_CONTROL_TWIST = None -ROBOT_REAL_TWIST = None - -ROBOT_STATUS_LOCK = threading.Lock() - -ROBOT_POSESTAMPED = None -CHARGE_STATUS = UInt32() - -SEND_DATA = bytearray( - [205, 235, 215, 36, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00,]) - -CONTROL_FLAG = False TILT_PUB = None NAV_LASTTIME = None - -T_HF = [0.0, 0.0, 0.0] -Q_HF = [0.0, 0.0, 0.0, 1.0] - rplidar_flag = False -def get_power(power): - with ROBOT_STATUS_LOCK: - ROBOT_STATUS.power = power.data - - -def get_image(image): - with ROBOT_STATUS_LOCK: - if image != None: - ROBOT_STATUS.imageStatus = True - else: - ROBOT_STATUS.imageStatus = False - - -def get_odom(odom): - global ROBOT_POSESTAMPED, ROBOT_REAL_TWIST, ROBOT_STATUS - with ROBOT_STATUS_LOCK: - if odom != None: - ROBOT_STATUS.odomStatus = True - ROBOT_POSESTAMPED = PoseStamped() - ROBOT_POSESTAMPED.pose = odom.pose.pose # 更新坐标 - ROBOT_POSESTAMPED.header = odom.header # 更新坐标 - ROBOT_REAL_TWIST = odom.twist - else: - ROBOT_STATUS.odomStatus = False - - -def get_cmd_vel(twist): - global ROBOT_CONTROL_TWIST - if twist != None: - ROBOT_CONTROL_TWIST = twist - - -def get_orb_start_status(orb_frame): - with ROBOT_STATUS_LOCK: - if orb_frame != None: - ROBOT_STATUS.orbStartStatus = True - else: - ROBOT_STATUS.orbStartStatus = False - - -def get_orb_tracking_flag(cam_pose): - with ROBOT_STATUS_LOCK: - if cam_pose != None: - ROBOT_STATUS.orbInitStatus = True - else: - ROBOT_STATUS.orbInitStatus = False - - def get_global_move_flag(moveEn): if not moveEn.data: # 关闭视觉导航 if not monitor_server.nav_thread.stopped(): monitor_server.nav_thread.stop() - monitor_server.nav_flag = False - - -def get_nav_flag(navRun): - if navRun.data and not monitor_server.nav_thread.stopped(): - monitor_server.nav_flag = True - else: - monitor_server.nav_flag = False - - -def get_orbgc_status(gc_flag): - with ROBOT_STATUS_LOCK: - ROBOT_STATUS.orbGCFlag = gc_flag.data - - -def get_orbgba_status(gba_flag): - with ROBOT_STATUS_LOCK: - ROBOT_STATUS.orbGBAFlag = gba_flag.data - - -def get_charge_status(charge_status): - global CHARGE_STATUS - with ROBOT_STATUS_LOCK: - CHARGE_STATUS = charge_status - + if monitor_server.nav_task is not None: + monitor_server.nav_task.shutdown() + monitor_server.nav_task = None def get_scan(scan): global rplidar_flag - if scan != None: - rplidar_flag = True - #print "get scan" + rplidar_flag = True def init_sub_pubs(): rospy.init_node("remote_server", anonymous=True) - rospy.Subscriber("/xqserial_server/Power", Float64, get_power) - rospy.Subscriber("/usb_cam/image_raw", rospy.AnyMsg, get_image) - rospy.Subscriber("/bWmono/Odom", Odometry, get_odom) - rospy.Subscriber("/ORB_SLAM/Camera", Pose, get_orb_tracking_flag) - rospy.Subscriber("/ORB_SLAM/Frame", rospy.AnyMsg, get_orb_start_status) - rospy.Subscriber("/ORB_SLAM/GC", Bool, get_orbgc_status) - rospy.Subscriber("/ORB_SLAM/GBA", Bool, get_orbgba_status) rospy.Subscriber("/global_move_flag", Bool, get_global_move_flag) - rospy.Subscriber('/nav_setStop', Bool, get_nav_flag) - rospy.Subscriber('/cmd_vel', Twist, get_cmd_vel) - rospy.Subscriber('/bw_auto_dock/Chargestatus', Int32, get_charge_status) rospy.Subscriber('/scan', LaserScan, get_scan) GLOBAL_MOVE_PUB = rospy.Publisher('/global_move_flag', Bool, queue_size=1) ELEVATOR_PUB = rospy.Publisher('/elevatorPose', UInt32, queue_size=1) @@ -209,185 +101,62 @@ def init_sub_pubs(): s = socket(AF_INET, SOCK_DGRAM) s.bind(('', 0)) s.setsockopt(SOL_SOCKET, SO_BROADCAST, 1) - # 开启udp接收监听线程 - monitor_server = MonitorServer(pubs, ROBOT_STATUS_LOCK, ROBOT_STATUS) + + # 配置全局系统状态 + galileo_status = GalileoStatus() + galileo_status_lock = threading.Lock() + + # 伽利略指令处理线程 + monitor_server = MonitorServer(pubs, galileo_status, galileo_status_lock) monitor_server.start() + + # 伽利略系统状态发布线程 + galileo_status_service = GalileoStatusService( + pubs["GALILEO_STATUS_PUB"], monitor_server, galileo_status, galileo_status_lock) + galileo_status_service.start() + + # UDP系统状态发布线程 + udp_status_service = UDPStatusService(monitor_server, galileo_status_service.galileo_status, + galileo_status_service.galileo_status_lock) + udp_status_service.start() + broadcast_count = 10 # 每1秒播放一次声音 heart_beat_count = 40 # 每4秒心跳维护一次 - listener = tf.TransformListener(True, rospy.Duration(10.0)) - t_ac = np.array([0., 0., 0.]) - theta_send = 0. + sub_process_thread = None sub_process_thread_ps_process = None while not rospy.is_shutdown(): - if heart_beat_count == 40: heart_beat_count = 0 - CONTROL_FLAG = False heart_beat_count += 1 - # 持续反馈状态 - if monitor_server.get_connection_status(): - if ROBOT_STATUS.odomStatus and ROBOT_POSESTAMPED is not None: - # 将map坐标系转换成ORB_SLAM/World坐标系 - current_pose = ROBOT_POSESTAMPED.pose - t_bc = np.array( - [current_pose.position.x, current_pose.position.y, current_pose.position.z]) - q = [current_pose.orientation.x, current_pose.orientation.y, - current_pose.orientation.z, current_pose.orientation.w] - m = tf.transformations.quaternion_matrix(q) - r_bc = m[:3, :3] - - ax, ay, theta_send = tf.transformations.euler_from_matrix(r_bc) - # 为了简化计算，下文的计算中 base_link 和 base_footprint 被看成是相同的坐标系 - - r_ab = TF_ROT.T - t_ab = -r_ab.dot(TF_TRANS) - - r_ac = r_ab.dot(r_bc) - t_ac = r_ab.dot(t_bc) + t_ab - - SEND_DATA[4:8] = map(ord, struct.pack('f', t_ac[0])) - SEND_DATA[8:12] = map(ord, struct.pack('f', t_ac[1])) - SEND_DATA[12:16] = map(ord, struct.pack('f', t_ac[2])) - SEND_DATA[16:20] = map(ord, struct.pack('f', ROBOT_STATUS.power)) - SEND_DATA[24:28] = map(ord, struct.pack('f', theta_send)) - if monitor_server.nav_task == None: - SEND_DATA[28:32] = map(ord, struct.pack('i', 3)) - SEND_DATA[32:36] = map(ord, struct.pack('i', -1)) - SEND_DATA[40:44] = map(ord, struct.pack('i', 0)) - else: - if monitor_server.nav_task.current_goal_status() == "FREE": - SEND_DATA[28:32] = map(ord, struct.pack('i', 0)) - SEND_DATA[32:36] = map(ord, struct.pack('i', -1)) - if monitor_server.nav_task.current_goal_status() == "WORKING": - SEND_DATA[28:32] = map(ord, struct.pack('i', 1)) - SEND_DATA[32:36] = map(ord, struct.pack('i', - monitor_server.nav_task.current_goal_id)) - if monitor_server.nav_task.current_goal_status() == "PAUSED": - SEND_DATA[28:32] = map(ord, struct.pack('i', 2)) - SEND_DATA[32:36] = map(ord, struct.pack('i', - monitor_server.nav_task.current_goal_id)) - SEND_DATA[40:44] = map(ord, struct.pack('i', monitor_server.nav_task.loop_running_flag)) - SEND_DATA[36:40] = map(ord, struct.pack('i', CHARGE_STATUS.data)) - - if monitor_server.nav_task != None: - statu0 = 0x01 # 导航状态 - else: - statu0 = 0x00 - if ROBOT_STATUS.imageStatus: - statu1 = 0x02 # 视觉摄像头 - else: - statu1 = 0x00 - if ROBOT_STATUS.orbStartStatus: - statu2 = 0x04 # 视觉系统状态 - else: - statu2 = 0x00 - if ROBOT_STATUS.orbInitStatus: - statu3 = 0x08 # 视觉系统状态 - # 已经track - if monitor_server.nav_task != None: - monitor_server.nav_task.track_init_flag = True - else: - statu3 = 0x00 - if ROBOT_STATUS.orbGCFlag: - status4 = 0x10 # ORB_SLAM 内存回收状态 - else: - status4 = 0 - if ROBOT_STATUS.orbGBAFlag: - status5 = 0x20 # ORB_SLAM2 GBA状态，一般对应LoopClosing - else: - status5 = 0x00 - SEND_DATA[20] = statu0 + statu1 + \ - statu2 + statu3 + status4 + status5 - SEND_DATA[3] = len(SEND_DATA) - 4 - monitor_server.sendto(bytes(SEND_DATA)) if heart_beat_count == 39: new_env = os.environ.copy() new_env['ROS_PACKAGE_PATH'] = ROS_PACKAGE_PATH - if sub_process_thread != None : - sub_process_thread_ps_process = psutil.Process(pid=sub_process_thread.pid) + if sub_process_thread != None: + sub_process_thread_ps_process = psutil.Process( + pid=sub_process_thread.pid) for child in sub_process_thread_ps_process.children(recursive=True): child.kill() sub_process_thread_ps_process.kill() sub_process_thread = None else: cmd = None - if ROBOT_STATUS.orbStartStatus and not rplidar_flag: - #打开雷达电机 - cmd = "rosservice call /start_motor" - sub_process_thread = subprocess.Popen(cmd, shell=True, env=new_env) - #print "start motor " + str(rplidar_flag) - elif rplidar_flag and not ROBOT_STATUS.orbStartStatus: - #关闭雷达电机 - cmd = "rosservice call /stop_motor" - sub_process_thread = subprocess.Popen(cmd, shell=True, env=new_env) - #print "stop motor " + str(rplidar_flag) + if galileo_status.navStatus == 1 and not rplidar_flag: + # 打开雷达电机 + if rosservice.get_service_node("/start_motor") is not None: + cmd = "rosservice call /start_motor" + sub_process_thread = subprocess.Popen( + cmd, shell=True, env=new_env) + elif rplidar_flag and galileo_status.navStatus == 0: + # 关闭雷达电机 + if rosservice.get_service_node("/stop_motor") is not None: + cmd = "rosservice call /stop_motor" + sub_process_thread = subprocess.Popen( + cmd, shell=True, env=new_env) if heart_beat_count == 1: rplidar_flag = False - # 发布状态topic - galileo_status = GalileoStatus() - galileo_status.loopStatus = 0 - if ROBOT_POSESTAMPED is not None: - galileo_status.header = ROBOT_POSESTAMPED.header - galileo_status.navStatus = 0 - if not ROBOT_STATUS.orbInitStatus: - galileo_status.visualStatus = 2 - - if monitor_server.nav_task != None and not monitor_server.nav_task.track_init_flag: - # 视觉未追踪状态，且从未追踪过 - galileo_status.visualStatus = 0 - else: - galileo_status.visualStatus = 1 - if monitor_server.nav_task != None: # 导航任务正在运行 - galileo_status.navStatus = 1 - else: - galileo_status.navStatus = 0 - galileo_status.power = ROBOT_STATUS.power - galileo_status.targetNumID = -1 - if monitor_server.nav_task != None: - galileo_status.targetNumID = monitor_server.nav_task.current_goal_id - galileo_status.loopStatus = monitor_server.nav_task.loop_running_flag - galileo_status.targetStatus = 0 - if monitor_server.nav_task != None: - if monitor_server.nav_task.current_goal_status() == "FREE": - galileo_status.targetStatus = 0 - if monitor_server.nav_task.current_goal_status() == "WORKING": - galileo_status.targetStatus = 1 - if monitor_server.nav_task.current_goal_status() == "PAUSED": - galileo_status.targetStatus = 2 - if monitor_server.nav_task.current_goal_status() == "ERROR": - galileo_status.targetStatus = -1 - galileo_status.targetDistance = -1 - if monitor_server.nav_task != None and \ - monitor_server.nav_task.current_goal_status() != "ERROR": - galileo_status.targetDistance = \ - monitor_server.nav_task.current_goal_distance() - galileo_status.angleGoalStatus = 1 - if ROBOT_CONTROL_TWIST is not None and ROBOT_REAL_TWIST is not None: - galileo_status.controlSpeedX = ROBOT_CONTROL_TWIST.linear.x - galileo_status.controlSpeedTheta = ROBOT_CONTROL_TWIST.angular.z - galileo_status.currentSpeedX = ROBOT_REAL_TWIST.twist.linear.x - galileo_status.currentSpeedTheta = ROBOT_REAL_TWIST.twist.angular.z - if monitor_server.map_thread.stopped(): - galileo_status.mapStatus = 0 - else: - galileo_status.mapStatus = 1 - if ROBOT_STATUS.orbGCFlag: - galileo_status.gcStatus = 1 - else: - galileo_status.gcStatus = 0 - if ROBOT_STATUS.orbGBAFlag: - galileo_status.gbaStatus = 1 - else: - galileo_status.gbaStatus = 0 - galileo_status.chargeStatus = CHARGE_STATUS.data - if galileo_status.header.stamp == rospy.Time(0): - galileo_status.header.stamp = rospy.Time.now() - - pubs["GALILEO_STATUS_PUB"].publish(galileo_status) - # 每秒广播一次 if broadcast_count == 10: broadcast_count = 0 @@ -397,13 +166,8 @@ def init_sub_pubs(): s.sendto(data, ('', BROADCAST_PORT)) except: continue - ROBOT_STATUS.brightness = 0.0 - ROBOT_STATUS.imageStatus = False - ROBOT_STATUS.odomStatus = False - ROBOT_STATUS.orbStartStatus = False - ROBOT_STATUS.orbInitStatus = False - ROBOT_STATUS.power = 0.0 - ROBOT_STATUS.orbScaleStatus = False broadcast_count += 1 rate.sleep() monitor_server.stop() + galileo_status_service.stop() + udp_status_service.stop() diff --git a/utils/galileo_status_service.py b/utils/galileo_status_service.py new file mode 100644 index 0000000..f3138d3 --- /dev/null +++ b/utils/galileo_status_service.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python +# encoding=utf-8 +# The MIT License (MIT) +# +# Copyright (c) 2018 Bluewhale Robot +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +# Author: Randoms +# + +import threading +import time + +import rospy +from galileo_serial_server.msg import GalileoStatus +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from std_msgs.msg import Bool, Float64, Int32 +from tf.transformations import euler_from_quaternion + + +class GalileoStatusService(threading.Thread): + + def __init__(self, galileo_pub, monitor_server, galileo_status, galileo_status_lock): + super(GalileoStatusService, self).__init__() + # status + self.galileo_pub = galileo_pub + self.monitor_server = monitor_server + self.visual_status = 0 + self.gc_status = 0 + self.gba_status = 0 + self.charge_status = 0 + self.power = 0 + self.control_speed_x = 0 + self.control_speed_theta = 0 + self.current_speed_x = 0 + self.current_speed_theta = 0 + self.visual_status_time = int(time.time() * 1000) + self.gc_status_time = int(time.time() * 1000) + self.gba_status_time = int(time.time() * 1000) + self.charge_status_time = int(time.time() * 1000) + self.power_time = int(time.time() * 1000) + self.control_speed_time = int(time.time() * 1000) + self.current_speed_time = int(time.time() * 1000) + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock + + # stop flag + self._stop = threading.Event() + self._stop.set() + + def update_visual_status(status): + self.visual_status_time = int(time.time() * 1000) + if status.data <= 1: + self.visual_status = 0 + if status.data == 2: + self.visual_status = 1 + if status.data == 3: + self.visual_status = 2 + + def update_gc_status(status): + self.gc_status_time = int(time.time() * 1000) + if status.data: + self.gc_status = 1 + else: + self.gc_status = 0 + + def update_gba_status(status): + self.gba_status_time = int(time.time() * 1000) + if status.data: + self.gba_status = 1 + else: + self.gba_status = 0 + + def update_charge_status(status): + self.charge_status_time = int(time.time() * 1000) + self.charge_status = status.data + + def update_power(power): + self.power_time = int(time.time() * 1000) + self.power = power.data + + def update_current_speed(odom): + self.current_speed_time = int(time.time() * 1000) + self.current_speed_x = odom.twist.twist.linear.x + self.current_speed_theta = odom.twist.twist.angular.z + + def update_control_speed(twist): + self.control_speed_time = int(time.time() * 1000) + self.control_speed_x = twist.linear.x + self.control_speed_theta = twist.angular.z + + self.visual_sub = rospy.Subscriber( + "/ORB_SLAM/TrackingStatus", Int32, update_visual_status) + self.gc_sub = rospy.Subscriber("/ORB_SLAM/GC", Bool, update_gc_status) + self.gba_sub = rospy.Subscriber( + "/ORB_SLAM/GBA", Bool, update_gba_status) + self.charge_sub = rospy.Subscriber( + "/bw_auto_dock/Chargestatus", Int32, update_charge_status) + self.power_sub = rospy.Subscriber( + "/xqserial_server/Power", Float64, update_power) + self.current_speed_sub = rospy.Subscriber( + "/cmd_vel", Twist, update_control_speed) + self.control_speed_sub = rospy.Subscriber( + "/xqserial_server/Odom", Odometry, update_current_speed) + + def stop(self): + self.visual_sub.unregister() + self.gc_sub.unregister() + self.gba_sub.unregister() + self.charge_sub.unregister() + self.power_sub.unregister() + self.current_speed_sub.unregister() + self.control_speed_sub.unregister() + self._stop.set() + + def stopped(self): + return self._stop.isSet() + + def run(self): + rate = rospy.Rate(30) + self._stop.clear() + while not self.stopped() and not rospy.is_shutdown(): + with self.galileo_status_lock: + if self.monitor_server.nav_task: + self.galileo_status.navStatus = 1 + else: + self.galileo_status.navStatus = 0 + self.galileo_status.visualStatus = self.visual_status + if self.monitor_server.map_thread.stopped(): + self.galileo_status.mapStatus = 0 + else: + self.galileo_status.mapStatus = 1 + self.galileo_status.gcStatus = self.gc_status + self.galileo_status.gbaStatus = self.gba_status + self.galileo_status.chargeStatus = self.charge_status + self.galileo_status.loopStatus = 0 + if self.monitor_server.nav_task != None: + self.galileo_status.loopStatus = self.monitor_server.nav_task.loop_running_flag + self.galileo_status.power = self.power + self.galileo_status.targetNumID = -1 + if self.monitor_server.nav_task != None: + self.galileo_status.targetNumID = self.monitor_server.nav_task.current_goal_id + + self.galileo_status.targetStatus = 0 + self.galileo_status.angleGoalStatus = 1 + if self.monitor_server.nav_task != None: + if self.monitor_server.nav_task.current_goal_status() == "FREE": + self.galileo_status.targetStatus = 0 + self.galileo_status.angleGoalStatus = 1 + if self.monitor_server.nav_task.current_goal_status() == "WORKING": + self.galileo_status.targetStatus = 1 + self.galileo_status.angleGoalStatus = 0 + if self.monitor_server.nav_task.current_goal_status() == "PAUSED": + self.galileo_status.targetStatus = 2 + if self.monitor_server.nav_task.current_goal_status() == "ERROR": + self.galileo_status.targetStatus = -1 + + self.galileo_status.targetDistance = -1 + if self.monitor_server.nav_task != None and \ + self.monitor_server.nav_task.current_goal_status() != "ERROR": + self.galileo_status.targetDistance = \ + self.monitor_server.nav_task.current_goal_distance() + if self.monitor_server.nav_task.update_pose() != -1: + self.galileo_status.currentPosX = self.monitor_server.nav_task.current_pose_stamped_map.pose.position.x + self.galileo_status.currentPosY = self.monitor_server.nav_task.current_pose_stamped_map.pose.position.y + current_oritation = self.monitor_server.nav_task.current_pose_stamped_map.pose.orientation + current_pose_q = [current_oritation.x, current_oritation.y, + current_oritation.z, current_oritation.w] + self.galileo_status.currentAngle = euler_from_quaternion(current_pose_q)[ + 2] + else: + self.galileo_status.currentPosX = -1 + self.galileo_status.currentPosY = -1 + self.galileo_status.currentAngle = -1 + else: + self.galileo_status.currentPosX = -1 + self.galileo_status.currentPosY = -1 + self.galileo_status.currentAngle = -1 + + self.galileo_status.controlSpeedX = self.control_speed_x + self.galileo_status.controlSpeedTheta = self.control_speed_theta + self.galileo_status.currentSpeedX = self.current_speed_x + self.galileo_status.currentSpeedTheta = self.current_speed_theta + self.galileo_status.header.frame_id = "map" + self.galileo_status.header.stamp = rospy.Time.now() + + # reset old status + now = int(time.time() * 1000) + if now - self.visual_status_time > 1000: + self.visual_status = -1 # 视觉程序未开始运行 + self.galileo_status.visualStatus = -1 + if now - self.gc_status_time > 1000: + self.gc_status = 0 + self.galileo_status.gcStatus = 0 + if now - self.gba_status_time > 1000: + self.gba_status = 0 + self.galileo_status.gbaStatus = 0 + if now - self.charge_status_time > 1000: + self.charge_status = 0 + self.galileo_status.chargeStatus = 0 + if now - self.power_time > 1000: + self.power = 0 + self.galileo_status.power = 0 + if now - self.control_speed_time > 1000: + self.control_speed_theta = 0 + self.control_speed_x = 0 + self.galileo_status.controlSpeedX = 0 + self.galileo_status.controlSpeedTheta = 0 + if now - self.current_speed_time > 1000: + self.current_speed_theta = 0 + self.current_speed_x = 0 + self.galileo_status.currentSpeedX = 0 + self.galileo_status.currentSpeedTheta = 0 + + self.galileo_pub.publish(self.galileo_status) + rate.sleep() diff --git a/utils/map_service.py b/utils/map_service.py index 40ff2df..5d247e8 100644 --- a/utils/map_service.py +++ b/utils/map_service.py @@ -39,16 +39,16 @@ class MapService(threading.Thread): # orb_slam建图线程 - def __init__(self, robot_status_lock, robot_status, update=False): + def __init__(self, galileo_status, galileo_status_lock, update=False): super(MapService, self).__init__() self._stop = threading.Event() self._stop.set() self.P = None self.ps_process = None self.scale_orb_thread = None - self.robot_status_lock = robot_status_lock - self.robot_status = robot_status self.update = update + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock def stop(self): if self.scale_orb_thread != None: @@ -62,7 +62,7 @@ def stop(self): self.ps_process.kill() self.P = None self._stop.set() - self.__init__(self.robot_status_lock, self.robot_status) + self.__init__(self.galileo_status, self.galileo_status_lock) def stopped(self): return self._stop.isSet() @@ -80,11 +80,9 @@ def run(self): self.ps_process = psutil.Process(pid=self.P.pid) else: if self.ps_process.is_running(): - self.robot_status_lock.acquire() - self.robot_status.orbStartStatus = True - self.robot_status_lock.release() if self.scale_orb_thread == None: - self.scale_orb_thread = ScaleORB(self.robot_status) + self.scale_orb_thread = ScaleORB( + self.galileo_status, self.galileo_status_lock) self.scale_orb_thread.start() else: if self.scale_orb_thread != None: diff --git a/utils/monitor_server.py b/utils/monitor_server.py index 8f8868b..4c665b1 100644 --- a/utils/monitor_server.py +++ b/utils/monitor_server.py @@ -52,7 +52,7 @@ class MonitorServer(threading.Thread): # 接收udp命令的socket - def __init__(self, pubs, robot_status_lock, robot_status, + def __init__(self, pubs, galileo_status, galileo_status_lock, host='', user_socket_port=20001, buf_size=1024): super(MonitorServer, self).__init__() self.host = host @@ -72,22 +72,26 @@ def __init__(self, pubs, robot_status_lock, robot_status, self.charge_pub = pubs["CHARGE_PUB"] self.charge_pose_pub = pubs["CHARGE_POSE_PUB"] - self.map_thread = MapService(robot_status_lock, robot_status) - self.nav_thread = NavigationService(robot_status_lock, robot_status) + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock + + self.map_thread = MapService(self.galileo_status, self.galileo_status_lock) + self.nav_thread = NavigationService(self.galileo_status, self.galileo_status_lock) self.last_nav_time = rospy.Time(0) - self.nav_flag = False self.user_socket_remote = None self.nav_task = None self.nav_guide_task = None + rospy.loginfo("service started") def get_galileo_cmds(cmds): self.parse_data([map(lambda x: ord(x), list(cmds.data))]) - rospy.Subscriber('/galileo/cmds', GalileoNativeCmds, get_galileo_cmds) + self.cmd_sub = rospy.Subscriber('/galileo/cmds', GalileoNativeCmds, get_galileo_cmds) def stop(self): + self.cmd_sub.unregister() if self.user_server_socket != None: self.user_server_socket.close() if not self.map_thread.stopped(): @@ -191,7 +195,6 @@ def parse_data(self, cmds): self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) - self.nav_flag = False os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") @@ -227,67 +230,6 @@ def parse_data(self, cmds): self.elevator_pub.publish(elePose) elif cmds[count][0] == ord('m'): time1_diff = time_now - self.last_nav_time - # if cmds[count][1] == 1: - # if time1_diff.to_sec() < 30: - # continue - # rospy.loginfo("开启导航服务") - - # rospy.loginfo("关闭建图服务") - # if not self.map_thread.stopped(): - # rospy.loginfo("关闭建图服务2") - # self.map_thread.stop() - # os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") - # os.system("pkill -f 'roslaunch nav_test update_map.launch'") - - # self.last_nav_time = time1_diff - # tilt_degree = Int16() - # tilt_degree.data = -19 - # self.tilt_pub.publish(tilt_degree) - # if self.nav_thread.stopped(): - # self.nav_thread.setspeed(1) - # self.nav_thread.start() - # self.nav_task = NavigationTask() - - # if cmds[count][1] == 2: - # if time1_diff.to_sec() < 30: - # continue - # rospy.loginfo("开始中速巡检") - - # rospy.loginfo("关闭视觉") - # if not self.map_thread.stopped(): - # rospy.loginfo("关闭视觉2") - # self.map_thread.stop() - # os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") - # os.system("pkill -f 'roslaunch nav_test update_map.launch'") - - # self.last_nav_time = time1_diff - # tilt_degree = Int16() - # tilt_degree.data = -19 - # self.tilt_pub.publish(tilt_degree) - # if self.nav_thread.stopped(): - # self.nav_thread.setspeed(2) - # self.nav_thread.start() - # self.nav_task = NavigationTask() - # if cmds[count][1] == 3: - # if time1_diff.to_sec() < 30: - # continue - # rospy.loginfo("开始高速巡检") - - # rospy.loginfo("关闭视觉") - # if not self.map_thread.stopped(): - # rospy.loginfo("关闭视觉2") - # self.map_thread.stop() - # os.system("pkill -f 'roslaunch ORB_SLAM2 map.launch'") - # os.system("pkill -f 'roslaunch nav_test update_map.launch'") - - # self.last_nav_time = time1_diff - # tilt_degree = Int16() - # tilt_degree.data = -19 - # self.tilt_pub.publish(tilt_degree) - # if self.nav_thread.stopped(): - # self.nav_thread.setspeed(3) - # self.nav_thread.start() - # self.nav_task = NavigationTask() if cmds[count][1] == 0: if time1_diff.to_sec() < 30: continue @@ -333,7 +275,6 @@ def parse_data(self, cmds): self.speed_cmd.linear.x = 0 self.speed_cmd.angular.z = 0 self.cmd_vel_pub.publish(self.speed_cmd) - self.nav_flag = False os.system("pkill -f 'roslaunch nav_test tank_blank_map0.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map1.launch'") os.system("pkill -f 'roslaunch nav_test tank_blank_map2.launch'") diff --git a/utils/nav_task.py b/utils/nav_task.py index 2008341..511d3f7 100644 --- a/utils/nav_task.py +++ b/utils/nav_task.py @@ -61,13 +61,13 @@ def __init__(self, nav_points_file="/home/xiaoqiang/slamdb/nav.csv", nav_path_fi rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") self.current_pose_stamped = None + self.current_pose_stamped_map = None self.status_lock = threading.Lock() self.current_goal_id = -1 self.goal_status = "FREE" self.loop_running_flag = False self.loop_exited_flag = True self.sleep_time = 1 - self.track_init_flag = False self.last_speed = None self.load_targets_exited_flag = True @@ -262,9 +262,21 @@ def current_goal_distance(self): except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception): return -1 - current_pose = self.current_pose_stamped.pose + self.current_pose_stamped_map = self.current_pose_stamped mgoal = self.waypoints[self.current_goal_id].pose - return self.pose_distance(mgoal, current_pose) + return self.pose_distance(mgoal, self.current_pose_stamped_map.pose) + + def update_pose(self): + latest = rospy.Time(0) + self.current_pose_stamped.header.stamp = latest + try: + self.current_pose_stamped = self.listener.transformPose( + "/map", self.current_pose_stamped) + except (tf.LookupException, tf.ConnectivityException, + tf.ExtrapolationException, tf.Exception): + return -1 + self.current_pose_stamped_map = self.current_pose_stamped + return self.current_pose_stamped def pose_distance(self, pose1, pose2): return math.sqrt(math.pow((pose1.position.x - pose2.position.x), 2) diff --git a/utils/navigation_service.py b/utils/navigation_service.py index 6c441d3..fb6014a 100644 --- a/utils/navigation_service.py +++ b/utils/navigation_service.py @@ -38,15 +38,15 @@ class NavigationService(threading.Thread): # orb_slam建图线程 - def __init__(self, robot_status_lock, robot_status): + def __init__(self, galileo_status, galileo_status_lock): super(NavigationService, self).__init__() self._stop = threading.Event() self._stop.set() self.p = None self.ps_process = None self.speed = 1 - self.robot_status_lock = robot_status_lock - self.robot_status = robot_status + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock def stop(self): if self.p != None: @@ -59,7 +59,7 @@ def stop(self): pass self.p = None self._stop.set() - self.__init__(self.robot_status_lock, self.robot_status) + self.__init__(self.galileo_status, self.galileo_status_lock) def stopped(self): return self._stop.isSet() @@ -85,11 +85,7 @@ def run(self): self.p = subprocess.Popen(cmd, shell=True, env=new_env) self.ps_process = psutil.Process(pid=self.p.pid) else: - if self.ps_process.is_running(): - self.robot_status_lock.acquire() - self.robot_status.orbStartStatus = True - self.robot_status_lock.release() - else: + if not self.ps_process.is_running(): break time.sleep(0.1) self.stop() diff --git a/utils/scale_orb.py b/utils/scale_orb.py index a540080..b608a4c 100644 --- a/utils/scale_orb.py +++ b/utils/scale_orb.py @@ -35,7 +35,7 @@ class ScaleORB(threading.Thread): - def __init__(self, robot_status): + def __init__(self, galileo_status, galileo_status_lock): super(ScaleORB, self).__init__() self._stop = threading.Event() self.car_odoms = 0.0 @@ -48,7 +48,8 @@ def __init__(self, robot_status): self.scale_lock = threading.Lock() self.car_begin_flag = True self.camera_begin_flag = True - self.robot_status = robot_status + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock def stop(self): self._stop.set() @@ -82,7 +83,10 @@ def update_camera(pose): self.camera_last_pose = pose self.camera_pose_lock.release() - while not self.robot_status.orbInitStatus and not rospy.is_shutdown(): + while not rospy.is_shutdown(): + with self.galileo_status_lock: + if self.galileo_status.visualStatus == 1: + break time.sleep(0.5) if rospy.is_shutdown(): self.stop() diff --git a/utils/udp_status_service.py b/utils/udp_status_service.py new file mode 100644 index 0000000..f3d4823 --- /dev/null +++ b/utils/udp_status_service.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python +# encoding=utf-8 +# The MIT License (MIT) +# +# Copyright (c) 2018 Bluewhale Robot +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +# Author: Randoms +# + +import struct +import threading + +import numpy as np +import rospy +import tf +from galileo_serial_server.msg import GalileoStatus +from geometry_msgs.msg import Pose, PoseStamped, Twist +from nav_msgs.msg import Odometry + +from config import BROADCAST_PORT, ROS_PACKAGE_PATH, TF_ROT, TF_TRANS + + +class UDPStatusService(threading.Thread): + + def __init__(self, monitor_server, galileo_status, galileo_status_lock): + super(UDPStatusService, self).__init__() + # stop flag + self._stop = threading.Event() + self._stop.set() + self.listener = tf.TransformListener(True, rospy.Duration(10.0)) + self.send_data = bytearray( + [205, 235, 215, 36, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + ]) + self.monitor_server = monitor_server + self.robot_pose_stamped = None + self.galileo_status = galileo_status + self.galileo_status_lock = galileo_status_lock + self.odom_flag = False + self.image_flag = False + + def update_odom(odom): + self.odom_flag = True + self.robot_pose_stamped = PoseStamped() + self.robot_pose_stamped.pose = odom.pose.pose # 更新坐标 + self.robot_pose_stamped.header = odom.header # 更新坐标 + + def update_camera_status(status): + self.image_flag = True + + self.odom_sub = rospy.Subscriber("/bWmono/Odom", Odometry, update_odom) + self.camera_sub = rospy.Subscriber( + "/camera_node/camera_info", rospy.AnyMsg, update_camera_status) + + def stop(self): + self.odom_sub.unregister() + self.camera_sub.unregister() + self._stop.set() + + def stopped(self): + return self._stop.isSet() + + def run(self): + rate = rospy.Rate(30) + self._stop.clear() + t_ac = np.array([0., 0., 0.]) + theta_send = 0. + while not self.stopped() and not rospy.is_shutdown(): + if self.monitor_server.get_connection_status(): + if self.odom_flag and self.robot_pose_stamped is not None: + # 将map坐标系转换成ORB_SLAM/World坐标系 + current_pose = self.robot_pose_stamped.pose + t_bc = np.array( + [current_pose.position.x, current_pose.position.y, current_pose.position.z]) + q = [current_pose.orientation.x, current_pose.orientation.y, + current_pose.orientation.z, current_pose.orientation.w] + m = tf.transformations.quaternion_matrix(q) + r_bc = m[:3, :3] + + _, _, theta_send = tf.transformations.euler_from_matrix( + r_bc) + # 为了简化计算，下文的计算中 base_link 和 base_footprint 被看成是相同的坐标系 + + r_ab = TF_ROT.T + t_ab = -r_ab.dot(TF_TRANS) + t_ac = r_ab.dot(t_bc) + t_ab + + self.send_data[4:8] = map(ord, struct.pack('f', t_ac[0])) + self.send_data[8:12] = map(ord, struct.pack('f', t_ac[1])) + self.send_data[12:16] = map(ord, struct.pack('f', t_ac[2])) + with self.galileo_status_lock: + self.send_data[16:20] = map( + ord, struct.pack('f', self.galileo_status.power)) + self.send_data[24:28] = map( + ord, struct.pack('f', theta_send)) + self.send_data[28:32] = map(ord, struct.pack( + 'i', self.galileo_status.targetStatus)) + self.send_data[32:36] = map(ord, struct.pack( + 'i', self.galileo_status.targetNumID)) + self.send_data[40:44] = map(ord, struct.pack( + 'i', self.galileo_status.loopStatus)) + self.send_data[36:40] = map(ord, struct.pack( + 'i', self.galileo_status.chargeStatus)) + + if self.galileo_status.navStatus == 1: + statu0 = 0x01 # 导航状态 + else: + statu0 = 0x00 + if self.image_flag: + statu1 = 0x02 # 视觉摄像头 + else: + statu1 = 0x00 + if self.galileo_status.visualStatus != -1: + statu2 = 0x04 # 视觉系统状态 + else: + statu2 = 0x00 + if self.galileo_status.visualStatus == 1: + statu3 = 0x08 # 视觉系统状态 + else: + statu3 = 0x00 + if self.galileo_status.gcStatus == 1: + status4 = 0x10 # ORB_SLAM 内存回收状态 + else: + status4 = 0 + if self.galileo_status.gbaStatus == 1: + status5 = 0x20 # ORB_SLAM2 GBA状态，一般对应LoopClosing + else: + status5 = 0x00 + self.send_data[20] = statu0 + statu1 + \ + statu2 + statu3 + status4 + status5 + self.send_data[3] = len(self.send_data) - 4 + self.monitor_server.sendto(bytes(self.send_data)) + rate.sleep()