simulate robot state

最新推荐文章于 2024-05-30 14:59:09 发布

雨秋yuqiu

最新推荐文章于 2024-05-30 14:59:09 发布

阅读量110

点赞数

分类专栏： robot code trans 文章标签：数据库

本文链接：https://blog.csdn.net/huashuideyu/article/details/126575959

版权

robot code trans 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

#!/usr/bin/env python3
#coding = 'utf-8'
# 导入sqlite3模块
from sqlite3 import Error
import sqlite3
import time
import rospy
from std_msgs.msg import String
import paho.mqtt.client as mqtt
import configparser
import json
from robot.msg import data,pose
import threading
import time
import _thread
import configparser
from distutils.command.config import config
import rospy
import paho.mqtt.client as mqtt
import time
import rospy
import json
from std_msgs.msg import String
from robot.msg import data,pose
import configparser
import datetime

##########
dis_connect = str("time")
re_connect = str("time")
conn_f= 0
#-----
conf = configparser.ConfigParser()
conf.read("/home/r/catkin_ws/src/robot/config.ini")
#dataBaseTrace = conf.get('dataBase','dataBaseTrace')
dataBaseTrace = "/home/r/ROSworkspace/sql"
HOST = conf.get('connect','serviceIp')
PORT = int(conf.get('connect','servicePort'))
MQTT_TOPIC = conf.get('connect','serviceTopic')
Heart = int(conf.get('connect','mqttHeartbeat'))
USER = conf.get('connect','serviceUsername')
PASSWORD = conf.get('connect','servicePassword')
method = conf.get('robot','Method')
#-----

def qt_callBack(msg):
    rospy.loginfo("Subcribe roobot Info: [%s] ", msg)
    # 数据库文件路径
    tim = msg.time
    global dis_connect
    global re_connect
    db_file = dataBaseTrace+'/robot.db'
    # 连接数据库
    conn = sqlite3.connect(db_file)
    # 创建游标
    cour = conn.cursor()
    # 编写sql语句

    #####
    lenSql = 'select count(*)  from ' + "tb_robot_state"
    cour.execute(lenSql)
    # 打印查询结果
    dbNumber = cour.fetchall()
    # 关闭游标
    totalNum = int(dbNumber[0][0])
    cour.close()
    #####
    # 查询语句sql：select 列名(*-所有列) from 表名 [where 条件]
    # 数据库文件路径
    db_file = dataBaseTrace+'/robot.db'

    # 连接数据库
    conn = sqlite3.connect(db_file)
    # 创建游标
    cour = conn.cursor()
    # 编写sql语句
    #sql = 'select MESSAGE from ' + "deadBroiler" + "where" "ID Between 2022-07-15 17:35:05 and  2022-07-15 17:35:16" 
    sql = 'select Method,Conf,Time,Distance,Vision,Rfid,Height,Ahead,Rfidnumber,p_time,Deviation,Rfidbz,Speed from tb_robot_state '
    
    # 执行sql语句
    cour.execute(sql)
    res = cour.fetchall()
    data = res[0]
    method = data[0]
    conf = data[1]
    distance = data[3]
    vision = data[4]
    rfid = data[5]
    height = data[6]
    ahead = data[7]
    rfidNum = data[8]
    p_time = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime())
    deviation = data[10]
    rfidbz = data[11]
    speed = data[12]
    
    #pose
    qt_pose = pose()
    qt_pose.pos_a = ahead
    qt_pose.pos_h = height
    qt_pose.pos_s = speed
    qt_pose.pos_g = "r100"
    qt_pose.pos_d = rfid

    cour.close()
    client = mqtt.Client()
    #client.username_pw_set(username='admin', password='public')  # 用于添加了插件认证方式后
    
    client.username_pw_set(username=USER, password=PASSWORD)  # 用于添加了插件认证方式后

    client.connect(HOST, PORT, Heart)

    Message = json.dumps({
     "method":method,
     "conf":conf,
     "time":tim,
     "distance":distance,
     "rfid":rfid,
     "rfidNumber":rfidNum,
     "hight":height,
     "ahead":ahead,
     "speed":speed,
     "p_time":p_time,
     "deviation":deviation,
     "vision":vision,
     "rfidbz":rfidbz
    })
    client.publish(MQTT_TOPIC, Message,2)  # 发布一个主题为'services',内容为‘Message’的json格式的信息
    # time.sleep(10)
    print("published",Message )
    client.loop_stop()
    client.disconnect()
    return(qt_pose)

def robot_subscriber():
    # ROS节点初始化
      # 创建一个Subscriber，订阅名为/data_info的topic，注册回调函数robotInfoCallback
    rospy.Subscriber("camera", data, qt_callBack)
    print("mqtt ok")
    # 循环等待回调函数
    rospy.spin()
def robotState():
    while True:
        import datetime
        db_file = dataBaseTrace+'/robot.db'
        # 连接数据库
        conn = sqlite3.connect(db_file)
        # 创建游标
        cour = conn.cursor()
        # 编写sql语句
        #####
        lenSql = 'select count(*)  from ' + "tb_robot_state"
        cour.execute(lenSql)
        # 打印查询结果
        dbNumber = cour.fetchall()
        # 关闭游标
        totalNum = int(dbNumber[0][0])
        cour.close()
        #####
        # 查询语句sql：select 列名(*-所有列) from 表名 [where 条件]
        # 数据库文件路径
        db_file = dataBaseTrace+'/robot.db'
        # 连接数据库
        conn = sqlite3.connect(db_file)
        # 创建游标
        cour = conn.cursor()
        # 编写sql语句
        #sql = 'select MESSAGE from ' + "deadBroiler" + "where" "ID Between 2022-07-15 17:35:05 and  2022-07-15 17:35:16" 
        sql = 'select Method,Conf,Time,Distance,Vision,Rfid,Height,Ahead,Rfidnumber,p_time,Deviation,Rfidbz,Speed from tb_robot_state '

        # 执行sql语句
        cour.execute(sql)
        res = cour.fetchall()
        data = res[0]
        method = data[0]
        conf = data[1]
        tim = data[2]
        distance = data[3]
        vision = data[4]
        rfid = data[5]
        height = data[6]
        ahead = data[7]
        rfidNum = data[8]
        p_time = data[9]
        deviation = data[10]
        rfidbz = data[11]
        speed = data[12]
        #pose
        qt_pose = pose()
        qt_pose.pos_a = ahead
        qt_pose.pos_h = height
        qt_pose.pos_s = speed
        qt_pose.pos_g = "r100"
        qt_pose.pos_d = rfid
        cour.close()
        print("-------")
        print("ahead",ahead)
        print("heught",height)
        print("speed",speed)
        print("now rfid",rfid)
        print("goal",qt_pose.pos_g)
        pub_tim = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime())
        now_time = rospy.Time.now()
        print(pub_tim)

        rospy.sleep(2)
if __name__ == '__main__':
    rospy.init_node('camera_subscriber', anonymous=True)
    t1 = threading.Thread(target=robot_subscriber,name="callBack")
    t2 = threading.Thread(target=robotState,name="robotState")
    t1.start()
    t2.start()