From 5bda2fa919fa09e7e7395bce45f37470deca42c9 Mon Sep 17 00:00:00 2001 From: six9527 <487844521@qq.com> Date: Wed, 10 May 2023 05:23:05 -0400 Subject: [PATCH] robobus --- CMakeLists.txt | 234 +++++++++++++ README.md | 2 +- launch/obu_run.launch | 10 + package.xml | 78 +++++ .../obu/chassisAbnormalnformationUpload.py | 47 +++ .../obu/chassisRealTimeInformationUpload.py | 223 ++++++++++++ .../chassisRegistrationInformationReport.py | 64 ++++ scripts/obu/chassisStatusInformationUpload.py | 68 ++++ scripts/obu/config.py | 31 ++ scripts/obu/config.pyc | Bin 0 -> 635 bytes scripts/obu/mySocket.py | 82 +++++ scripts/obu/mySocket.pyc | Bin 0 -> 2809 bytes scripts/obu/udp_interface_node.py | 331 ++++++++++++++++++ srv/OBUinterflowVehicle.srv | 5 + 14 files changed, 1174 insertions(+), 1 deletion(-) create mode 100644 CMakeLists.txt create mode 100644 launch/obu_run.launch create mode 100644 package.xml create mode 100755 scripts/obu/chassisAbnormalnformationUpload.py create mode 100755 scripts/obu/chassisRealTimeInformationUpload.py create mode 100755 scripts/obu/chassisRegistrationInformationReport.py create mode 100755 scripts/obu/chassisStatusInformationUpload.py create mode 100644 scripts/obu/config.py create mode 100644 scripts/obu/config.pyc create mode 100644 scripts/obu/mySocket.py create mode 100644 scripts/obu/mySocket.pyc create mode 100755 scripts/obu/udp_interface_node.py create mode 100644 srv/OBUinterflowVehicle.srv diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f73b530 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,234 @@ +cmake_minimum_required(VERSION 3.0.2) +project(obu) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + rospy + message_generation + std_msgs + pix_driver_msgs + autoware_perception_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# ) + + +add_service_files(FILES OBUinterflowVehicle.srv) +generate_messages(DEPENDENCIES std_msgs autoware_perception_msgs) +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES obu +# CATKIN_DEPENDS geometry_msgs roscpp rospy +# DEPENDS system_lib + CATKIN_DEPENDS roscpp rospy std_msgs message_runtime pix_driver_msgs autoware_perception_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/obu.cpp +# ) + + + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/obu_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +catkin_install_python(PROGRAMS + scripts/obu/chassisAbnormalnformationUpload.py + scripts/obu/chassisRealTimeInformationUpload.py + scripts/obu/chassisRegistrationInformationReport.py + scripts/obu/chassisStatusInformationUpload.py + scripts/obu/config.py + scripts/obu/mySocket.py + scripts/obu/udp_interface_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +install(TARGETS + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_obu.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/README.md b/README.md index adb20de..c4dc5d1 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,2 @@ # pix_obu -robobus +obu通信基于citybot_v2 diff --git a/launch/obu_run.launch b/launch/obu_run.launch new file mode 100644 index 0000000..1661797 --- /dev/null +++ b/launch/obu_run.launch @@ -0,0 +1,10 @@ + + + --> + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a9416ee --- /dev/null +++ b/package.xml @@ -0,0 +1,78 @@ + + + obu + 0.0.0 + The obu package + + + + + six + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + roscpp + rospy + pix_driver_msgs + autoware_perception_msgs + geometry_msgs + roscpp + rospy + pix_driver_msgs + autoware_perception_msgs + geometry_msgs + roscpp + rospy + std_msgs + std_msgs + pix_driver_msgs + autoware_perception_msgs + message_generation + message_runtime + + + + + + + + diff --git a/scripts/obu/chassisAbnormalnformationUpload.py b/scripts/obu/chassisAbnormalnformationUpload.py new file mode 100755 index 0000000..227ceac --- /dev/null +++ b/scripts/obu/chassisAbnormalnformationUpload.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- +""" +3.3.1.4 车载主机车辆异常信息 -- 触发 chassis abnormal Information report + +""" +import rospy +import json +from obu.srv import * +from pix_driver_msgs.msg import VcuReport +from pix_driver_msgs.msg import BrakeReport + +class AbnormalInformationUpload: + def __init__(self): + self.server = rospy.Service("/obu/chassis/AbnormalInformationUpload",OBUinterflowVehicle, self.callback_doReq) + self.sub_vcu_report = rospy.Subscriber('/pix/vcu_report', VcuReport, self.vcu_report_callback) + self.sub_brake_report = rospy.Subscriber("/pix/break_report",BrakeReport,self.brake_report_callback) + self.msgMap={ + "vehicleElectronicState":"0", + "carAbnormal":1 + } + # self.msgMap={"warningType":1, + # "signType":0, + # "evenType":0, + # "warningInfo":" 前 车 碰 撞 预 警", + # "warningSuggestion":"请减速，前方有碰撞危险", + # "distant":40} + + def callback_doReq(self, req): + msg = json.dumps(self.msgMap, ensure_ascii=False) + return msg + + def vcu_report_callback(self,msg): + self.msgMap["vehicleElectronicState"] = str(msg.chassis_errcode) + + def brake_report_callback(self,msg): + if (msg.brake_flt2 == 1 or msg.brake_flt1 ==1 ): + self.msgMap["carAbnormal"] = 64 + + + + + +if __name__ == "__main__": + rospy.init_node("obu_chassis_AbnormalInformationUpload") + AbnormalInformationUpload() + rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisRealTimeInformationUpload.py b/scripts/obu/chassisRealTimeInformationUpload.py new file mode 100755 index 0000000..9d2b07d --- /dev/null +++ b/scripts/obu/chassisRealTimeInformationUpload.py @@ -0,0 +1,223 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- +import rospy +import json +from obu.srv import * +from pix_driver_msgs.msg import BrakeReport +from pix_driver_msgs.msg import GearReport +from pix_driver_msgs.msg import SteerReport +from pix_driver_msgs.msg import VcuReport +from pix_driver_msgs.msg import ParkReport +from pix_driver_msgs.msg import ThrottleReport +from sensor_msgs.msg import NavSatFix, Imu +from pix_driver_msgs.msg import VehicleModeCommand +from tf import transformations + + +class RealTimeInformationUpload: + def __init__(self): + self.server = rospy.Service("/obu/chassis/RealTimeInformationUpload",OBUinterflowVehicle, self.callback_doReq) + + #转向，车速，启动状态，驾驶模式，车辆状态，加速度，刹车灯 + self.sub_vcu_report = rospy.Subscriber('/pix/vcu_report', VcuReport, self.vcu_report_callback) + #刹车 + self.sub_brake_report = rospy.Subscriber("/pix/break_report",BrakeReport,self.brake_report_callback) + #倒车 + self.sub_park_report = rospy.Subscriber("/pix/park_report",ParkReport,self.park_report_callback) + #档位状态 + self.sub_gear_report = rospy.Subscriber("/pix/gear_report",GearReport,self.gear_report_callback) + #方向盘转角，转角速度 + self.sub_steering_report = rospy.Subscriber("/pix/steering_report",SteerReport,self.steering_report_callback) + #油门状态 + self.sub_throttle_report = rospy.Subscriber("/pix/throttle_report",ThrottleReport,self.throttle_report_callback) + #车门 + self.vehicle_command = rospy.Subscriber("/pix/vehicle_command",VehicleModeCommand,self.vehicle_command_callback) + # ？？？？ + # 经纬度，海拔，车身姿态，定位状态，从车速度和距离，电机功率转速，横向纵向加速度，住车灯，刹车灯，倒车灯？？ + self.sub_lon_lat = rospy.Subscriber("/fix", NavSatFix, self.lon_lat_callback)#经纬度 + # /sensing/imu/imu_data + self.sub_imu= rospy.Subscriber("/chc_imu", Imu, self.Imu_callback)#imu数据 + + + self.msgMap={ + "LocationType":31, + #经纬度海拔 + "longitude":113.5951157, + "latitude":22.74467066, + "altitude":26.04, + #定位精度 + "accuracy":1.0, + + "braking":0, + "gear":bytes.decode("N"), + "speed":0, + "SWA":0, + "turnLight":0, + #车身位姿，imu数据deg + "vehiclePosture":{ + "postureX":0, + "postureY":0, + "postureZ":0}, + + #从车位置 + "relativePosition":{ + "speed2":45, + "distanceR0":1, + "distanceRY":3, + "distanceRX":3, + "distance0":3}, + + "ifCarOnline":1, + "throttle":30, + #电机转速 + "rotationRate":50, + + #电机功率 + " power":100, + + "carMode1":2, + "carStatus":3, + "SterringAngleSpd":34, + + # 车辆纵向，横向加速度 + "VehLatrlAccel":1, + "VehLongAccell":1, + + #驾驶位门锁 + "DriverDoorLockSt":0, + + + "BrakeLightSwitchSt":0, + "ParkingLampSt":1, + + #住车灯，倒车灯 + "Reverse_light_status":0 + } + + def park_report_callback(self,msg): + self.msgMap["ParkingLampSt"] = msg.parking_actual + + + def callback_doReq(self, req): + print(self.msgMap) + return json.dumps(self.msgMap,ensure_ascii=False) + + def vcu_report_callback(self,msg): + self.msgMap["speed"] = msg.speed + self.msgMap["turnLight"] =msg.turn_light_actual + + #启动判断 + # if ( msg.CarPower_State < 2): + # # 0不再线1在线 + # self.msgMap["ifCarOnline"] = msg.CarPower_State + # else: + # self.msgMap["ifCarOnline"] = 0 + + + # 模式判断 + # 0x3: Standby Mode + # 0x2: Emergency Mode + # 0x1: Auto Mode + # 0x0: Manual Remote Mode + # msg.Vehicle_ModeState + #msg.vehicle_mode_state + if (msg.vehicle_mode_state == 0):#手动远程模式 + self.msgMap["carMode1"] = 1 + + elif(msg.vehicle_mode_state == 1):#自动驾驶模式 + self.msgMap["carMode1"] = 2 + + elif(msg.vehicle_mode_state == 2):#紧急模式 + self.msgMap["carMode1"] = 255 + + elif(msg.vehicle_mode_state == 3):#待机模式 + self.msgMap["carMode1"] = 255 + + # 车辆状态 + # "0x7: crash + # 0x6: error + # 0x5: E-stop + # 0x4: work + # 0x3: 3 + # 0x2: 2 + # 0x1: 1 + # 0x0: init + # " + + # 表示车辆状态，有未知 + # （FF），未启动(1)，停车 + # （2）或驾驶中(3)四种，数 + # 值类型如：1，未知为 255 + + # if (msg.CarWork_State == 4):# + # self.msgMap["carStatus"] = 3 + + # elif (msg.CarWork_State == 5):# + # self.msgMap["carStatus"] = 2 + # else: + # self.msgMap["carStatus"] = 255 + + # 车辆纵向，横向加速度 + self.msgMap["VehLongAccell"] = msg.acc#车辆加速度 + + #刹车灯 + self.msgMap["BrakeLightSwitchSt"] = msg.brake_light_actual + + def brake_report_callback(self,msg): + self.msgMap["braking"] = msg.brake_pedal_actual + + def gear_report_callback(self,msg): + if (msg.gear_actual == 1): + self.msgMap["gear"] = 80 + + elif (msg.gear_actual == 2): + self.msgMap["gear"] = 82 + + elif (msg.gear_actual == 3): + self.msgMap["gear"] = 78 + + elif (msg.gear_actual == 4): + self.msgMap["gear"] = 68 + else: + self.msgMap["gear"] = 0xff + + def steering_report_callback(self,msg): + if(msg != None): + self.msgMap["SWA"] = msg.steer_angle_actual + self.msgMap["SterringAngleSpd"] = msg.steer_angle_spd_actual + + else: + self.msgMap["SWA"] = 0 + self.msgMap["SterringAngleSpd"] = 0 + + return 1 + + def throttle_report_callback(self,msg): + self.msgMap["throttle"] = msg.throttle_pedal_actual + return 1 + +#0-invaild 1-close_door 2-open_door + def vehicle_command_callback(self,msg): + if (msg.door_status_ctrl ==1 ): + self.msgMap["DriverDoorLockSt"] = 1 + elif (msg.door_status_ctrl ==2 ): + self.msgMap["DriverDoorLockSt"] = 0 + else: + self.msgMap["DriverDoorLockSt"] = 255 + + def lon_lat_callback(self, msg): + self.msgMap["longitude"] = msg.longitude + self.msgMap["latitude"] = msg.latitude + self.msgMap["altitude"] = msg.altitude + + def Imu_callback(self,msg): + (r, p, y) = transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) + self.msgMap["vehiclePosture"]["postureX"]= p + self.msgMap["vehiclePosture"]["postureY"]= r + self.msgMap["vehiclePosture"]["postureZ"]= y + self.msgMap["VehLatrlAccel"] = msg.linear_acceleration.y + +if __name__ == "__main__": + rospy.init_node("obu_chassis_RealTimeInformationUpload") + RealTimeInformationUpload() + rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisRegistrationInformationReport.py b/scripts/obu/chassisRegistrationInformationReport.py new file mode 100755 index 0000000..6c6d7d0 --- /dev/null +++ b/scripts/obu/chassisRegistrationInformationReport.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- +""" +3.3.1.1 车载主机信息注册 -- 双向 一次 chassis Registration Information report +""" +import rospy +import json +from obu.srv import OBUinterflowVehicleResponse, OBUinterflowVehicle, OBUinterflowVehicleRequest +from pix_driver_msgs.msg import VehicleModeCommand + +class RegistrationInformationReport: + def __init__(self): + self.server = rospy.Service("/obu/chassis/RegistratIonInformationReport",OBUinterflowVehicle, self.callback_doReq) + self.vehicle_command = rospy.Subscriber("/pix/vehicle_command",VehicleModeCommand,self.vehicle_command_callback) + self.msgMap={ + # "vin_IN":"车辆标识,车架号, 字符类型", + "vin":"P-CQZW4110072A0N08001", + + # "plateNumber_IN":"车牌号, 字符类型。如:粤A0V08", + "plateNumber":"粤A0V09", + + # "personInCharge_IN":"安全员名字", + "personInCharge":"PIXMOVING", + + # "manufacturer_IN":"车辆厂商名字", + "manufacturer":"PIXMOVING", + + # "company_IN":"无人驾驶公司名字", + "company":"PIXMOVING", + + # "vehicleType_IN":"车辆类型 uint8 有未知(FF)纯电动(1),纯油 (2)，混合动力(3)四种", + "vehicleType":1, + + # "vehicleProperty_IN":"车辆类型 uint8 有未知(FF) 测试(1) 运营(2)", + "vehicleProperty":1, + + # "carType_IN":"车辆种类, A为轿车(5 座及以下)、B为小巴、C为中巴、D为大巴、E为货车、F为卡车、G挂车、H为物流车(包含外卖、快递等)、I 为清洁车、J 为巡逻车、K 为消毒车、L 为售卖车（销售 饮料等)", + "carType":"B", + + # "brand_IN":"车辆品牌, 如:奥迪、阿尔法·罗密欧、奔驰、宝马、红旗、Jeep、捷豹、雷克萨斯、凯迪拉克、路 虎、林肯、讴歌、乔治·巴顿、沃尔沃、英菲尼迪……。 如:日产,注意不填时,brand为null,即不上报, 不要填字符串”null”、””、“none", + "brand":"PIXMOVING", + + # "series_IN":"车系,车型，如奥迪 a6中的a6, 林肯MKZ。如:MKZ", + "series":"PIXMOVING", + + # "equip_code_IN":"测试车企设备编码", + "equip_code":"PIXMOVING004", + + # "equip_name_IN":"测试车企设备名称", + "equip_name":"PIXMOVING" + } + def vehicle_command_callback(self,msg): + self.msgMap["vin"] = msg.vin_req + + def callback_doReq(self, req): + msg = json.dumps(self.msgMap,ensure_ascii=False) + return msg + + + +if __name__ == "__main__": + rospy.init_node("obu_chassis_RegistratIonInformationReport") + RegistrationInformationReport() + rospy.spin() \ No newline at end of file diff --git a/scripts/obu/chassisStatusInformationUpload.py b/scripts/obu/chassisStatusInformationUpload.py new file mode 100755 index 0000000..9999a66 --- /dev/null +++ b/scripts/obu/chassisStatusInformationUpload.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- +import rospy +import json +import time +from obu.srv import OBUinterflowVehicle,OBUinterflowVehicleRequest,OBUinterflowVehicleResponse +from pix_driver_msgs.msg import BmsReport + + +class StatusInformationUpload: + def __init__(self): + self.server = rospy.Service("/obu/chassis/StatusInformationUpload",OBUinterflowVehicle, self.callback_doReq) + self.sub_bms_report = rospy.Subscriber("/pix/bms_report",BmsReport,self.bms_report_callback) + self.count = 0 + + self.msgMap={ + #剩余电量% + "batteryQuantity":0, + + "oilQuantity":255, + #理论可以计算得出（电池电压×电池容量×时速÷电机功率） + "restdistance":0, + #定位状态1就绪,2未就绪,255未知 + "locationModelState":1, + #电机状态1就绪,2未就绪,255未知 + "electricalMachineState":1, + #传感器状态1就绪，2未就绪，255未知 + "sensorState":1, + + #无法获取不上报 + # "departure":" 广 州 市 南 沙 区 海 滨 路", + # "dep_longitude":113.6145444, + # "dep_latitude":22.74776556, + + "destination":"广州市", + "des_longitude":113.28064, + "des_latitude":23.125177, + + "cloudState":1, + "_4gState":2, + "_5gState":2, + "ltevState":4, + #摄像头状态 + "camerNum":4, + + "cameraIp":"192.168.19.11", + "cameraConfig":0, + "markType":1, + "time":"0", + "UTC":"0", + "markNum":0 + } + + def bms_report_callback(self,msg): + self.msgMap["batteryQuantity"] = msg.battery_soc + self.msgMap["restdistance"] = msg.battery_soc*100/100 + self.msgMap["time"] = time.strftime('%Y-%m-%d %H:%M:%S',time.localtime()) + self.msgMap["UTC"] = str(time.time()) + self.msgMap["markNum"] = 0 + + def callback_doReq(self, req): + return json.dumps(self.msgMap,ensure_ascii=False) + + +if __name__ == "__main__": + rospy.init_node("obu_chassis_StatusInformationUpload") + StatusInformationUpload() + rospy.spin() \ No newline at end of file diff --git a/scripts/obu/config.py b/scripts/obu/config.py new file mode 100644 index 0000000..a76314e --- /dev/null +++ b/scripts/obu/config.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +# 服务器配置信息 +server_ip="192.168.4.103" +# server_port=1111 +# server_ip="127.0.0.1" +server_port=1247 + +""" +一般网络人为协议都是前缀+消息体+后缀 +<小端 >大端 +消息协议：命令码+消息体长度+消息体 +命令码 1Byte +消息体长度 4Byte +消息体 NByte +""" +msg_format_prefix="EUUpZT=;dQSy*EoA$Vr~S!7fM5+E0?+_r016>O&=6t>8bORe zV~8;{hXpwZmx9-hK*y*ehzKge;{=b>f6fdl#;?E+!3U0Uzc}$V zOjy$F>g*l&R6h?H`TB0|K07whffuIGBQK|~0XJi>(#w_0(hpgBt$**q>ny#}Z};Fu znw{%-55CXRbN!dwm&G9`b-o&_%y(s^?)rOnQP&#he8II{%Ca?e?qp|FsW8a)|Dd96O1=E#0qed%yU$KmX@s#Jb4^G24cmYjalNta3 literal 0 HcmV?d00001 diff --git a/scripts/obu/mySocket.py b/scripts/obu/mySocket.py new file mode 100644 index 0000000..201dd7e --- /dev/null +++ b/scripts/obu/mySocket.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- +import socket +import struct +import os +import config as msgConfig +import json +import yaml + + +import sys #reload()之前必须要引入模块 +reload(sys) +sys.setdefaultencoding('utf-8') + +class UdpSocket(object): + """ + socket通讯, 上传信息和下载信息 + """ + def __init__(self, addr, udp_socket_obj): + self.addr=addr + self.udp_socket=udp_socket_obj + # self.udp_socket.setblocking(False) + # self.udp_socket.settimeout(0.0) + # 加载日志模块 + # logging.config.fileConfig(msgConfig.configFile_dir+"/socket.conf") + # # self.root_logger = logging.getLogger() + # self.send_app_logger = logging.getLogger('send_applog') + # self.recv_app_logger = logging.getLogger('recv_applog') + # self.error_app_logger = logging.getLogger('error_applog') + + + + + + @staticmethod + def __data_fotmat(msg_body_len): + # + format_touple = (msgConfig.msg_format_prefix, str(msg_body_len), "s", msgConfig.msg_format_suffix) + return "".join(format_touple) + + # 数据打包成大端字节流的网络数据。computer string data 计算机数据 + def construct_byte(self, cmd, msg_body): + msg_body_len =len(msg_body) + communication_format = self.__data_fotmat(msg_body_len) + return struct.pack(communication_format, cmd, msg_body_len, msg_body.encode('utf-8')) + + + # 网络数据解包成元组, Ndata-网络数据 + def deconstruct_message(self, Ndata): + communication_format = self.__data_fotmat(len(Ndata)-msgConfig.msg_prefix_suffix_len) + return struct.unpack(communication_format, Ndata) + + # 单次发生已经打包好的数据 + def single_send(self, cmd, send_str): + """ + cmd 命令码--uint_8 + send_str 发送的字符串 + """ + # send_str = json.dumps(msg) + send_data = self.construct_byte(cmd, send_str) + self.udp_socket.sendto(send_data, self.addr) + + # 单次接受已经解包的数据 + def single_recv(self): + data, addr = self.udp_socket.recvfrom(4096) + recv_msg = self.deconstruct_message(data) + # msg_body = json.loads(recv_msg[2]) + # print(msg_body["vin"]) + return recv_msg + + +def main(): + # rospy.init_node("tcp_socket_node") + # 获取服务器和服务器端口 + addr=(msgConfig.server_ip, msgConfig.server_port) + # 创建udp套接字 + udp_socket=socket.socket(socket.AF_INET,socket.SOCK_DGRAM) + # udp_test = UdpSocket(addr, udp_socket) + udp_socket.close() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/scripts/obu/mySocket.pyc b/scripts/obu/mySocket.pyc new file mode 100644 index 0000000000000000000000000000000000000000..29c7de0bdd16a7b6db4b1d3a80d5f301c4c8a434 GIT binary patch literal 2809 zcmb_e&u<$=6rSC+ojP?$DNu5#P$WRIpv6K+P!K{b;Wr{k8#rJgjhx+ax{i0(+8LKb zN~K2a36M~^AoWzFiW5+YQzeM_6S(jvB(y#E4}kA`VVv>-uvczZ#H@` zH~Z%w^)ykFC&KS}GS~|@{0y2Z4P}r<%FL=hY}eE^rLHRVS@*a~>l}1j%FL;>q1HoH-BhN<>7Wqj zR@yUgH2WLRvaLfLyZ+g8t_kVWbhJ_?>&EQ|7ef0cn*9Ksfnf=SK?Jf7d_WMPIDkMA zL?n7dAYQ@!iATri4qa=i*t=j~g3Z}QJXSWnf91x#pKqU8+`aSV?r%4DfB*ik?c00b ze!Y9=t9v^;ekiLGBF;wVmeA}ebX$SiQc6PzMG!|<+5x7P+g#~t=z3{dxmgI~bfo>Z zZJLrvYli)pFSth@TA|C;K^qz+oqYvT=VFi*)iT}GLsPgpC?@7yIUWto5g|ppTBkkNgSQUeJ`IJ3D|#vr4luw5 zn!xoM1jLQg2O5_nasbe6MKY`rL*TY0a6yS>fDU+K@zom8BlQ1D2M->BVT@1WSMMjs zLtZ&L0Px0%6QoAHWLXK=g3jv=` zGn$OavG@c#lHEOlj^TQ)X-_ILCqBt(zA`R|nh@AS#e-eu(Z@voh+ts4rRD&L+*sRN zGcQ6t$->i~RwVrYAi)9ya)k%v8Xzw{(ZI4$1)!5Bl3jrbsa33Qd~ooKd)Ie%f4z10 z=JwgMCyTHO2Q`u!{0cW+$V`{}#8KmM?L=a>C;%We7=FR+u6Cgj75 zu9Ujvr|El4*oA6SyHW{cKwysu)8p91X2sgjXqtYRL%OIMCw)cZfSwAgSkXMIkeNRB zJ_nq_DCz;HP>tCeO>;6+;F9NtgiKOZCb`tF$~-=e!9mcHfI=H7;?tEH^gH@^>BD(! zKgpwoU_NT|VwsfqqL&-3<*Ly-FVis~(vCMT%14=K9ZPv;IkgT+pXA0}EK{F7a=?r& zUS#(Sx`O~jI?N>$!XX zp9qF4A1{5V-+DK`@R1{K$*{CWYPRrRlHmZM!+;|k%6nPy9$l3h8J!Fl!pX66M(Z|# zi%Dl+#0Xev#edWnT(mw~l7C>!LTqmK@}b-TcGIt{%&_dGj#bbamzsWWJalqvAfdz- xK08d}