diff --git a/src/common/launch_system/launch/start_all.launch.py b/src/common/launch_system/launch/start_all.launch.py index 10a4418..d9d3d73 100644 --- a/src/common/launch_system/launch/start_all.launch.py +++ b/src/common/launch_system/launch/start_all.launch.py @@ -127,6 +127,16 @@ def generate_launch_description(): ) ], ) + rtk = TimerAction( + period=3.4, + actions=[ + ExecuteProcess( + cmd=["ros2", "launch", "rtk", "rtk.launch.py"], + output="screen", + prefix="taskset -c 7", + ) + ], + ) pbox_node = TimerAction( period=3.2, @@ -197,7 +207,7 @@ def generate_launch_description(): mqtt_report, lidar_driver, pointcloud_merger, - pbox_node, + # pbox_node, rtk, route, sub, diff --git a/src/perception/pbox_node_dirve/.gitignore b/src/perception/pbox_node_dirve/.gitignore deleted file mode 100644 index 685f1f9..0000000 --- a/src/perception/pbox_node_dirve/.gitignore +++ /dev/null @@ -1,16 +0,0 @@ -# Prerequisites -*.o -*.i -**/**Release**/ -**/**Debug**/ -Firmware/ -M4_BL/combine.sh -M4_BL/config.txt -*.rej -**/**install**/ -**/**log**/ -**/**build**/ -**/**devel**/ -**/**vscode**/ - - diff --git a/src/perception/pbox_node_dirve/README.md b/src/perception/pbox_node_dirve/README.md deleted file mode 100644 index 3c690ec..0000000 --- a/src/perception/pbox_node_dirve/README.md +++ /dev/null @@ -1,55 +0,0 @@ -2022-12-30 初始版本： - Imu_Pub Gnss_Pub节点Topic发布成功 - -2023-03-02 修复Gnss数据错误问题： - Gnss_Pub发布的数据不正确，重新定义gnss_msg - 按BDDB10格式解析Gnss数据到gnss_msg -2023-04-09 - 增加AG041 三种协议支持 -2023-04-21 - 更新协议处理逻辑，解决BDDB数据处理丢帧问题 -2023-04-26 V1.0.4 - 1. 添加lanuch文件，引入启动参数，解决串口号\波特率\加表陀螺量程范围问题 - 2. 时间戳\ver_pos\ver_vel信号值不对问题修复 - 3. 软中通/VG/升沉版本兼容优化 -2023-06-14 V1.1.0 - 1.添加AG072,AG051协议解析 - 2.修复数据不匹配及丢帧问题 - 3.修复AG041动中通模式温度跳零问题 - 4.增加日志模块TinyLog - 5.增加launch启动可配置功能 -2023-07-30 V2.0.0 - 1.增加udp连接兼容 - 2.协议解析模块封装 - 3.增加标准msg输出兼容 - 4.删除不再使用的代码 - 5.修改包含内部型号相关信息的代码 - 6.增加imu原始数据保存功能 -2023-08-30 V2.0.1 - 1.四元数转换依赖tf2库，修复有些ros无法找到tf2库的问题 -2024-02-19 V3.0.1 - 1. 兼容支持ROS1，通过configure.sh 进行切换 - 切换ROS1指令：sh configure.sh ROS1 - 切换ROS2指令：sh configure.sh ROS2 - 2. 修改node topice名称： - pbox_node: - /imu0A - /imu04 - /imu8B - /ins - /gnss - /odom -2024-02-27 V3.0.2 - 1. 对齐协议版本，修复INS 状态解析错误问题 - 2. INS数据增加周内秒解析输出 -2024-02-28 V3.0.3 - 1. ROS1日志文件存储路径到install目录 -2024-03-27 V3.0.4 - 1. 10 帧数据(GNSS) 按571最新协议进行解析 - 2. 0B帧的轮询数据中的std值进行解析赋值到ins topic -2024-04-12 V3.0.5 - 1. 日志文件写入问题修复 - 2. 10帧数据dop数据解析错位问题修复 - -2025-04-12 V3.0.6 - 1. 修复0A帧解析 时间戳异常问题 \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt b/src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt deleted file mode 100644 index cdcc569..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(imu_msgs) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(std_msgs REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/ASENSING.msg" - "msg/Imu.msg" - "msg/ImuInitial.msg" - "msg/Gnss.msg" - "msg/Imu04.msg" - "msg/Imu0A.msg" - "msg/Imu8B.msg" - "msg/Odom.msg" - DEPENDENCIES std_msgs builtin_interfaces - ) -ament_export_dependencies(rosidl_default_runtime) -ament_package() diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg deleted file mode 100644 index c0a7a5f..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg +++ /dev/null @@ -1,53 +0,0 @@ - -float64 latitude -float64 longitude -float64 altitude - -float64 north_velocity -float64 east_velocity -float64 ground_velocity - -float64 roll -float64 pitch -float64 azimuth - -float64 x_angular_velocity -float64 y_angular_velocity -float64 z_angular_velocity - -float64 x_acc -float64 y_acc -float64 z_acc - -float32 latitude_std -float32 longitude_std -float32 altitude_std - -float32 north_velocity_std -float32 east_velocity_std -float32 ground_velocity_std - - -float32 roll_std -float32 pitch_std -float32 azimuth_std - - -uint32 ins_status -uint32 position_type - -float64 sec_of_week - -float64 gps_week_number - -float32 temperature - -uint32 wheel_speed_status - - - -uint32 heading_type -uint32 numsv - - - diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg deleted file mode 100644 index adf6643..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg +++ /dev/null @@ -1,39 +0,0 @@ -std_msgs/Header header -float64 longitude -float32 lon_sigma -float64 latitude -float32 lat_sigma -float64 altitude -float32 alt_sigma - -uint16 gps_fix -uint16 rtk_age -uint8 flags_pos -uint8 flags_vel -uint8 flags_attitude -uint8 flags_time - -float32 hor_vel -float32 track_angle -float32 ver_vel -float32 latency_vel -float32 base_length - -float32 yaw -float32 yaw_sigma -float32 pitch -float32 pitch_sigma - -string utc_time -uint32 ts_pos -uint32 ts_vel -uint32 ts_heading -uint8 state -uint8 num_master - -float32 gdop -float32 pdop -float32 hdop -float32 htdop -float32 tdop -uint8 num_reserve \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg deleted file mode 100644 index fad581e..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -ASENSING imu_msg \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg deleted file mode 100644 index 7cd41b5..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg +++ /dev/null @@ -1,26 +0,0 @@ -std_msgs/Header header - -float32 roll -float32 pitch -float32 yaw -float32 gx -float32 gy -float32 gz -float32 ax -float32 ay -float32 az -float32 temperature -float32 time -uint8[14] gps_message -uint8 gps_heading_status -uint8 ptype -uint16 pdata -float32 ver_pos -float32 ver_vel -uint16 info_byte - -#0x00:deinit -#0x01:动中通版本数据帧 -#0x02:VG版本数据帧 -#0x03:升沉版本数据帧 -uint8 msg_type diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg deleted file mode 100644 index ae3e507..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg +++ /dev/null @@ -1,12 +0,0 @@ -std_msgs/Header header - -float32 gx -float32 gy -float32 gz -float32 ax -float32 ay -float32 az -float32 temperature -float64 imu_time_stamp -uint8 status -uint16 frame_count \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg deleted file mode 100644 index c34639b..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg +++ /dev/null @@ -1,14 +0,0 @@ -std_msgs/Header header - -uint8 type -uint16 data_length -uint32 frame_count -uint8 serial_number -float32 gx -float32 gy -float32 gz -float32 ax -float32 ay -float32 az -float32 temperature -uint8 status diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/ImuInitial.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/ImuInitial.msg deleted file mode 100644 index 649e3cc..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/ImuInitial.msg +++ /dev/null @@ -1,10 +0,0 @@ -std_msgs/Header header - -float32 gx -float32 gy -float32 gz -float32 ax -float32 ay -float32 az -float32 temperature -float64 imu_time_stamp \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg deleted file mode 100644 index 0c4aa41..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg +++ /dev/null @@ -1,45 +0,0 @@ -std_msgs/Header header -#四元数q0~q3 -float32 q0_w -float32 q1_x -float32 q2_y -float32 q3_z -#位置坐标 -float32 pos_x -float32 pos_y -float32 pos_z -#速度 -float32 vel_x -float32 vel_y -float32 vel_z -#XYZ3个方向合速度 -float32 vel -#角速度 -float32 ang_vel_x -float32 ang_vel_y -float32 ang_vel_z -#加速度 -float32 acc_x -float32 acc_y -float32 acc_z -#odom状态 -uint8 status -#传感器状态 -uint32 sensor_status -#位置信息精度 -float32 pos_x_std -float32 pos_y_std -float32 pos_z_std -#速度信息精度 -float32 vel_x_std -float32 vel_y_std -float32 vel_z_std -#姿态信息精度 -float32 roll_std -float32 pitch_std -float32 yaw_std -#周内秒毫秒 -uint32 tow_ms - - - diff --git a/src/perception/pbox_node_dirve/src/imu_msgs/package.xml b/src/perception/pbox_node_dirve/src/imu_msgs/package.xml deleted file mode 100644 index 80b7b79..0000000 --- a/src/perception/pbox_node_dirve/src/imu_msgs/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - imu_msgs - 0.0.0 - TODO: Package description - shixiaowu - TODO: License declaration - - ament_cmake - rosidl_default_generators - - rosidl_default_runtime - - rosidl_interface_packages - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt b/src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt deleted file mode 100644 index d2f543b..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(pbox_node) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -add_definitions(-DROS2_PLATFORM) -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(serial REQUIRED) -find_package(std_msgs REQUIRED) -find_package(imu_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -#find_package(rosodl_default_generators REQUIRED) -find_package(nav_msgs REQUIRED) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) -include_directories(include ${autopilot_msgs_INCLUDE_DIRS} - ${serial_INCLUDE_DIRS}) -include_directories("/opt/ros/eloquent/include")#eloquent -aux_source_directory(./src SOURCES) -add_executable(pbox_pub ${SOURCES}) - -ament_target_dependencies(pbox_pub rclcpp imu_msgs serial sensor_msgs nav_msgs std_msgs) - -install(TARGETS - pbox_pub - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/perception/pbox_node_dirve/src/pbox_node/include/AGPbox.hpp b/src/perception/pbox_node_dirve/src/pbox_node/include/AGPbox.hpp deleted file mode 100644 index 9c32be0..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/include/AGPbox.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#ifndef _AG_Pbox_HPP_ -#define _AG_Pbox_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "TinyLog.h" -#include "protocol_asensing.h" -#include "rclcpp/rclcpp.hpp" -using namespace std; -using namespace std::chrono_literals; -using namespace serial; - -#define UART_PORT "/dev/ttyTHS1" -#define UART_BAUDRATE (115200) - -class AGPbox : public rclcpp::Node -{ - public: - AGPbox(); - void init_pbox_node(); - - private: - void timer_callback(); - void openSerialPort(); - void readSerialPort(); - - void bindUdp(); - void readUdp(); - - private: - /* serial port info */ - serial::Serial imu_ser; - std::string m_port; - int m_baud; - int m_UsbLatencyTime; - - /* UDP info */ - int m_serverSocket; - std::string m_udpAddr; - int m_udpPort; - struct sockaddr_in m_server; - socklen_t m_sockaddrLen; - bool m_isBind; - char readBuffer[1024]; - - /* read cache */ - std::string m_input; - std::string m_read; - - int m_connectionType; - rclcpp::TimerBase::SharedPtr timer_; - ProtocolAsensing connect_manager; - /* imu log */ - bool m_isPrintLog = false; - FILE* m_logFd = nullptr; -}; -#endif diff --git a/src/perception/pbox_node_dirve/src/pbox_node/include/TinyLog.h b/src/perception/pbox_node_dirve/src/pbox_node/include/TinyLog.h deleted file mode 100644 index dd64b86..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/include/TinyLog.h +++ /dev/null @@ -1,49 +0,0 @@ - -#ifndef __TINYLOG_H__ -#define __TINYLOG_H__ - -#include - -class TinyLog -{ -public: - enum LEVEL{ - DEBUG, - INFO, - WARNING, - ERROR, - FATAL - }; - enum MODE { - SINGLE_THREAD, - MULTI_THREAD - }; - - static void debug(const char* format,...); - static void info(const char* format, ...); - static void warning(const char* format, ...); - static void error(const char* format, ...); - static void fatal(const char* format, ...); - - static void setStorageLevel(int level); - static void setSingleMaxSize(int size); - static void setStorageDir(const char* dir); - static void setLogMode(int mode); - -private: - static int fileSize(const char* path); - static void logConstruct(const int& level, - const char* format, - va_list args); - static void multiThreadConstruct(const int& level, - const char* format, - va_list args); - -private: - static int storageLevel; - static int singleMaxSize; - static std::string storageDir; - static int logMode; -}; - -#endif \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/pbox_node/include/common.h b/src/perception/pbox_node_dirve/src/pbox_node/include/common.h deleted file mode 100644 index a34dcb0..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/include/common.h +++ /dev/null @@ -1,250 +0,0 @@ -#ifndef __COMMON_H__ -#define __COMMON_H__ - -// #include -#include -#include -#include -// #include -// #include -#include -#ifndef PI -#define PI 3.1415926535897932 /* pi */ -#endif - -#ifndef D2R -#define D2R (PI/180.0) /* deg to rad */ -#endif - -const int G_INTERVAL_UPDATE_TIME = 100;//��������ˢ�¼�� - -const std::string G_DOWN_SOURCETABLE_FINISHED = "download source table finished"; - -/// -/// ��־�������� -/// -enum class ConnectType -{ - eNone = 0, - eTxt = 1,//�ı� - eVideo = 2,//ý�����ݣ�����USB����ͷ - eSerial = 3,//�������� - eCan = 4,//can���� - eUdp = 5,//��̫������ - eTcp = 6,//��̫������ - eSomeip = 7,//��̫������ - eDoip = 8,//��̫������ - eNtrip = 9 -}; - -/// -/// ������ݵ����� -/// -enum class ProtocolType -{ - eNone = 0, - eIns = 1,//ins - eGnss = 2,//gnss - ePoll = 3,//���� - eVideo = 4 -}; - -enum class LogType { - eNone = 0, - eText,//��ͨ�ı� - eVideoRecordStart,//��ʼ¼����Ƶ - eVideoRecordStop,//����¼����Ƶ - eSerialPortConnect,//���� - eSerialPort,//���� - eSerialPortClose,//���� - eCanConnect,//CAN - eCan,//CAN - eCanClose,//CAN - eCanfdConnect,//CANFD - eCanfd,//CANFD - eCanfdClose,//CANFD - eUdpListen,//udp - eUdp,//udp - eUdpClose,//udp - eTcpConnect,//tcp - eTcp,//tcp - eTcpClose,//tcp - eDoipConnectTcp,//Doip - eDoipTcp,//Doip - eDoipCloseTcp,//Doip - eDoipConnectUdp,//Doip - eDoipUdp,//Doip - eDoipCloseUdp,//Doip - eSomeipConnectTcp,//Some/Ip - eSomeipTcp,//Some/Ip - eSomeipCloseTcp,//Some/Ip - eSomeipConnectUdp,//Some/Ip - eSomeipUdp,//Some/Ip - eSomeipCloseUdp,//Some/Ip - eNtripConnect,//tcp - eNtrip,//tcp - eNtripClose//tcp -}; - -/// -/// ״̬�ռ�ö��ֵ -/// -enum class StatusUi -{ - eNone, - eSerial, - eCan, - eUdp, - eTcp, - ePosLoss,//RTK - eBiasDiffuse,//��ƫ��ɢ - eSizeDiffuse,//�˱۷�ɢ - eAngleDiffuse,//��̬��ɢ - eRtkOff,//�����ʱ - eWheelSpeedLoss,//���ٶ�ʧ - eGnssLoss//GNSS��ʧ -}; - -/// -/// ����״ֵ̬ -/// -enum class StatusType -{ - eNone, - eOpenOrValid, - eError, - eCloseOrOver -}; - -/// -/// ������¼��������¼��key��ip��ַ+���� -/// -struct ReceiveDataKey -{ - ReceiveDataKey() {} - ReceiveDataKey(uint32_t ip, int type) :_ip(ip), _asc_type(type) {} - uint32_t _ip = 0; - int _asc_type = 0;//1:IMUInfoNotify,2:INSInfoNotify,3:INSStatusInfoNotify,4:GNSSInfoInfoNotify,5:PartNum - bool operator<(const ReceiveDataKey& other) const { - if (_ip < other._ip) - { - return true; - } - if (_ip == other._ip && _asc_type < other._asc_type) - { - return true; - } - return false; - } -}; - -/// -/// ��ת���ݣ���С��ת���� -/// -/// -/// -void reverseData(uint8_t* temp, int length); - -/// -/// ������תʵ������,�Դ���С��ת�� -/// -/// -/// -/// -/// -template -T serializeData(const uint8_t* add, int& index) -{ - if (sizeof(T) > 1) - { - uint8_t* data = (uint8_t*)malloc(sizeof(T)); - memcpy(data, add + index, sizeof(T)); - reverseData(data, sizeof(T)); - T result = *((T*)(data)); - free(data); - index += sizeof(T); - return result; - } - else - { - T result = *(T*)(add + index); - index += sizeof(T); - return result; - } -} - -/// -/// ������תʵ������ -/// -/// -/// -/// -/// -template -T toValue(const uint8_t* add, int& index) -{ - T result = *(T*)(add + index); - index += sizeof(T); - return result; -} - -/// -/// ������תʵ������ -/// -/// -/// -/// -/// -template -T toValue(const char* add, int& index) -{ - T result = *(T*)(add + index); - index += sizeof(T); - return result; -} - -template -std::string toBytes(const T& data) -{ - return std::string((char*)&data, sizeof(T)); -} - -/// -/// �ַ���0~F/f)תʮ���� -/// -/// -/// -char convertToBit(char data); - -/// -/// ������ת16�������� -/// -/// -/// -// QString convertToHex(const std::string& data); - -/// -/// 16��������ת�����ƣ����硰FF"->255 -/// -/// -/// -// std::string convertToBin(const QString& origin_data); - -// #define COLOR_RGBToINT(r,g,b) COLOR_RGBAToINT(r,g,b,255) -// #define COLOR_RGBAToINT(r,g,b,a) (b + g * 256 + r * 65536 + a * 16777216) -// #define COLOR_R(c) ((c & 0x00ff0000) >> 16) -// #define COLOR_G(c) ((c & 0x0000ff00) >> 8) -// #define COLOR_B(c) ((c & 0x000000ff)) -// #define COLOR_A(c) ((c & 0xff000000) >> 24) - -// static QColor IntToColor(int color) -// { -// return QColor(COLOR_R(color), COLOR_G(color), COLOR_B(color), COLOR_A(color)); -// } - -// static int ColorToInt(const QColor& color) -// { -// return COLOR_RGBAToINT(color.red(), color.green(), color.blue(), color.alpha()); -// } - -#endif diff --git a/src/perception/pbox_node_dirve/src/pbox_node/include/protocol_asensing.h b/src/perception/pbox_node_dirve/src/pbox_node/include/protocol_asensing.h deleted file mode 100644 index 0b28675..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/include/protocol_asensing.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef __PROTOCOL_ASENSING_H__ -#define __PROTOCOL_ASENSING_H__ - -#include -#include -#include -#include -#include -#include "common.h" - -class ProtocolAsensing -{ -public: - - explicit ProtocolAsensing(); - virtual ~ProtocolAsensing(); - - /* subclass parse Protocol */ - virtual void subData(const uint8_t* sub_address, int& index){ - printf("get protocol type:%02X %02X %02X \n" , sub_address[index] , - sub_address[index + 1] , - sub_address[index + 2]); - } - - void addData(const std::string& data); - void clearCache(); - bool registProtocol(const std::string &protocolFlag , int length , ProtocolAsensing* sub); - void toQuaternion(double* rpy , double* quaterArray); - - inline int getLength(const std::string& flag) - { - return protocolLengthMap[flag]; - } - inline uint32_t getDataSize() - { - return _receive_data.size(); - } - inline void changeLength(const std::string& flag , int len) - { - protocolLengthMap[flag] = len; - } - -private: - bool analysisData(const std::string& data); - -private: - std::string _receive_data;//解析自定义数据 - - static std::map protocolLengthMap; - static std::map protocolMap; -}; - -#endif diff --git a/src/perception/pbox_node_dirve/src/pbox_node/launch/pbox_node.launch.py b/src/perception/pbox_node_dirve/src/pbox_node/launch/pbox_node.launch.py deleted file mode 100644 index 64003c4..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/launch/pbox_node.launch.py +++ /dev/null @@ -1,52 +0,0 @@ -"""Launch a talker and a listener in a component container.""" - -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - """Generate launch description with multiple components.""" - container = ComposableNodeContainer( - package="pbox_node", - executable="pbox_pub", - namespace="", - name="pbox_pub", - output="screen", - # AG_Pbox 节点运行配置参数 - parameters=[ - # 输出msg类型：ros2标准msg：0 , Asensing msg：1 - # default: 1 Asensing msg - {"MsgType": 1}, - # 连接类型：serial port：0 , UDP：1 - # default: 0 serial port - {"ConnectionType": 0}, - # 串口设备串 defaule: /dev/ttyTHS1 - {"UART_Port": "/dev/ttyTHS1"}, - # 串口波特率 default: 115200 - {"UART_Baudrate": 115200}, - # latency_timer :1 ~ 16, default:16 - {"USB_LatencyTime": 16}, - # UDP addr default 192.168.225.2 - {"UDP_Addr": "192.168.225.2"}, - # UDP port default 12300 - {"UDP_Port": 12300}, - # IMU BDDB0A协议类型 0：V1.0 1：V2.0----072 - {"ProtocolType": 0}, - # 5503协议：陀螺量程 - {"Grange04": 250.0}, - # AG041协议：加表量程 - {"Arange04": 4.0}, - # 570D协议：陀螺量程 - {"Grange0B": 250.0}, - # 570D协议：加表量程 - {"Arange0B": 4.0}, - # 设置日志文件路径和文件名 - # default: close - {"LogInfo": "./debug.log"}, - # 设置日志打印等级：DEBUG:0 (save imu rawdata),INFO:1,WARNING:2,ERROR:3,FATAL:4 - # default:INFO - {"LogLevel": 1}, - ], - ) - return launch.LaunchDescription([container]) diff --git a/src/perception/pbox_node_dirve/src/pbox_node/package.xml b/src/perception/pbox_node_dirve/src/pbox_node/package.xml deleted file mode 100644 index e48b46e..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - pbox_node - 3.0.1 - TODO: Package description - yfsw@asensing - TODO: License declaration - - ament_cmake - serial - - rclcpp - sensor_msgs - imu_msgs - std_msgs - nav_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp deleted file mode 100644 index fc11873..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include -#include "AGPbox.hpp" -#include -#include "TinyLog.h" -#include "protocol_asensing.h" - -const std::string copyright = \ -"\n*******************************************************************************\n\ - Copyright (C) Asensing (2024). \n\ -All rights reserved. This software is Asensing property. Duplication or \n\ -disclosure without Asensing written authorization is prohibited. \n\ -********************************************************************************\n\ -@Node Description : ROS2 node dirver for Asensing Pbox \n\ -@Author : yfsw email:yfsw@asensing.com \n\ -@Version : V3.0.6 \n\ -@Release_Date : 2025-06-30 \n\ -********************************************************************************"; - -AGPbox :: AGPbox() : Node("pbox_node") -{ - -} - -void AGPbox::init_pbox_node() -{ - std::string logInfo = ""; - int32_t logLevel = 1; - - TinyLog::info("pbox_node"); - - /* all parameter declare and init here */ - this->declare_parameter("MsgType", 1); - this->declare_parameter("ConnectionType", 0); - /* serial port */ - this->declare_parameter("UART_Port", UART_PORT); - this->declare_parameter("UART_Baudrate", UART_BAUDRATE); - /* UDP */ - this->declare_parameter("UDP_Addr", "192.168.119.133"); - this->declare_parameter("UDP_Port", 12300); - this->declare_parameter("USB_LatencyTime" , 16); - /* log */ - this->declare_parameter("LogInfo", ""); - this->declare_parameter("LogLevel", 1); - /* range */ - this->declare_parameter("Grange04", 250.0); - this->declare_parameter("Arange04", 4.0); - this->declare_parameter("Grange0B", 4.0); - this->declare_parameter("Arange0B", 4.0); - - /* init parameter */ - this->get_parameter("ConnectionType" , m_connectionType); - this->get_parameter("UART_Port",m_port); - this->get_parameter("UART_Baudrate",m_baud); - this->get_parameter("UDP_Addr" , m_udpAddr); - this->get_parameter("UDP_Port" , m_udpPort); - this->get_parameter("USB_LatencyTime" , m_UsbLatencyTime); - - /* set log config */ - - this->get_parameter("LogInfo",logInfo); - this->get_parameter("LogLevel",logLevel); - std::string path = "./ImuDebug.log"; - - TinyLog::info("LogPathInfo: %s",logInfo.c_str()); - TinyLog::setStorageDir(logInfo.c_str()); - TinyLog::setStorageLevel(logLevel); - TinyLog::info("%s",copyright.c_str()); - if(logLevel == 0) - { - m_isPrintLog = true; - m_logFd = fopen(path.c_str(), "wb+"); - if (m_logFd == nullptr) - { - TinyLog::error("open log path fail,please check your launch file!"); - } - TinyLog::info("open log path : %s ",path.c_str()); - } - if(0 == m_connectionType) - { - openSerialPort(); - } - else if(1 == m_connectionType) - { - bindUdp(); - } - - // 创建定时器（在串口/UDP打开之后，使用10ms周期避免过快读取） - timer_ = this->create_wall_timer(10ms, std::bind(&AGPbox::timer_callback, this)); -} - -void AGPbox::timer_callback() -{ - if(0 == m_connectionType) - { - readSerialPort(); - } - else if(1 == m_connectionType) - { - readUdp(); - } -} -void AGPbox::openSerialPort() -{ - try - { - if((m_UsbLatencyTime < 16) && (m_UsbLatencyTime >= 1)) - { - char command[128] = {0}; - sprintf(command , "echo %d > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer" , m_UsbLatencyTime); - system("sudo chmod 777 /sys/bus/usb-serial/devices/ttyUSB0/latency_timer"); - system((const char*)command); - system("sync"); - } - - imu_ser.setPort(m_port); - imu_ser.setBaudrate(m_baud); - serial::Timeout to = serial::Timeout::simpleTimeout(100); - imu_ser.setTimeout(to); - imu_ser.setFlowcontrol(serial::flowcontrol_t::flowcontrol_none); - imu_ser.open(); - } - catch (serial::IOException& e) - { - TinyLog::error("imu_serial opened failed! please check your connection!"); - } - - if (imu_ser.isOpen()) - { - TinyLog::info("imu_serial opened successfully!"); - } - else - { - static bool isFirstOpen = true; - if(isFirstOpen) - { - isFirstOpen = false; - exit(0); - } - } -} -void AGPbox::readSerialPort() -{ - try - { - if (imu_ser.isOpen()) - { - if(imu_ser.available()) - { - m_read = imu_ser.read(imu_ser.available()); - m_input += m_read; - connect_manager.addData(m_input); - m_input.erase(0, m_input.size()); - if(m_isPrintLog) - { - fwrite(m_read.c_str() , m_read.size() , 1u , m_logFd); - } - } - } - else - { - openSerialPort(); - } - } - catch (serial::IOException& e) - { - TinyLog::error("Error reading from the serial port:%s close port!" , imu_ser.getPort().c_str()); - imu_ser.close(); - } -} - -void AGPbox::bindUdp() -{ - m_serverSocket = socket(AF_INET, SOCK_DGRAM, 0); - (void)fcntl(m_serverSocket, F_SETFL, fcntl(m_serverSocket, F_GETFL, 0)|O_NONBLOCK); - - m_server.sin_family = AF_INET; - m_server.sin_addr.s_addr = inet_addr(m_udpAddr.c_str()); - printf("%s : %d",m_udpAddr.c_str(),m_udpPort); - m_server.sin_port = htons(m_udpPort); - - if(bind(m_serverSocket, (sockaddr *)&m_server, sizeof(sockaddr)) < 0) - { - (void)close(m_serverSocket); - TinyLog::error("UDP bind fail!"); - m_isBind = false; - } - else - { - m_isBind = true; - TinyLog::info("UDP bind success!"); - } -} -void AGPbox::readUdp() -{ - if(m_isBind) - { - m_sockaddrLen = sizeof(sockaddr); - int readLen = 0; - if(m_sockaddrLen) - { - do - { - memset(readBuffer , 0 , sizeof(readBuffer)); - readLen = recvfrom(m_serverSocket , readBuffer, 1024*sizeof(char), 0, (sockaddr *)&m_server, &m_sockaddrLen); - - if (readLen > 0) - { - // printf("%d\n" , readLen); - connect_manager.addData(std::string((char*)readBuffer, readLen)); - if(m_isPrintLog) - { - // for(int i = 0; i < readLen;i++) - { - // fprintf(m_logFd , "%02X" , readBuffer[i]); - fwrite(readBuffer , readLen , 1u , m_logFd); - } - } - } - }while (readLen > 0); - } - } -} diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp deleted file mode 100644 index 15b0b92..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp +++ /dev/null @@ -1,245 +0,0 @@ - -#include "TinyLog.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef _WIN32 -#include -#else -#include -#include -#endif - -int TinyLog::storageLevel = TinyLog::INFO; /* storage to file , default INFO */ - -int TinyLog::singleMaxSize = 10485760; /* unit��Byte��default 10MB */ - -std::string TinyLog::storageDir = "./"; /* storage to dir , default ./log */ - -int TinyLog::logMode = TinyLog::MULTI_THREAD; - -void TinyLog::debug(const char* format, ...) -{ - va_list args; - va_start(args, format); - - logConstruct(TinyLog::DEBUG, format, args); - - va_end(args); -} - -void TinyLog::info(const char* format, ...) -{ - va_list args; - va_start(args, format); - - logConstruct(TinyLog::INFO, format, args); - - va_end(args); -} - -void TinyLog::warning(const char* format, ...) -{ - va_list args; - va_start(args, format); - - logConstruct(TinyLog::WARNING, format, args); - - va_end(args); -} - -void TinyLog::error(const char* format, ...) -{ - va_list args; - va_start(args, format); - - logConstruct(TinyLog::ERROR, format, args); - - va_end(args); -} - -void TinyLog::fatal(const char* format, ...) -{ - va_list args; - va_start(args, format); - - logConstruct(TinyLog::FATAL, format, args); - - va_end(args); -} - -void TinyLog::setStorageLevel(int level) -{ - switch (level) - { - case TinyLog::DEBUG: - storageLevel = TinyLog::DEBUG; - break; - case TinyLog::INFO: - storageLevel = TinyLog::INFO; - break; - case TinyLog::WARNING: - storageLevel = TinyLog::WARNING; - break; - case TinyLog::ERROR: - storageLevel = TinyLog::ERROR; - break; - case TinyLog::FATAL: - storageLevel = TinyLog::FATAL; - break; - default: - break; - } -} - -void TinyLog::setSingleMaxSize(int size) -{ - singleMaxSize = size; -} - -void TinyLog::setStorageDir(const char* dir) -{ - storageDir = dir; -} - -void TinyLog::setLogMode(int mode) -{ - switch (mode) - { - case TinyLog::SINGLE_THREAD: - logMode = TinyLog::SINGLE_THREAD; - break; - case TinyLog::MULTI_THREAD: - logMode = TinyLog::MULTI_THREAD; - break; - default: - break; - } -} - -int TinyLog::fileSize(const char *path) -{ - int size = -1; - FILE *fp = fopen(path, "r"); - if (fp) - { - fseek(fp, 0L, SEEK_END); - size = ftell(fp); - fclose(fp); - } - return size; -} - -void TinyLog::logConstruct(const int& level, - const char* format, - va_list args) -{ - switch (logMode) - { - case TinyLog::SINGLE_THREAD: - break; - case TinyLog::MULTI_THREAD: - multiThreadConstruct(level, format, args); - break; - default: - break; - } -} - - -void TinyLog::multiThreadConstruct(const int& level, - const char* format, - va_list args) -{ - const char* logLevel = NULL; - switch (level) - { - case TinyLog::DEBUG: - logLevel = "DEBUG"; - break; - case TinyLog::INFO: - logLevel = "INFO"; - break; - case TinyLog::WARNING: - logLevel = "WARNING"; - break; - case TinyLog::ERROR: - logLevel = "ERROR"; - break; - case TinyLog::FATAL: - logLevel = "FATAL"; - break; - default: - logLevel = ""; - break; - } - - time_t tt = time(NULL); - tm* t = localtime(&tt); - struct timeval ttm; - gettimeofday(&ttm , 0); - auto ttt = ttm.tv_usec; - - char content[2048] = { 0 }; - char log[sizeof(content) + 50] = { 0 }; - - std::ostringstream oss; - oss << std::this_thread::get_id(); - std::string threadId = oss.str(); - - vsprintf(content, format, args); - sprintf(log, - "[%s] [%d-%02d-%02d %02d:%02d:%02d.%03ld] %s", - logLevel, - t->tm_year + 1900, - t->tm_mon + 1, - t->tm_mday, - t->tm_hour, - t->tm_min, - t->tm_sec, - ttt/1000, - content); - // printf("%s\n", log); - std::cout<= storageLevel) - { - std::string filename = storageDir; - if (fileSize(filename.c_str()) > singleMaxSize) - { - std::chrono::milliseconds now = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - long long msTimestamp = now.count(); - std::string backupFilename = filename + "." + std::to_string(msTimestamp); - rename(filename.c_str(), backupFilename.c_str()); - } - else - { - FILE *fp = NULL; - fp = fopen(filename.c_str(), "a+"); - if (fp != NULL) - { - fputs(log, fp); - fputs("\n", fp); - fclose(fp); - fp = NULL; - } - else - { - static bool printWarning = false; - if(false == printWarning) - { - printWarning = true; - // printf("[TinyLog]: open or create log file failed!\n"); - } - } - } - } -} \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp deleted file mode 100644 index 9ce7f3b..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "common.h" - -void reverseData(uint8_t* temp, int length) -{ - for (int i = 0; i < length / 2; i++) - { - std::swap(temp[i], temp[length - 1 - i]); - } -} - -char convertToBit(char data) -{ - if (data >= 48 && data <= 57) - {//0-9 - return data - 48; - } - else if (data >= 65 && data <= 70) - { - return data - 55; - } - else - { - return data - 87; - } -} - -// QString convertToHex(const std::string& data) -// { -// QString result_data; -// char sub_data[4] = { 0 }; -// uchar* address = (uchar*)data.c_str(); -// for (size_t i = 0; i < data.size(); ++i) -// { -// sprintf(sub_data, "%02X ", address[i]); -// result_data += QString::fromLocal8Bit(QByteArray(sub_data, 3)); -// } -// return result_data; -// } - -// std::string convertToBin(const QString& origin_data) -// { -// QString data = origin_data; -// std::string result_data(data.size() / 2, 0); -// for (size_t i = 0; i < data.size(); i += 2) -// { -// uint8_t f = convertToBit(data.at(i).cell()); -// uint8_t s = convertToBit(data.at(i + 1).cell()); -// result_data[i / 2] = (f << 4) + s; -// } -// return result_data; -// } \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp deleted file mode 100644 index 3d8f804..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// #include "AG_Pbox.hpp" -#include "AGPbox.hpp" - -#include "./protocol/decode_0A.cpp" -#include "./protocol/decode_04.cpp" -#include "./protocol/decode_8B.cpp" -#include "./protocol/decode_0B.cpp" -#include "./protocol/decode_10.cpp" -#include "./protocol/decode_1B.cpp" - -int main(int argc, char** argv) -{ - //初始化节点 - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - node->init_pbox_node(); - /* create publisher */ - #ifdef PROTOCOL_BDDB0A - auto publish0A = std::make_shared(node); - #endif - #ifdef PROTOCOL_BDDB04 - auto publish04 = std::make_shared(node); - #endif - #ifdef PROTOCOL_BDDB8B - auto publish8B = std::make_shared(node); - #endif - #ifdef PROTOCOL_BDDB0B - auto publish0B = std::make_shared(node); - #endif - #ifdef PROTOCOL_BDDB10 - auto publish10 = std::make_shared(node); - #endif - #ifdef PROTOCOL_BDDB1B - auto publish1B = std::make_shared(node); - #endif - - //循环节点 - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_04.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_04.cpp deleted file mode 100644 index a1d676f..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_04.cpp +++ /dev/null @@ -1,137 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" -#include "AGPbox.hpp" - -/* Asensing msg */ -#include "imu_msgs/msg/imu04.hpp" -/* ros2 msg */ -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" - -#define PROTOCOL_BDDB04 - -class Decode_04 final : public ProtocolAsensing -{ - -public: - Decode_04(std::shared_ptr node) : m_node(node) - { - m_node->get_parameter("MsgType" , m_msgSwitch); - m_node->get_parameter("Grange04" , m_grange); - m_node->get_parameter("Arange04" , m_arange); - registProtocol(m_type , m_length , this); - } - - ~Decode_04(){} - - void subData(const uint8_t* sub_address, int& index) - { - int dataLength = getLength(m_type); - - // 简单的XOR校验 - uint8_t check_sum = 0; - for (int i = 0; i < dataLength - 1; ++i) - { - check_sum ^= sub_address[i]; - } - - if (check_sum == sub_address[dataLength - 1]) - { - int16_t middle; - uint32_t tmpU32; - int sub_index = 3; - - // gyro - m_pubAsensingMsg.gx = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - m_pubAsensingMsg.gy = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - m_pubAsensingMsg.gz = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - // acc - m_pubAsensingMsg.ax = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - m_pubAsensingMsg.ay = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - m_pubAsensingMsg.az = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - // temp - middle = toValue(sub_address, sub_index); - m_pubAsensingMsg.temperature = middle * 200.0 / 32768; - // time - tmpU32 = toValue(sub_address, sub_index); - m_pubAsensingMsg.time = (double)tmpU32 / 4000; - - /* msg header */ - m_pubAsensingMsg.header.frame_id = m_frameId; - m_pubAsensingMsg.header.stamp = rclcpp::Clock().now(); - - if(m_msgSwitch == 1) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicName.c_str(), 500); - } - publisher->publish(std::move(m_pubAsensingMsg)); - } - else if(m_msgSwitch == 0) - { - /* ros2 msg type */ - static bool isCreate = false; - static sensor_msgs::msg::Imu pubMsg; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicName.c_str(), 500); - } - sub_index = 3; - pubMsg.angular_velocity.x = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - pubMsg.angular_velocity.y = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - pubMsg.angular_velocity.z = (toValue(sub_address, sub_index)) * m_grange / 32768.0; - - pubMsg.linear_acceleration.x = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - pubMsg.linear_acceleration.y = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - pubMsg.linear_acceleration.z = (toValue(sub_address, sub_index)) * m_arange / 32768.0; - - /* msg header */ - pubMsg.header.frame_id = m_frameId; - pubMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(pubMsg)); - } - else - { - static bool isPrint = false; - if(!isPrint) - { - TinyLog::error("unknown msg type , please check your launch file!"); - isPrint = true; - } - } - index += dataLength; - } - else - { - index += 3; - } - } - -private: - /* protocol info */ - std::string m_type = "BDDB04"; - int m_length = 44; - - /* topic info */ - std::string m_topicName = "Imu04"; - std::string m_frameId = "Imu04"; - - /* node */ - std::shared_ptr m_node; - /* msg */ - sensor_msgs::msg::Imu m_pubRos2Msg; - imu_msgs::msg::Imu04 m_pubAsensingMsg; - - /* use ros msg or asensing msg */ - int m_msgSwitch = 1; - /* range */ - float m_grange; - float m_arange; -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0A.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0A.cpp deleted file mode 100644 index 3c4d59c..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0A.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" - -#include "AGPbox.hpp" -/* Asensing msg */ -#include "imu_msgs/msg/imu0_a.hpp" -#include "imu_msgs/msg/imu_initial.hpp" -// /* ros2 msg */ -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" - -#define PROTOCOL_BDDB0A - -class Decode_0A final : public ProtocolAsensing -{ - -public: - Decode_0A(std::shared_ptr node) : m_node(node) - { - // m_node->get_parameter("MsgType" , m_msgSwitch); - // m_node->get_parameter("ProtocolType" , m_protocolType); - registProtocol(m_type , m_length , this); - } - - ~Decode_0A(){} - - void subData(const uint8_t* sub_address, int& index) - { - - int sub_index = 3; - uint8_t check_sum = 0; - int dataLength = getLength(m_type); - /* check xor */ - for (int i = 0; i < dataLength -1; ++i) - { - check_sum ^= sub_address[i]; - } - - if (check_sum == sub_address[dataLength -1]) - { - int16_t middle; - uint32_t tmpU32; - sub_index = 3; - // gyro - imu0a_data.gyro_x = toValue(sub_address, sub_index); - imu0a_data.gyro_y = toValue(sub_address, sub_index); - imu0a_data.gyro_z = toValue(sub_address, sub_index); - // acc - imu0a_data.acc_x = toValue(sub_address, sub_index); - imu0a_data.acc_y = toValue(sub_address, sub_index); - imu0a_data.acc_z = toValue(sub_address, sub_index); - - // temp - middle = toValue(sub_address, sub_index); - imu0a_data.temperature = middle * 200.0 / 32768; - - tmpU32 = toValue(sub_address, sub_index); - imu0a_data.time_stamp = (double)tmpU32/4000; //for 570D 1/1000; other 1/4000 - - publish_ros_msg(); - - index += dataLength; - } - else - { - index += 3; - } - } - - //BDDB0A protocol define - typedef struct _asensing_imu0a - { - float gyro_x; - float gyro_y; - float gyro_z; - float acc_x; - float acc_y; - float acc_z; - float temperature; - double time_stamp; - uint8_t status; - uint16_t frame_count; - }asensing_imu0a_t; - -private: - void publish_ros_msg(void) - { - if(m_msgSwitch == 1) - { - /* Asensing msg type */ - m_msg0A.gx = m_msg.gx = imu0a_data.gyro_x; - m_msg0A.gy = m_msg.gy = imu0a_data.gyro_y; - m_msg0A.gz = m_msg.gz = imu0a_data.gyro_z; - m_msg0A.ax = m_msg.ax = imu0a_data.acc_x; - m_msg0A.ay = m_msg.ay = imu0a_data.acc_y; - m_msg0A.gz = m_msg.az = imu0a_data.acc_z; - m_msg0A.temperature = m_msg.temperature = imu0a_data.temperature; - m_msg0A.imu_time_stamp = m_msg.imu_time_stamp = imu0a_data.time_stamp; - - if(m_protocolType == 1) - { - m_msg0A.status = imu0a_data.status; - m_msg0A.frame_count = imu0a_data.frame_count; - /* msg header */ - m_msg0A.header.frame_id = m_frameId; - m_msg0A.header.stamp = rclcpp::Clock().now(); - if(!isCreate) - { - isCreate = true; - m_pub0A = m_node->create_publisher(m_topicName.c_str(), 500); - } - m_pub0A->publish(std::move(m_msg0A)); - } - else if(m_protocolType == 0) - { - /* msg header */ - m_msg.header.frame_id = m_frameId; - m_msg.header.stamp = rclcpp::Clock().now(); - if(!isCreate) - { - isCreate = true; - m_pub = m_node->create_publisher(m_topicName.c_str(), 500); - } - m_pub->publish(std::move(m_msg)); - } - else - { - TinyLog::info("protocol type error:%d" , m_protocolType); - } - } - else if(m_msgSwitch == 0) - { - /* ros2 msg type */ - // static bool isCreate = false; - // static sensor_msgs::msg::Imu pubMsg; - // static rclcpp::Publisher::SharedPtr publisher; - // if(!isCreate) - // { - // isCreate = true; - // publisher = m_node->create_publisher(m_topicName.c_str(), 500); - // } - // /* frame BDDB0A not have roll pitch yaw */ - - // pubMsg.angular_velocity.x = toValue(sub_address, sub_index); - // pubMsg.angular_velocity.y = toValue(sub_address, sub_index); - // pubMsg.angular_velocity.z = toValue(sub_address, sub_index); - - // pubMsg.linear_acceleration.x = toValue(sub_address, sub_index); - // pubMsg.linear_acceleration.y = toValue(sub_address, sub_index); - // pubMsg.linear_acceleration.z = toValue(sub_address, sub_index); - - // /* msg header */ - // pubMsg.header.frame_id = m_frameId; - // pubMsg.header.stamp = rclcpp::Clock().now(); - // publisher->publish(std::move(pubMsg)); - } - else - { - static bool isPrint = false; - if(!isPrint) - { - TinyLog::error("unknown msg type , please check your launch file!"); - isPrint = true; - } - } - } - -public: - asensing_imu0a_t imu0a_data; - -private: - /* protocol info */ - std::string m_type = "BDDB0A"; - int m_length = 34; - - /* topic info */ - std::string m_topicName = "Imu0A"; - std::string m_frameId = "Imu0A"; - - /* node */ - std::shared_ptr m_node; - imu_msgs::msg::ImuInitial m_msg; - imu_msgs::msg::Imu0A m_msg0A; - rclcpp::Publisher::SharedPtr m_pub; - rclcpp::Publisher::SharedPtr m_pub0A; - - /* use ros msg or asensing msg */ - int m_msgSwitch = 1; - int m_protocolType; - bool isCreate = false; -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0B.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0B.cpp deleted file mode 100644 index 37748aa..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0B.cpp +++ /dev/null @@ -1,195 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" -#include "AGPbox.hpp" - -/* Asensing msg */ -#include "imu_msgs/msg/imu.hpp" -/* ros2 msg */ -#include "sensor_msgs/msg/imu.hpp" - -#define PROTOCOL_BDDB0B - -class Decode_0B final : public ProtocolAsensing -{ - -public: - Decode_0B(std::shared_ptr node) : m_node(node) - { - m_node->get_parameter("MsgType" , m_msgSwitch); - m_node->get_parameter("Grange0B" , m_grange); - m_node->get_parameter("Arange0B" , m_arange); - registProtocol(m_typeImu , m_lengthImu , this); - } - - ~Decode_0B(){} - - void subData(const uint8_t* sub_address, int& index) - { - std::string type = ""; - char str[16] = {0}; - sprintf(str , "%02X%02X%02X" , sub_address[0] , - sub_address[1] , - sub_address[2]); - type += str; - if(type == m_typeImu) - { - parse0B(sub_address , index); - } - else - { - index += 3; - TinyLog::error("protocol type error:%s!" , type.c_str()); - } - } - - void parse0B(const uint8_t* data, int& pos) - { - int sub_index = 3; - uint8_t check_sum = 0; - int dataLength = getLength(m_typeImu); - /* check xor */ - for (int i = 0; i < dataLength -1; ++i) - { - check_sum ^= data[i]; - } - - if(check_sum != data[dataLength - 1]) - { - pos += 4; - } - else - { - int16_t temp[3]; - /* roll pitch yaw */ - m_imuMsg.imu_msg.roll = (toValue(data, sub_index)) * 360.0 / 32768; - m_imuMsg.imu_msg.pitch = (toValue(data, sub_index)) * 360.0 / 32768; - m_imuMsg.imu_msg.azimuth = (toValue(data, sub_index)) * 360.0 / 32768; - - /* gx gy gz , ax ay az */ - m_imuMsg.imu_msg.x_angular_velocity = (toValue(data, sub_index)) * 300.0 / 32768; - m_imuMsg.imu_msg.y_angular_velocity = (toValue(data, sub_index)) * 300.0 / 32768; - m_imuMsg.imu_msg.z_angular_velocity = (toValue(data, sub_index)) * 300.0 / 32768; - m_imuMsg.imu_msg.x_acc = (toValue(data, sub_index)) * 12.0 / 32768; - m_imuMsg.imu_msg.y_acc = (toValue(data, sub_index)) * 12.0 / 32768; - m_imuMsg.imu_msg.z_acc = (toValue(data, sub_index)) * 12.0 / 32768; - - m_imuMsg.imu_msg.latitude = (toValue(data, sub_index)) * 0.0000001; - m_imuMsg.imu_msg.longitude = (toValue(data, sub_index)) * 0.0000001; - m_imuMsg.imu_msg.altitude = (toValue(data, sub_index)) * 0.001; - m_imuMsg.imu_msg.north_velocity = (toValue(data, sub_index)) * 100.0 / 32768; - m_imuMsg.imu_msg.east_velocity = (toValue(data, sub_index)) * 100.0 / 32768; - m_imuMsg.imu_msg.ground_velocity = (toValue(data, sub_index)) * 100.0 / 32768; - - m_imuMsg.imu_msg.ins_status = data[sub_index ++]; - - sub_index += 4; - sub_index += 2; - temp[0] = toValue(data, sub_index); - temp[1] = toValue(data, sub_index); - temp[2] = toValue(data, sub_index); - m_ts = (toValue(data, sub_index)) / 4000.0; - switch (data[sub_index++]) - { - case 0: - m_imuMsg.imu_msg.latitude_std = exp(temp[0]/100); - m_imuMsg.imu_msg.longitude_std = exp(temp[1]/100); - m_imuMsg.imu_msg.altitude_std = exp(temp[2]/100); - break; - case 1: - m_imuMsg.imu_msg.north_velocity_std = exp(temp[0]/100); - m_imuMsg.imu_msg.east_velocity_std = exp(temp[1]/100); - m_imuMsg.imu_msg.ground_velocity_std = exp(temp[2]/100); - break; - case 2: - m_imuMsg.imu_msg.roll_std = exp(temp[0]/100); - m_imuMsg.imu_msg.pitch_std = exp(temp[1]/100); - m_imuMsg.imu_msg.azimuth_std = exp(temp[2]/100); - break; - case 22: - m_imuMsg.imu_msg.temperature = temp[0] * 200.0 / 32768; - break; - case 32: - m_imuMsg.imu_msg.position_type = temp[0]; - m_imuMsg.imu_msg.numsv = temp[1]; - m_imuMsg.imu_msg.heading_type = temp[2]; - break; - case 33: - m_imuMsg.imu_msg.wheel_speed_status = temp[1]; - break; - default: - break; - } - m_imuMsg.imu_msg.sec_of_week = m_ts; - sub_index++; // check byte - m_imuMsg.imu_msg.gps_week_number = toValue(data, sub_index); - - pos += m_lengthImu; - /* roll pitch yaw to quaternion */ - double toQua[3]; - toQua[0] = m_imuMsg.imu_msg.roll * 6.283185307179586232 / 360.0; - toQua[1] = m_imuMsg.imu_msg.pitch * 6.283185307179586232 / 360.0; - toQua[2] = m_imuMsg.imu_msg.azimuth * 6.283185307179586232 / 360.0; - double quater[4] = {0}; - this->toQuaternion(toQua , quater); - m_rosImuMsg.orientation.x = quater[0]; - m_rosImuMsg.orientation.y = quater[1]; - m_rosImuMsg.orientation.z = quater[2]; - m_rosImuMsg.orientation.w = quater[3]; - - m_rosImuMsg.angular_velocity.x = m_imuMsg.imu_msg.x_angular_velocity; - m_rosImuMsg.angular_velocity.y = m_imuMsg.imu_msg.y_angular_velocity; - m_rosImuMsg.angular_velocity.z = m_imuMsg.imu_msg.z_angular_velocity; - m_rosImuMsg.linear_acceleration.x = m_imuMsg.imu_msg.x_acc; - m_rosImuMsg.linear_acceleration.y = m_imuMsg.imu_msg.y_acc; - m_rosImuMsg.linear_acceleration.z = m_imuMsg.imu_msg.z_acc; - if(m_msgSwitch == 1) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameImu.c_str(), 500); - } - m_imuMsg.header.frame_id = m_frameIdImu; - m_imuMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_imuMsg)); - } - else if(m_msgSwitch == 0) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameImu.c_str(), 500); - } - m_rosImuMsg.header.frame_id = m_frameIdImu; - m_rosImuMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_rosImuMsg)); - } - } - } - -private: - /* protocol info */ - std::string m_typeImu = "BDDB0B"; - int m_lengthImu = 58; - - /* topic info */ - std::string m_topicNameImu = "Ins"; - std::string m_frameIdImu = "Ins"; - /* use ros msg or asensing msg */ - int m_msgSwitch = 0; - /* node */ - std::shared_ptr m_node; - /* msg */ - sensor_msgs::msg::Imu m_rosImuMsg; - imu_msgs::msg::Imu m_imuMsg; - - /* range */ - float m_grange; - float m_arange; - double m_ts = 0.0; -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_10.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_10.cpp deleted file mode 100644 index a4eef06..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_10.cpp +++ /dev/null @@ -1,174 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" -#include "AGPbox.hpp" - -/* Asensing msg */ -#include "imu_msgs/msg/gnss.hpp" -/* ros2 msg */ -#include "sensor_msgs/msg/nav_sat_fix.hpp" - -#define PROTOCOL_BDDB10 - -class Decode_10 final : public ProtocolAsensing -{ - -public: - Decode_10(std::shared_ptr node) : m_node(node) - { - m_node->get_parameter("MsgType" , m_msgSwitch); - registProtocol(m_typeGnss , m_lengthGnss , this); - } - - ~Decode_10(){} - - void subData(const uint8_t* sub_address, int& index) - { - std::string type = ""; - char str[16] = {0}; - sprintf(str , "%02X%02X%02X" , sub_address[0] , - sub_address[1] , - sub_address[2]); - type += str; - if(type == m_typeGnss) - { - parseGnss(sub_address , index); - } - else - { - index += 3; - TinyLog::error("protocol type error:%s!" , type.c_str()); - } - } - void parseGnss(const uint8_t* data, int& pos) - { - int sub_index = 3; - uint8_t check_sum = 0; - int dataLength = getLength(m_typeGnss); - /* check xor */ - for (int i = 0; i < dataLength -1; ++i) - { - check_sum ^= data[i]; - } - - if(check_sum != data[dataLength - 1]) - { - pos += 3; - } - else - { - m_gnssMsg.longitude = (toValue(data, sub_index)) * 0.0000001; - m_gnssMsg.lon_sigma = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.latitude = (toValue(data, sub_index)) * 0.0000001; - m_gnssMsg.lat_sigma = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.altitude = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.alt_sigma = (toValue(data, sub_index)) * 0.001; - - // m_gnssMsg.gps_fix = toValue(data , sub_index); // sAcc - sub_index += 2; - - // m_gnssMsg.rtk_age = toValue(data , sub_index); //age & type - m_gnssMsg.rtk_age = data[sub_index++]; //age - sub_index ++; //msg type - - m_gnssMsg.flags_pos = data[sub_index++]; - m_gnssMsg.gps_fix = m_gnssMsg.flags_pos; - - m_gnssMsg.flags_vel = data[sub_index++]; - m_gnssMsg.flags_attitude = data[sub_index++]; - m_gnssMsg.flags_time = data[sub_index++]; - - m_gnssMsg.hor_vel = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.track_angle = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.ver_vel = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.latency_vel = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.base_length = (toValue(data, sub_index)) * 0.001; - - m_gnssMsg.yaw = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.yaw_sigma = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.pitch = (toValue(data, sub_index)) * 0.001; - m_gnssMsg.pitch_sigma = (toValue(data, sub_index)) * 0.001; - - std::string timeStr; - timeStr += std::to_string(((toValue(data, sub_index) & 0x3F) + 2000)); - timeStr += "-"; - timeStr += std::to_string(data[sub_index++]); - timeStr += "-"; - timeStr += std::to_string(data[sub_index++]); - timeStr += " "; - timeStr += std::to_string(data[sub_index++]); - timeStr += ":"; - timeStr += std::to_string(data[sub_index++]); - timeStr += ":"; - timeStr += std::to_string(toValue(data, sub_index)*0.001); - m_gnssMsg.utc_time = timeStr; - - m_gnssMsg.ts_pos = toValue(data, sub_index); - m_gnssMsg.ts_vel = toValue(data, sub_index); - m_gnssMsg.ts_heading = toValue(data, sub_index); - m_gnssMsg.state = data[sub_index++]; - m_gnssMsg.num_master = data[sub_index++]; - sub_index++; - sub_index++; - m_gnssMsg.gdop = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.pdop = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.hdop = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.htdop = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.tdop = (toValue(data, sub_index)) * 0.01; - m_gnssMsg.num_reserve = data[sub_index++]; - - pos += m_lengthGnss; - - m_rosGnssMsg.latitude = m_gnssMsg.latitude ; - m_rosGnssMsg.longitude = m_gnssMsg.longitude; - m_rosGnssMsg.altitude = m_gnssMsg.altitude; - - if(m_msgSwitch == 1) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameGnss.c_str(), 500); - } - m_gnssMsg.header.frame_id = m_frameIdGnss; - m_gnssMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_gnssMsg)); - } - else if(m_msgSwitch == 0) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameGnss.c_str(), 500); - } - m_rosGnssMsg.header.frame_id = m_frameIdGnss; - m_rosGnssMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_rosGnssMsg)); - } - } - - } - -private: - /* protocol info */ - std::string m_typeGnss = "BDDB10"; - int m_lengthGnss = 70; - - /* topic info */ - std::string m_topicNameGnss = "Gnss"; - std::string m_frameIdGnss = "Gnss"; - /* use ros msg or asensing msg */ - int m_msgSwitch = 0; - /* node */ - std::shared_ptr m_node; - /* msg */ - sensor_msgs::msg::NavSatFix m_rosGnssMsg; - imu_msgs::msg::Gnss m_gnssMsg; - - /* range */ - float m_ts = 0.0; -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_1B.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_1B.cpp deleted file mode 100644 index aaab889..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_1B.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" -#include "AGPbox.hpp" - -/* Asensing msg */ -#include "imu_msgs/msg/odom.hpp" -/* ros2 msg */ -#include "nav_msgs/msg/odometry.hpp" - -#define PROTOCOL_BDDB1B - -class Decode_1B final : public ProtocolAsensing -{ - -public: - Decode_1B(std::shared_ptr node) : m_node(node) - { - m_node->get_parameter("MsgType" , m_msgSwitch); - registProtocol(m_typeOdom , m_lengthOdom , this); - } - - ~Decode_1B(){} - - void subData(const uint8_t* sub_address, int& index) - { - std::string type = ""; - char str[16] = {0}; - sprintf(str , "%02X%02X%02X" , sub_address[0] , - sub_address[1] , - sub_address[2]); - type += str; - if(type == m_typeOdom) - { - parse1B(sub_address , index); - } - else - { - index += 3; - TinyLog::error("protocol type error:%s!" , type.c_str()); - } - } - - void parse1B(const uint8_t* data, int& pos) - { - int sub_index = 3; - uint8_t check_sum = 0; - int dataLength = getLength(m_typeOdom); - /* check xor */ - for (int i = 0; i < dataLength -1; ++i) - { - check_sum ^= data[i]; - } - - if(check_sum != data[dataLength - 1]) - { - pos += 3; - } - else - { - int16_t temp[3]; - uint8_t poll_type ; - uint8_t routing_cnt = 0; - uint32_t ts_ms = 0; - m_OdomMsg.q0_w = (toValue(data,sub_index)) * (1.0e-9); - m_OdomMsg.q1_x = (toValue(data,sub_index)) * (1.0e-9); - m_OdomMsg.q2_y = (toValue(data,sub_index)) * (1.0e-9); - m_OdomMsg.q3_z = (toValue(data,sub_index)) * (1.0e-9); - m_OdomMsg.pos_x = (toValue(data,sub_index)) * (1.0e-3); - m_OdomMsg.pos_y = (toValue(data,sub_index)) * (1.0e-3); - m_OdomMsg.pos_z = (toValue(data,sub_index)) * (1.0e-3); - m_OdomMsg.vel_x = (toValue(data,sub_index)) * 100 / 32768.0; - m_OdomMsg.vel_y = (toValue(data,sub_index)) * 100 / 32768.0; - m_OdomMsg.vel_z = (toValue(data,sub_index)) * 100 / 32768.0; - m_OdomMsg.vel = (toValue(data,sub_index)) * 100 / 32768.0; - m_OdomMsg.ang_vel_x = (toValue(data,sub_index)) * 300 / 32768.0; - m_OdomMsg.ang_vel_y = (toValue(data,sub_index)) * 300 / 32768.0; - m_OdomMsg.ang_vel_z = (toValue(data,sub_index)) * 300 / 32768.0; - m_OdomMsg.acc_x = (toValue(data,sub_index)) * 12.0 / 32768.0; - m_OdomMsg.acc_y = (toValue(data,sub_index)) * 12.0 / 32768.0; - m_OdomMsg.acc_z = (toValue(data,sub_index)) * 12.0 / 32768.0; - m_OdomMsg.status = data[sub_index++]; - m_OdomMsg.sensor_status = (toValue(data,sub_index)); - temp[0] = (toValue(data,sub_index)); - temp[1] = (toValue(data,sub_index)); - temp[2] = (toValue(data,sub_index)); - poll_type = data[sub_index++]; - routing_cnt = data[sub_index++]; - ts_ms = (toValue(data,sub_index))*0.25; - m_OdomMsg.tow_ms = ts_ms; - switch(poll_type) - { - //pos std - case 0: - m_OdomMsg.pos_x_std = (double)exp((double)temp[0]/100); - m_OdomMsg.pos_y_std = (double)exp((double)temp[1]/100); - m_OdomMsg.pos_z_std = (double)exp((double)temp[2]/100); - break; - //vel std - case 1: - m_OdomMsg.vel_x_std = (double)exp((double)temp[0]/100); - m_OdomMsg.vel_y_std = (double)exp((double)temp[1]/100); - m_OdomMsg.vel_z_std = (double)exp((double)temp[2]/100); - break; - //att std - case 2: - m_OdomMsg.roll_std = (double)exp((double)temp[0]/100); - m_OdomMsg.pitch_std = (double)exp((double)temp[1]/100); - m_OdomMsg.yaw_std = (double)exp((double)temp[2]/100); - break; - default: - break; - } - pos += m_lengthOdom; - publishOdomMsg(); - } - } - void publishOdomMsg(void) - { - if(m_msgSwitch == 1) - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameOdom.c_str(), 500); - } - m_OdomMsg.header.frame_id = m_frameIdOdom; - m_OdomMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_OdomMsg)); - } - else if(m_msgSwitch == 0) - { - m_rosPboxOdom.pose.pose.orientation.x = m_OdomMsg.q1_x; - m_rosPboxOdom.pose.pose.orientation.y = m_OdomMsg.q2_y; - m_rosPboxOdom.pose.pose.orientation.z = m_OdomMsg.q3_z; - m_rosPboxOdom.pose.pose.orientation.w = m_OdomMsg.q0_w; - m_rosPboxOdom.pose.pose.position.x = m_OdomMsg.pos_x; - m_rosPboxOdom.pose.pose.position.y = m_OdomMsg.pos_y; - m_rosPboxOdom.pose.pose.position.z = m_OdomMsg.pos_z; - m_rosPboxOdom.pose.covariance[0] = m_OdomMsg.pos_x_std; - m_rosPboxOdom.pose.covariance[7] = m_OdomMsg.pos_y_std; - m_rosPboxOdom.pose.covariance[14] = m_OdomMsg.pos_z_std; - m_rosPboxOdom.pose.covariance[21] = m_OdomMsg.roll_std; - m_rosPboxOdom.pose.covariance[28] = m_OdomMsg.pitch_std; - m_rosPboxOdom.pose.covariance[35] = m_OdomMsg.yaw_std; - - m_rosPboxOdom.twist.twist.linear.x = m_OdomMsg.vel_x; - m_rosPboxOdom.twist.twist.linear.y = m_OdomMsg.vel_y; - m_rosPboxOdom.twist.twist.linear.z = m_OdomMsg.vel_z; - - m_rosPboxOdom.twist.twist.angular.x = m_OdomMsg.ang_vel_x; - m_rosPboxOdom.twist.twist.angular.y = m_OdomMsg.ang_vel_y; - m_rosPboxOdom.twist.twist.angular.z = m_OdomMsg.ang_vel_z; - - m_rosPboxOdom.twist.covariance[0] = m_OdomMsg.vel_x_std; - m_rosPboxOdom.twist.covariance[7] = m_OdomMsg.vel_y_std; - m_rosPboxOdom.twist.covariance[14] = m_OdomMsg.vel_z_std; - - m_rosPboxOdom.header.frame_id = m_frameIdOdom; - m_rosPboxOdom.header.stamp = rclcpp::Clock().now(); - { - static bool isCreate = false; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicNameOdom.c_str(), 500); - } - m_OdomMsg.header.frame_id = m_frameIdOdom; - m_OdomMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(m_rosPboxOdom)); - } - } - - } -private: - /* protocol info */ - std::string m_typeOdom = "BDDB1B"; - int m_lengthOdom = 69; - - /* topic info */ - std::string m_topicNameOdom = "Odom"; - std::string m_frameIdOdom = "Odom"; - - /* use ros msg or asensing msg */ - int m_msgSwitch = 0; - - - /* node */ - std::shared_ptr m_node; - /* msg */ - nav_msgs::msg::Odometry m_rosPboxOdom; - imu_msgs::msg::Odom m_OdomMsg; - -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_8B.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_8B.cpp deleted file mode 100644 index 15ac80d..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_8B.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// #include "AG_Pbox/protocol_asensing.h" -// #include "AG_Pbox/TinyLog.h" -// #include "rclcpp/rclcpp.hpp" -#include "AGPbox.hpp" - -/* Asensing msg */ -#include "imu_msgs/msg/imu8_b.hpp" -/* ros2 msg */ -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" - -#define PROTOCOL_BDDB8B - -class Decode_8B final : public ProtocolAsensing -{ - -public: - Decode_8B(std::shared_ptr node) : m_node(node) - { - m_node->get_parameter("MsgType" , m_msgSwitch); - registProtocol(m_type , m_length , this); - } - - ~Decode_8B(){} - - void subData(const uint8_t* sub_address, int& index) - { - - int sub_index = 3; - int dataLength = getLength(m_type); - /* check crc16 */ - uint16_t crc16Result = checkCrc16(sub_address , dataLength - 2); - uint16_t getCrc16 = 0; - memcpy(&getCrc16 , &sub_address[dataLength - 2] , 2); - if(getCrc16 == crc16Result) - { - int16_t middle; - sub_index = 3; - - if(m_msgSwitch == 1) - { - pubMsg.type = toValue(sub_address, sub_index); - pubMsg.data_length = toValue(sub_address, sub_index); - pubMsg.frame_count = toValue(sub_address, sub_index); - pubMsg.serial_number = toValue(sub_address, sub_index); - - // gyro - pubMsg.gx = toValue(sub_address, sub_index); - pubMsg.gy = toValue(sub_address, sub_index); - pubMsg.gz = toValue(sub_address, sub_index); - - // acc - pubMsg.ax = toValue(sub_address, sub_index); - pubMsg.ay = toValue(sub_address, sub_index); - pubMsg.az = toValue(sub_address, sub_index); - - // temp - middle = toValue(sub_address, sub_index); - pubMsg.temperature = middle * 200.0 / 32768; - - sub_index++; - pubMsg.status = toValue(sub_address, sub_index); - - { - /* Asensing msg type */ - static bool isCreate = false; - - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicName.c_str(), 500); - } - /* msg header */ - pubMsg.header.frame_id = m_frameId; - pubMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(pubMsg)); - } - - } - else if(m_msgSwitch == 0) - { - /* ros2 msg type */ - static bool isCreate = false; - static sensor_msgs::msg::Imu pubMsg; - static rclcpp::Publisher::SharedPtr publisher; - if(!isCreate) - { - isCreate = true; - publisher = m_node->create_publisher(m_topicName.c_str(), 500); - } - /* frame BDDB8B not have roll pitch yaw */ - // tf2::Quaternion curr_quater; - // curr_quater.setRPY(pLoosely->roll, pLoosely->pitch, pLoosely->yaw); - // msg.orientation.x = curr_quater.x(); - // msg.orientation.y = curr_quater.y(); - // msg.orientation.z = curr_quater.z(); - // msg.orientation.w = curr_quater.w(); - sub_index += 8; - pubMsg.angular_velocity.x = toValue(sub_address, sub_index); - pubMsg.angular_velocity.y = toValue(sub_address, sub_index); - pubMsg.angular_velocity.z = toValue(sub_address, sub_index); - - pubMsg.linear_acceleration.x = toValue(sub_address, sub_index); - pubMsg.linear_acceleration.y = toValue(sub_address, sub_index); - pubMsg.linear_acceleration.z = toValue(sub_address, sub_index); - - /* msg header */ - pubMsg.header.frame_id = m_frameId; - pubMsg.header.stamp = rclcpp::Clock().now(); - publisher->publish(std::move(pubMsg)); - } - else - { - static bool isPrint = false; - if(!isPrint) - { - TinyLog::error("unknown msg type , please check your launch file!"); - isPrint = true; - } - } - index += dataLength; - } - else - { - index += 4; - } - } - - uint16_t checkCrc16(const uint8_t* data , const uint32_t len) - { - uint16_t wCRCin = 0xFFFFu; - uint16_t wCPoly = 0x1021u; - uint8_t wChar = 0u; - uint32_t i = 0u; - uint32_t j = 0u; - - for( i=0u; i < len; i++) - { - wChar = data[i]; - wCRCin ^= (wChar << 8); - - for( j = 0u; j <8; j++) - { - if(wCRCin & (uint16_t)0x8000u) - { - wCRCin = (uint16_t)((uint16_t)(wCRCin << 1) ^ wCPoly); - }else - { - wCRCin = (uint16_t)(wCRCin << 1); - } - } - } - return (wCRCin); - } - -private: - /* protocol info */ - std::string m_type = "AB548B"; - int m_length = 41; - - /* node */ - std::shared_ptr m_node; - - imu_msgs::msg::Imu8B pubMsg; - - /* topic info */ - std::string m_topicName = "Imu8B"; - std::string m_frameId = "Imu_8B"; - - /* use ros msg or asensing msg */ - int m_msgSwitch = 0; -}; diff --git a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol_asensing.cpp b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol_asensing.cpp deleted file mode 100644 index 5fc7d58..0000000 --- a/src/perception/pbox_node_dirve/src/pbox_node/src/protocol_asensing.cpp +++ /dev/null @@ -1,179 +0,0 @@ -#include "protocol_asensing.h" -#include -#include -#include -#include "TinyLog.h" -using namespace std; - -enum ERROR_CODE -{ - NO_ERR, - POLL_TIMEOUT, //Polling toggle bit failed - VERIFY_WRITE, // Verifying write to flash failed - INVALID_SECTOR, //Invalid Sector - INVALID_BLOCK, //Invalid Block - UNKNOWN_COMMAND, //Unknown Command - PROCESS_COMMAND_ERR, // Processing command - NOT_READ_ERROR, //Could not read memory from target - DRV_NOTAT_BREAK, //The drive was not at AFP_BreakReady - BUFFER_IS_NULL, //Could not allocate storage for the buffer - NO_ACCESS_SECTOR, //Cannot access the sector( could be locked or something is stored there that should not be touched ) - NUM_ERROR_CODES, - UART_RX_FULL, //Lost one byte when the RX circular buffer is full. - UART_TX_FULL, //When TX circular buffer is full, push one byte will be lost - UART_RX_BUF_EMPTY, //Try to pop one byte from empty RX circular buffer - OUT_OF_FLASH_NUM, //Out of flash number (0-2) - FLASH_GET_CODE_FAIL, //Check codes of flash devices failed - FLASH_COMPARE_ERROR, //Compare read/write flash error - UNDEFINED_URX_STAT, //Undefined uart rx buf status - UNDEFINED_INS, //Undefined instruction - FAIL_ON_MALLOC, //Failed on malloc() - CHECK_SUM_ERR, //Check sum error, VR101 protocol - TOO_MUCH_DATA, //Too much data to print in a data rate period - OUT_OF_16BIT_RANGE, //Out of 16bit range - MATRIX_NO_INVERSE, //Matrix no inverse - WRITE_BACK_FLASH, //Try to write to a Flash address that have been written - SDRAM_TEST_ERR, //SDRAM self test error - COMP_SELFCHECK_FAIL, //Compass self check fail - UNFINISHED_HMC5883SEQ, //Unfinished HMC5883 continous read sequence while starting a new one - SENSOR_SATURATION, //Sensor saturation - GYRO_BIT_ERR, //Gyro Built-in-test error - CALIB_MAG2D_LARGE_INCLIN, //too large pitch or roll angle during 2D magnetometer calib - ACCEL_INITIAL_CHECK_ERR, //Accel initial check error - GYRO_INITIAL_CHECK_ERR //gyroscope initial check error -}; - -std::map ProtocolAsensing::protocolLengthMap{}; -std::map ProtocolAsensing::protocolMap{}; - -ProtocolAsensing::ProtocolAsensing() -{ - -} - -ProtocolAsensing::~ProtocolAsensing() -{ - -} - -void ProtocolAsensing::addData(const std::string& data) -{ - analysisData(data); -} - -void ProtocolAsensing::clearCache() -{ - _receive_data.erase(_receive_data.begin(), _receive_data.end()); -} - -bool ProtocolAsensing::analysisData(const std::string& data) -{ - _receive_data += data; - //解析自定义协议数据 - int start_index = 0;//当前读取到的字节下标 - const uint8_t* data_address = (uint8_t*)_receive_data.c_str(); - bool is_exit_while = false; - - while (start_index < (int)_receive_data.size() - 11 && !is_exit_while) - { - /* get message head */ - std::string packet_type = ""; - char str[16] = {0}; - sprintf(str , "%02X%02X%02X" , data_address[start_index] , - data_address[start_index + 1] , - data_address[start_index + 2]); - packet_type += str; - - /* find message head */ - bool isRight = false; - for(auto it = protocolMap.begin();it != protocolMap.end();++it) - { - if(packet_type == it->first) - { - isRight = true; - break; - } - else - { - continue; - } - } - - if(isRight) - { - /* get message length */ - const uint8_t* sub_address = data_address + start_index; - if (protocolLengthMap.find(packet_type) == protocolLengthMap.end()) - { - if (start_index < _receive_data.size() - 2) - { - start_index += 2; - } - else - { - is_exit_while = true; - } - } - else - { - /* parse massage */ - if (_receive_data.size() - start_index - 1 > protocolLengthMap[packet_type]) - { - protocolMap[packet_type]->subData(sub_address, start_index); - } - else - { - is_exit_while = true; - } - } - } - else - { - if(start_index < _receive_data.size() - 1) - { - start_index++; - } - else - { - is_exit_while = true; - } - } - }//end while - - if (start_index > 0 && _receive_data.size() > start_index) - { - //去掉已遍历过的数据 - _receive_data.erase(_receive_data.begin(), _receive_data.begin() + start_index); - } - else if (start_index > 0 && _receive_data.size() == start_index) - { - _receive_data.clear(); - } - - return true; -} - -bool ProtocolAsensing::registProtocol(const std::string &protocolFlag , int length , ProtocolAsensing* sub) -{ - protocolLengthMap.insert(std::pair(protocolFlag, length)); - protocolMap.insert(std::pair(protocolFlag, sub)); - //TinyLog::info("regist protocol:%s length: %d\n" , protocolFlag.c_str() , length); - return true; -} -void ProtocolAsensing::toQuaternion(double* rpy , double* quaterArray) -{ - double halfYaw = rpy[2] * double(0.5); - double halfPitch = rpy[1] * double(0.5); - double halfRoll = rpy[0] * double(0.5); - double cosYaw = std::cos(halfYaw); - double sinYaw = std::sin(halfYaw); - double cosPitch = std::cos(halfPitch); - double sinPitch = std::sin(halfPitch); - double cosRoll = std::cos(halfRoll); - double sinRoll = std::sin(halfRoll); - - quaterArray[0] = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; - quaterArray[1] = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; - quaterArray[2] = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; - quaterArray[3] = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; -} \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/serial/CMakeLists.txt b/src/perception/pbox_node_dirve/src/serial/CMakeLists.txt deleted file mode 100644 index aa10d4d..0000000 --- a/src/perception/pbox_node_dirve/src/serial/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(serial) - -find_package(ament_cmake REQUIRED) - -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) - -## Sources -## Add serial library -add_library(${PROJECT_NAME} SHARED - src/serial.cc - include/serial/serial.h - include/serial/v8stdint.h -) - -if(APPLE) # macOS - find_library(IOKIT_LIBRARY IOKit) - find_library(FOUNDATION_LIBRARY Foundation) - target_sources(${PROJECT_NAME} PRIVATE - src/impl/unix.cc - src/impl/list_ports/list_ports_osx.cc - ) - target_link_libraries(${PROJECT_NAME} ${FOUNDATION_LIBRARY} ${IOKIT_LIBRARY}) -elseif(UNIX) # .*nix - target_sources(${PROJECT_NAME} PRIVATE - src/impl/unix.cc - src/impl/list_ports/list_ports_linux.cc - ) - target_link_libraries(${PROJECT_NAME} rt pthread) -elseif(WIN32) # Windows - target_sources(${PROJECT_NAME} PRIVATE - src/impl/win.cc - src/impl/list_ports/list_ports_win.cc - ) - target_link_libraries(${PROJECT_NAME} setupapi) - ament_export_libraries(setupapi) -endif() - - -## Include headers -target_include_directories(${PROJECT_NAME} PRIVATE include) - -## Uncomment for example -# add_executable(serial_example examples/serial_example.cc) -# add_dependencies(serial_example ${PROJECT_NAME}) -# target_link_libraries(serial_example ${PROJECT_NAME}) - -## Install executable -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -## Install headers -install(FILES include/serial/serial.h include/serial/v8stdint.h - DESTINATION include/serial -) - -## Tests -#if(BUILD_TESTING) -# add_subdirectory(tests) -#endif() - -ament_package() diff --git a/src/perception/pbox_node_dirve/src/serial/Makefile b/src/perception/pbox_node_dirve/src/serial/Makefile deleted file mode 100644 index e172072..0000000 --- a/src/perception/pbox_node_dirve/src/serial/Makefile +++ /dev/null @@ -1,62 +0,0 @@ -all: serial - -CMAKE_FLAGS := -DCMAKE_INSTALL_PREFIX=/tmp/usr/local -UNAME := $(shell uname -s) - -install_deps: -ifeq ($(UNAME),Darwin) - brew tap ros/deps - brew update - brew outdated boost || brew upgrade boost || brew install boost - brew outdated python || brew upgrade python || brew install python - sudo -H python2 -m pip install -U pip setuptools - sudo -H python2 -m pip install --force-reinstall --no-deps -U pip - sudo -H python2 -m pip install rosinstall_generator wstool rosdep empy catkin_pkg - sudo -H rosdep init - rosdep update - mkdir catkin_ws - cd catkin_ws && rosinstall_generator catkin --rosdistro hydro --tar > catkin.rosinstall - cd catkin_ws && wstool init src catkin.rosinstall - cd catkin_ws && rosdep install --from-paths src --ignore-src -y - cd catkin_ws && python2 ./src/catkin/bin/catkin_make -DPYTHON_EXECUTABLE=`which python2` install - echo "source catkin_ws/install/setup.bash" > setup.bash -else - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update - sudo apt-get install ros-hydro-catkin libboost-dev - echo "source /opt/ros/hydro/setup.bash" > setup.bash -endif - -install: - cd build && make install - -serial: - @mkdir -p build - cd build && cmake $(CMAKE_FLAGS) .. -ifneq ($(MAKE),) - cd build && $(MAKE) -else - cd build && make -endif - -.PHONY: clean -clean: - rm -rf build - -.PHONY: doc -doc: - @doxygen doc/Doxyfile -ifeq ($(UNAME),Darwin) - @open doc/html/index.html -endif - -.PHONY: test -test: - @mkdir -p build - cd build && cmake $(CMAKE_FLAGS) .. -ifneq ($(MAKE),) - cd build && $(MAKE) run_tests -else - cd build && make run_tests -endif diff --git a/src/perception/pbox_node_dirve/src/serial/README.md b/src/perception/pbox_node_dirve/src/serial/README.md deleted file mode 100644 index eb4b279..0000000 --- a/src/perception/pbox_node_dirve/src/serial/README.md +++ /dev/null @@ -1,58 +0,0 @@ -Adapt to ROS2 foxy. - - - -#### Install - -Get the code: - - # open a new terminal - cd serial - mkdir build - -Build: - - cd build - cmake .. - make - -Install: - - sudo make install - -**like this**: - - - - - -也可以不用 make install ,直接和你想要调用本API的ROS2 节点放在同一个 src 下编译即可。有关于本串口操作包的 API 在线文档如下： - - - -[serial: Serial Library](http://wjwwood.io/serial/doc/1.1.0/index.html) - - - -本共 ROS Package 修改来自： - -### License - -The MIT License - -Copyright (c) 2012 William Woodall, John Harrison - -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. - -### Authors - -William Woodall -John Harrison - -### Contact - -William Woodall diff --git a/src/perception/pbox_node_dirve/src/serial/README.pdf b/src/perception/pbox_node_dirve/src/serial/README.pdf deleted file mode 100644 index c884b65..0000000 Binary files a/src/perception/pbox_node_dirve/src/serial/README.pdf and /dev/null differ diff --git a/src/perception/pbox_node_dirve/src/serial/images/install_Serial_ubuntu.png b/src/perception/pbox_node_dirve/src/serial/images/install_Serial_ubuntu.png deleted file mode 100644 index 90c9e02..0000000 Binary files a/src/perception/pbox_node_dirve/src/serial/images/install_Serial_ubuntu.png and /dev/null differ diff --git a/src/perception/pbox_node_dirve/src/serial/include/serial/impl/unix.h b/src/perception/pbox_node_dirve/src/serial/include/serial/impl/unix.h deleted file mode 100644 index 0fb38f2..0000000 --- a/src/perception/pbox_node_dirve/src/serial/include/serial/impl/unix.h +++ /dev/null @@ -1,221 +0,0 @@ -/*! - * \file serial/impl/unix.h - * \author William Woodall - * \author John Harrison - * \version 0.1 - * - * \section LICENSE - * - * The MIT License - * - * Copyright (c) 2012 William Woodall, John Harrison - * - * 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. - * - * \section DESCRIPTION - * - * This provides a unix based pimpl for the Serial class. This implementation is - * based off termios.h and uses select for multiplexing the IO ports. - * - */ - -#if !defined(_WIN32) - -#ifndef SERIAL_IMPL_UNIX_H -#define SERIAL_IMPL_UNIX_H - -#include "serial/serial.h" - -#include - -namespace serial { - -using std::size_t; -using std::string; -using std::invalid_argument; - -using serial::SerialException; -using serial::IOException; - -class MillisecondTimer { -public: - MillisecondTimer(const uint32_t millis); - int64_t remaining(); - -private: - static timespec timespec_now(); - timespec expiry; -}; - -class serial::Serial::SerialImpl { -public: - SerialImpl (const string &port, - unsigned long baudrate, - bytesize_t bytesize, - parity_t parity, - stopbits_t stopbits, - flowcontrol_t flowcontrol); - - virtual ~SerialImpl (); - - void - open (); - - void - close (); - - bool - isOpen () const; - - size_t - available (); - - bool - waitReadable (uint32_t timeout); - - void - waitByteTimes (size_t count); - - size_t - read (uint8_t *buf, size_t size = 1); - - size_t - write (const uint8_t *data, size_t length); - - void - flush (); - - void - flushInput (); - - void - flushOutput (); - - void - sendBreak (int duration); - - void - setBreak (bool level); - - void - setRTS (bool level); - - void - setDTR (bool level); - - bool - waitForChange (); - - bool - getCTS (); - - bool - getDSR (); - - bool - getRI (); - - bool - getCD (); - - void - setPort (const string &port); - - string - getPort () const; - - void - setTimeout (Timeout &timeout); - - Timeout - getTimeout () const; - - void - setBaudrate (unsigned long baudrate); - - unsigned long - getBaudrate () const; - - void - setBytesize (bytesize_t bytesize); - - bytesize_t - getBytesize () const; - - void - setParity (parity_t parity); - - parity_t - getParity () const; - - void - setStopbits (stopbits_t stopbits); - - stopbits_t - getStopbits () const; - - void - setFlowcontrol (flowcontrol_t flowcontrol); - - flowcontrol_t - getFlowcontrol () const; - - void - readLock (); - - void - readUnlock (); - - void - writeLock (); - - void - writeUnlock (); - -protected: - void reconfigurePort (); - -private: - string port_; // Path to the file descriptor - int fd_; // The current file descriptor - - bool is_open_; - bool xonxoff_; - bool rtscts_; - - Timeout timeout_; // Timeout for read operations - unsigned long baudrate_; // Baudrate - uint32_t byte_time_ns_; // Nanoseconds to transmit/receive a single byte - - parity_t parity_; // Parity - bytesize_t bytesize_; // Size of the bytes - stopbits_t stopbits_; // Stop Bits - flowcontrol_t flowcontrol_; // Flow Control - - // Mutex used to lock the read functions - pthread_mutex_t read_mutex; - // Mutex used to lock the write functions - pthread_mutex_t write_mutex; -}; - -} - -#endif // SERIAL_IMPL_UNIX_H - -#endif // !defined(_WIN32) diff --git a/src/perception/pbox_node_dirve/src/serial/include/serial/impl/win.h b/src/perception/pbox_node_dirve/src/serial/include/serial/impl/win.h deleted file mode 100644 index 2c0c6cd..0000000 --- a/src/perception/pbox_node_dirve/src/serial/include/serial/impl/win.h +++ /dev/null @@ -1,207 +0,0 @@ -/*! - * \file serial/impl/windows.h - * \author William Woodall - * \author John Harrison - * \version 0.1 - * - * \section LICENSE - * - * The MIT License - * - * Copyright (c) 2012 William Woodall, John Harrison - * - * 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. - * - * \section DESCRIPTION - * - * This provides a windows implementation of the Serial class interface. - * - */ - -#if defined(_WIN32) - -#ifndef SERIAL_IMPL_WINDOWS_H -#define SERIAL_IMPL_WINDOWS_H - -#include "serial/serial.h" - -#include "windows.h" - -namespace serial { - -using std::string; -using std::wstring; -using std::invalid_argument; - -using serial::SerialException; -using serial::IOException; - -class serial::Serial::SerialImpl { -public: - SerialImpl (const string &port, - unsigned long baudrate, - bytesize_t bytesize, - parity_t parity, - stopbits_t stopbits, - flowcontrol_t flowcontrol); - - virtual ~SerialImpl (); - - void - open (); - - void - close (); - - bool - isOpen () const; - - size_t - available (); - - bool - waitReadable (uint32_t timeout); - - void - waitByteTimes (size_t count); - - size_t - read (uint8_t *buf, size_t size = 1); - - size_t - write (const uint8_t *data, size_t length); - - void - flush (); - - void - flushInput (); - - void - flushOutput (); - - void - sendBreak (int duration); - - void - setBreak (bool level); - - void - setRTS (bool level); - - void - setDTR (bool level); - - bool - waitForChange (); - - bool - getCTS (); - - bool - getDSR (); - - bool - getRI (); - - bool - getCD (); - - void - setPort (const string &port); - - string - getPort () const; - - void - setTimeout (Timeout &timeout); - - Timeout - getTimeout () const; - - void - setBaudrate (unsigned long baudrate); - - unsigned long - getBaudrate () const; - - void - setBytesize (bytesize_t bytesize); - - bytesize_t - getBytesize () const; - - void - setParity (parity_t parity); - - parity_t - getParity () const; - - void - setStopbits (stopbits_t stopbits); - - stopbits_t - getStopbits () const; - - void - setFlowcontrol (flowcontrol_t flowcontrol); - - flowcontrol_t - getFlowcontrol () const; - - void - readLock (); - - void - readUnlock (); - - void - writeLock (); - - void - writeUnlock (); - -protected: - void reconfigurePort (); - -private: - wstring port_; // Path to the file descriptor - HANDLE fd_; - - bool is_open_; - - Timeout timeout_; // Timeout for read operations - unsigned long baudrate_; // Baudrate - - parity_t parity_; // Parity - bytesize_t bytesize_; // Size of the bytes - stopbits_t stopbits_; // Stop Bits - flowcontrol_t flowcontrol_; // Flow Control - - // Mutex used to lock the read functions - HANDLE read_mutex; - // Mutex used to lock the write functions - HANDLE write_mutex; -}; - -} - -#endif // SERIAL_IMPL_WINDOWS_H - -#endif // if defined(_WIN32) diff --git a/src/perception/pbox_node_dirve/src/serial/include/serial/serial.h b/src/perception/pbox_node_dirve/src/serial/include/serial/serial.h deleted file mode 100644 index a165785..0000000 --- a/src/perception/pbox_node_dirve/src/serial/include/serial/serial.h +++ /dev/null @@ -1,775 +0,0 @@ -/*! - * \file serial/serial.h - * \author William Woodall - * \author John Harrison - * \version 0.1 - * - * \section LICENSE - * - * The MIT License - * - * Copyright (c) 2012 William Woodall - * - * 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. - * - * \section DESCRIPTION - * - * This provides a cross platform interface for interacting with Serial Ports. - */ - -#ifndef SERIAL_H -#define SERIAL_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \ -__LINE__, (message) ) - -namespace serial { - -/*! - * Enumeration defines the possible bytesizes for the serial port. - */ -typedef enum { - fivebits = 5, - sixbits = 6, - sevenbits = 7, - eightbits = 8 -} bytesize_t; - -/*! - * Enumeration defines the possible parity types for the serial port. - */ -typedef enum { - parity_none = 0, - parity_odd = 1, - parity_even = 2, - parity_mark = 3, - parity_space = 4 -} parity_t; - -/*! - * Enumeration defines the possible stopbit types for the serial port. - */ -typedef enum { - stopbits_one = 1, - stopbits_two = 2, - stopbits_one_point_five -} stopbits_t; - -/*! - * Enumeration defines the possible flowcontrol types for the serial port. - */ -typedef enum { - flowcontrol_none = 0, - flowcontrol_software, - flowcontrol_hardware -} flowcontrol_t; - -/*! - * Structure for setting the timeout of the serial port, times are - * in milliseconds. - * - * In order to disable the interbyte timeout, set it to Timeout::max(). - */ -struct Timeout { -#ifdef max -# undef max -#endif - static uint32_t max() {return std::numeric_limits::max();} - /*! - * Convenience function to generate Timeout structs using a - * single absolute timeout. - * - * \param timeout A long that defines the time in milliseconds until a - * timeout occurs after a call to read or write is made. - * - * \return Timeout struct that represents this simple timeout provided. - */ - static Timeout simpleTimeout(uint32_t timeout) { - return Timeout(max(), timeout, 0, timeout, 0); - } - - /*! Number of milliseconds between bytes received to timeout on. */ - uint32_t inter_byte_timeout; - /*! A constant number of milliseconds to wait after calling read. */ - uint32_t read_timeout_constant; - /*! A multiplier against the number of requested bytes to wait after - * calling read. - */ - uint32_t read_timeout_multiplier; - /*! A constant number of milliseconds to wait after calling write. */ - uint32_t write_timeout_constant; - /*! A multiplier against the number of requested bytes to wait after - * calling write. - */ - uint32_t write_timeout_multiplier; - - explicit Timeout (uint32_t inter_byte_timeout_=0, - uint32_t read_timeout_constant_=0, - uint32_t read_timeout_multiplier_=0, - uint32_t write_timeout_constant_=0, - uint32_t write_timeout_multiplier_=0) - : inter_byte_timeout(inter_byte_timeout_), - read_timeout_constant(read_timeout_constant_), - read_timeout_multiplier(read_timeout_multiplier_), - write_timeout_constant(write_timeout_constant_), - write_timeout_multiplier(write_timeout_multiplier_) - {} -}; - -/*! - * Class that provides a portable serial port interface. - */ -class Serial { -public: - /*! - * Creates a Serial object and opens the port if a port is specified, - * otherwise it remains closed until serial::Serial::open is called. - * - * \param port A std::string containing the address of the serial port, - * which would be something like 'COM1' on Windows and '/dev/ttyS0' - * on Linux. - * - * \param baudrate An unsigned 32-bit integer that represents the baudrate - * - * \param timeout A serial::Timeout struct that defines the timeout - * conditions for the serial port. \see serial::Timeout - * - * \param bytesize Size of each byte in the serial transmission of data, - * default is eightbits, possible values are: fivebits, sixbits, sevenbits, - * eightbits - * - * \param parity Method of parity, default is parity_none, possible values - * are: parity_none, parity_odd, parity_even - * - * \param stopbits Number of stop bits used, default is stopbits_one, - * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two - * - * \param flowcontrol Type of flowcontrol used, default is - * flowcontrol_none, possible values are: flowcontrol_none, - * flowcontrol_software, flowcontrol_hardware - * - * \throw serial::PortNotOpenedException - * \throw serial::IOException - * \throw std::invalid_argument - */ - Serial (const std::string &port = "", - uint32_t baudrate = 9600, - Timeout timeout = Timeout(), - bytesize_t bytesize = eightbits, - parity_t parity = parity_none, - stopbits_t stopbits = stopbits_one, - flowcontrol_t flowcontrol = flowcontrol_none); - - /*! Destructor */ - virtual ~Serial (); - - /*! - * Opens the serial port as long as the port is set and the port isn't - * already open. - * - * If the port is provided to the constructor then an explicit call to open - * is not needed. - * - * \see Serial::Serial - * - * \throw std::invalid_argument - * \throw serial::SerialException - * \throw serial::IOException - */ - void - open (); - - /*! Gets the open status of the serial port. - * - * \return Returns true if the port is open, false otherwise. - */ - bool - isOpen () const; - - /*! Closes the serial port. */ - void - close (); - - /*! Return the number of characters in the buffer. */ - size_t - available (); - - /*! Block until there is serial data to read or read_timeout_constant - * number of milliseconds have elapsed. The return value is true when - * the function exits with the port in a readable state, false otherwise - * (due to timeout or select interruption). */ - bool - waitReadable (); - - /*! Block for a period of time corresponding to the transmission time of - * count characters at present serial settings. This may be used in con- - * junction with waitReadable to read larger blocks of data from the - * port. */ - void - waitByteTimes (size_t count); - - /*! Read a given amount of bytes from the serial port into a given buffer. - * - * The read function will return in one of three cases: - * * The number of requested bytes was read. - * * In this case the number of bytes requested will match the size_t - * returned by read. - * * A timeout occurred, in this case the number of bytes read will not - * match the amount requested, but no exception will be thrown. One of - * two possible timeouts occurred: - * * The inter byte timeout expired, this means that number of - * milliseconds elapsed between receiving bytes from the serial port - * exceeded the inter byte timeout. - * * The total timeout expired, which is calculated by multiplying the - * read timeout multiplier by the number of requested bytes and then - * added to the read timeout constant. If that total number of - * milliseconds elapses after the initial call to read a timeout will - * occur. - * * An exception occurred, in this case an actual exception will be thrown. - * - * \param buffer An uint8_t array of at least the requested size. - * \param size A size_t defining how many bytes to be read. - * - * \return A size_t representing the number of bytes read as a result of the - * call to read. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - size_t - read (uint8_t *buffer, size_t size); - - /*! Read a given amount of bytes from the serial port into a give buffer. - * - * \param buffer A reference to a std::vector of uint8_t. - * \param size A size_t defining how many bytes to be read. - * - * \return A size_t representing the number of bytes read as a result of the - * call to read. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - size_t - read (std::vector &buffer, size_t size = 1); - - /*! Read a given amount of bytes from the serial port into a give buffer. - * - * \param buffer A reference to a std::string. - * \param size A size_t defining how many bytes to be read. - * - * \return A size_t representing the number of bytes read as a result of the - * call to read. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - size_t - read (std::string &buffer, size_t size = 1); - - /*! Read a given amount of bytes from the serial port and return a string - * containing the data. - * - * \param size A size_t defining how many bytes to be read. - * - * \return A std::string containing the data read from the port. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - std::string - read (size_t size = 1); - - /*! Reads in a line or until a given delimiter has been processed. - * - * Reads from the serial port until a single line has been read. - * - * \param buffer A std::string reference used to store the data. - * \param size A maximum length of a line, defaults to 65536 (2^16) - * \param eol A string to match against for the EOL. - * - * \return A size_t representing the number of bytes read. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - size_t - readline (std::string &buffer, size_t size = 65536, std::string eol = "\n"); - - /*! Reads in a line or until a given delimiter has been processed. - * - * Reads from the serial port until a single line has been read. - * - * \param size A maximum length of a line, defaults to 65536 (2^16) - * \param eol A string to match against for the EOL. - * - * \return A std::string containing the line. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - std::string - readline (size_t size = 65536, std::string eol = "\n"); - - /*! Reads in multiple lines until the serial port times out. - * - * This requires a timeout > 0 before it can be run. It will read until a - * timeout occurs and return a list of strings. - * - * \param size A maximum length of combined lines, defaults to 65536 (2^16) - * - * \param eol A string to match against for the EOL. - * - * \return A vector containing the lines. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - */ - std::vector - readlines (size_t size = 65536, std::string eol = "\n"); - - /*! Write a string to the serial port. - * - * \param data A const reference containing the data to be written - * to the serial port. - * - * \param size A size_t that indicates how many bytes should be written from - * the given data buffer. - * - * \return A size_t representing the number of bytes actually written to - * the serial port. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - * \throw serial::IOException - */ - size_t - write (const uint8_t *data, size_t size); - - /*! Write a string to the serial port. - * - * \param data A const reference containing the data to be written - * to the serial port. - * - * \return A size_t representing the number of bytes actually written to - * the serial port. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - * \throw serial::IOException - */ - size_t - write (const std::vector &data); - - /*! Write a string to the serial port. - * - * \param data A const reference containing the data to be written - * to the serial port. - * - * \return A size_t representing the number of bytes actually written to - * the serial port. - * - * \throw serial::PortNotOpenedException - * \throw serial::SerialException - * \throw serial::IOException - */ - size_t - write (const std::string &data); - - /*! Sets the serial port identifier. - * - * \param port A const std::string reference containing the address of the - * serial port, which would be something like 'COM1' on Windows and - * '/dev/ttyS0' on Linux. - * - * \throw std::invalid_argument - */ - void - setPort (const std::string &port); - - /*! Gets the serial port identifier. - * - * \see Serial::setPort - * - * \throw std::invalid_argument - */ - std::string - getPort () const; - - /*! Sets the timeout for reads and writes using the Timeout struct. - * - * There are two timeout conditions described here: - * * The inter byte timeout: - * * The inter_byte_timeout component of serial::Timeout defines the - * maximum amount of time, in milliseconds, between receiving bytes on - * the serial port that can pass before a timeout occurs. Setting this - * to zero will prevent inter byte timeouts from occurring. - * * Total time timeout: - * * The constant and multiplier component of this timeout condition, - * for both read and write, are defined in serial::Timeout. This - * timeout occurs if the total time since the read or write call was - * made exceeds the specified time in milliseconds. - * * The limit is defined by multiplying the multiplier component by the - * number of requested bytes and adding that product to the constant - * component. In this way if you want a read call, for example, to - * timeout after exactly one second regardless of the number of bytes - * you asked for then set the read_timeout_constant component of - * serial::Timeout to 1000 and the read_timeout_multiplier to zero. - * This timeout condition can be used in conjunction with the inter - * byte timeout condition with out any problems, timeout will simply - * occur when one of the two timeout conditions is met. This allows - * users to have maximum control over the trade-off between - * responsiveness and efficiency. - * - * Read and write functions will return in one of three cases. When the - * reading or writing is complete, when a timeout occurs, or when an - * exception occurs. - * - * A timeout of 0 enables non-blocking mode. - * - * \param timeout A serial::Timeout struct containing the inter byte - * timeout, and the read and write timeout constants and multipliers. - * - * \see serial::Timeout - */ - void - setTimeout (Timeout &timeout); - - /*! Sets the timeout for reads and writes. */ - void - setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant, - uint32_t read_timeout_multiplier, uint32_t write_timeout_constant, - uint32_t write_timeout_multiplier) - { - Timeout timeout(inter_byte_timeout, read_timeout_constant, - read_timeout_multiplier, write_timeout_constant, - write_timeout_multiplier); - return setTimeout(timeout); - } - - /*! Gets the timeout for reads in seconds. - * - * \return A Timeout struct containing the inter_byte_timeout, and read - * and write timeout constants and multipliers. - * - * \see Serial::setTimeout - */ - Timeout - getTimeout () const; - - /*! Sets the baudrate for the serial port. - * - * Possible baudrates depends on the system but some safe baudrates include: - * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000, - * 57600, 115200 - * Some other baudrates that are supported by some comports: - * 128000, 153600, 230400, 256000, 460800, 500000, 921600 - * - * \param baudrate An integer that sets the baud rate for the serial port. - * - * \throw std::invalid_argument - */ - void - setBaudrate (uint32_t baudrate); - - /*! Gets the baudrate for the serial port. - * - * \return An integer that sets the baud rate for the serial port. - * - * \see Serial::setBaudrate - * - * \throw std::invalid_argument - */ - uint32_t - getBaudrate () const; - - /*! Sets the bytesize for the serial port. - * - * \param bytesize Size of each byte in the serial transmission of data, - * default is eightbits, possible values are: fivebits, sixbits, sevenbits, - * eightbits - * - * \throw std::invalid_argument - */ - void - setBytesize (bytesize_t bytesize); - - /*! Gets the bytesize for the serial port. - * - * \see Serial::setBytesize - * - * \throw std::invalid_argument - */ - bytesize_t - getBytesize () const; - - /*! Sets the parity for the serial port. - * - * \param parity Method of parity, default is parity_none, possible values - * are: parity_none, parity_odd, parity_even - * - * \throw std::invalid_argument - */ - void - setParity (parity_t parity); - - /*! Gets the parity for the serial port. - * - * \see Serial::setParity - * - * \throw std::invalid_argument - */ - parity_t - getParity () const; - - /*! Sets the stopbits for the serial port. - * - * \param stopbits Number of stop bits used, default is stopbits_one, - * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two - * - * \throw std::invalid_argument - */ - void - setStopbits (stopbits_t stopbits); - - /*! Gets the stopbits for the serial port. - * - * \see Serial::setStopbits - * - * \throw std::invalid_argument - */ - stopbits_t - getStopbits () const; - - /*! Sets the flow control for the serial port. - * - * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none, - * possible values are: flowcontrol_none, flowcontrol_software, - * flowcontrol_hardware - * - * \throw std::invalid_argument - */ - void - setFlowcontrol (flowcontrol_t flowcontrol); - - /*! Gets the flow control for the serial port. - * - * \see Serial::setFlowcontrol - * - * \throw std::invalid_argument - */ - flowcontrol_t - getFlowcontrol () const; - - /*! Flush the input and output buffers */ - void - flush (); - - /*! Flush only the input buffer */ - void - flushInput (); - - /*! Flush only the output buffer */ - void - flushOutput (); - - /*! Sends the RS-232 break signal. See tcsendbreak(3). */ - void - sendBreak (int duration); - - /*! Set the break condition to a given level. Defaults to true. */ - void - setBreak (bool level = true); - - /*! Set the RTS handshaking line to the given level. Defaults to true. */ - void - setRTS (bool level = true); - - /*! Set the DTR handshaking line to the given level. Defaults to true. */ - void - setDTR (bool level = true); - - /*! - * Blocks until CTS, DSR, RI, CD changes or something interrupts it. - * - * Can throw an exception if an error occurs while waiting. - * You can check the status of CTS, DSR, RI, and CD once this returns. - * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a - * resolution of less than +-1ms and as good as +-0.2ms. Otherwise a - * polling method is used which can give +-2ms. - * - * \return Returns true if one of the lines changed, false if something else - * occurred. - * - * \throw SerialException - */ - bool - waitForChange (); - - /*! Returns the current status of the CTS line. */ - bool - getCTS (); - - /*! Returns the current status of the DSR line. */ - bool - getDSR (); - - /*! Returns the current status of the RI line. */ - bool - getRI (); - - /*! Returns the current status of the CD line. */ - bool - getCD (); - -private: - // Disable copy constructors - Serial(const Serial&); - Serial& operator=(const Serial&); - - // Pimpl idiom, d_pointer - class SerialImpl; - SerialImpl *pimpl_; - - // Scoped Lock Classes - class ScopedReadLock; - class ScopedWriteLock; - - // Read common function - size_t - read_ (uint8_t *buffer, size_t size); - // Write common function - size_t - write_ (const uint8_t *data, size_t length); - -}; - -class SerialException : public std::exception -{ - // Disable copy constructors - SerialException& operator=(const SerialException&); - std::string e_what_; -public: - SerialException (const char *description) { - std::stringstream ss; - ss << "SerialException " << description << " failed."; - e_what_ = ss.str(); - } - SerialException (const SerialException& other) : e_what_(other.e_what_) {} - virtual ~SerialException() throw() {} - virtual const char* what () const throw () { - return e_what_.c_str(); - } -}; - -class IOException : public std::exception -{ - // Disable copy constructors - IOException& operator=(const IOException&); - std::string file_; - int line_; - std::string e_what_; - int errno_; -public: - explicit IOException (std::string file, int line, int errnum) - : file_(file), line_(line), errno_(errnum) { - std::stringstream ss; -#if defined(_WIN32) && !defined(__MINGW32__) - char error_str [1024]; - strerror_s(error_str, 1024, errnum); -#else - char * error_str = strerror(errnum); -#endif - ss << "IO Exception (" << errno_ << "): " << error_str; - ss << ", file " << file_ << ", line " << line_ << "."; - e_what_ = ss.str(); - } - explicit IOException (std::string file, int line, const char * description) - : file_(file), line_(line), errno_(0) { - std::stringstream ss; - ss << "IO Exception: " << description; - ss << ", file " << file_ << ", line " << line_ << "."; - e_what_ = ss.str(); - } - virtual ~IOException() throw() {} - IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {} - - int getErrorNumber () const { return errno_; } - - virtual const char* what () const throw () { - return e_what_.c_str(); - } -}; - -class PortNotOpenedException : public std::exception -{ - // Disable copy constructors - const PortNotOpenedException& operator=(PortNotOpenedException); - std::string e_what_; -public: - PortNotOpenedException (const char * description) { - std::stringstream ss; - ss << "PortNotOpenedException " << description << " failed."; - e_what_ = ss.str(); - } - PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {} - virtual ~PortNotOpenedException() throw() {} - virtual const char* what () const throw () { - return e_what_.c_str(); - } -}; - -/*! - * Structure that describes a serial device. - */ -struct PortInfo { - - /*! Address of the serial port (this can be passed to the constructor of Serial). */ - std::string port; - - /*! Human readable description of serial device if available. */ - std::string description; - - /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */ - std::string hardware_id; - -}; - -/* Lists the serial ports available on the system - * - * Returns a vector of available serial ports, each represented - * by a serial::PortInfo data structure: - * - * \return vector of serial::PortInfo. - */ -std::vector -list_ports(); - -} // namespace serial - -#endif diff --git a/src/perception/pbox_node_dirve/src/serial/include/serial/v8stdint.h b/src/perception/pbox_node_dirve/src/serial/include/serial/v8stdint.h deleted file mode 100644 index f3be96b..0000000 --- a/src/perception/pbox_node_dirve/src/serial/include/serial/v8stdint.h +++ /dev/null @@ -1,57 +0,0 @@ -// This header is from the v8 google project: -// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h - -// Copyright 2012 the V8 project authors. All rights reserved. -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are -// met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of Google Inc. nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -// Load definitions of standard types. - -#ifndef V8STDINT_H_ -#define V8STDINT_H_ - -#include -#include - -#if defined(_WIN32) && !defined(__MINGW32__) - -typedef signed char int8_t; -typedef unsigned char uint8_t; -typedef short int16_t; // NOLINT -typedef unsigned short uint16_t; // NOLINT -typedef int int32_t; -typedef unsigned int uint32_t; -typedef __int64 int64_t; -typedef unsigned __int64 uint64_t; -// intptr_t and friends are defined in crtdefs.h through stdio.h. - -#else - -#include - -#endif - -#endif // V8STDINT_H_ diff --git a/src/perception/pbox_node_dirve/src/serial/package.xml b/src/perception/pbox_node_dirve/src/serial/package.xml deleted file mode 100644 index f1ac9d3..0000000 --- a/src/perception/pbox_node_dirve/src/serial/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - serial - 1.2.1 - - Serial is a cross-platform, simple to use library for using serial ports on computers. - This library provides a C++, object oriented interface for interacting with RS-232 - like devices on Linux and Windows. - - - William Woodall - - MIT - - http://wjwwood.github.com/serial/ - https://github.com/wjwwood/serial - https://github.com/wjwwood/serial/issues - - William Woodall - John Harrison - - ament_cmake - - boost - - - ament_cmake - - - diff --git a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_linux.cc b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_linux.cc deleted file mode 100644 index 9779d5e..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_linux.cc +++ /dev/null @@ -1,335 +0,0 @@ -#if defined(__linux__) - -/* - * Copyright (c) 2014 Craig Lilley - * This software is made available under the terms of the MIT licence. - * A copy of the licence can be obtained from: - * http://opensource.org/licenses/MIT - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "serial/serial.h" - -using serial::PortInfo; -using std::istringstream; -using std::ifstream; -using std::getline; -using std::vector; -using std::string; -using std::cout; -using std::endl; - -static vector glob(const vector& patterns); -static string basename(const string& path); -static string dirname(const string& path); -static bool path_exists(const string& path); -static string realpath(const string& path); -static string usb_sysfs_friendly_name(const string& sys_usb_path); -static vector get_sysfs_info(const string& device_path); -static string read_line(const string& file); -static string usb_sysfs_hw_string(const string& sysfs_path); -static string format(const char* format, ...); - -vector -glob(const vector& patterns) -{ - vector paths_found; - - if(patterns.size() == 0) - return paths_found; - - glob_t glob_results; - - int glob_retval = glob(patterns[0].c_str(), 0, NULL, &glob_results); - - vector::const_iterator iter = patterns.begin(); - - while(++iter != patterns.end()) - { - glob_retval = glob(iter->c_str(), GLOB_APPEND, NULL, &glob_results); - } - - for(int path_index = 0; path_index < glob_results.gl_pathc; path_index++) - { - paths_found.push_back(glob_results.gl_pathv[path_index]); - } - - globfree(&glob_results); - - return paths_found; -} - -string -basename(const string& path) -{ - size_t pos = path.rfind("/"); - - if(pos == std::string::npos) - return path; - - return string(path, pos+1, string::npos); -} - -string -dirname(const string& path) -{ - size_t pos = path.rfind("/"); - - if(pos == std::string::npos) - return path; - else if(pos == 0) - return "/"; - - return string(path, 0, pos); -} - -bool -path_exists(const string& path) -{ - struct stat sb; - - if( stat(path.c_str(), &sb ) == 0 ) - return true; - - return false; -} - -string -realpath(const string& path) -{ - char* real_path = realpath(path.c_str(), NULL); - - string result; - - if(real_path != NULL) - { - result = real_path; - - free(real_path); - } - - return result; -} - -string -usb_sysfs_friendly_name(const string& sys_usb_path) -{ - unsigned int device_number = 0; - - istringstream( read_line(sys_usb_path + "/devnum") ) >> device_number; - - string manufacturer = read_line( sys_usb_path + "/manufacturer" ); - - string product = read_line( sys_usb_path + "/product" ); - - string serial = read_line( sys_usb_path + "/serial" ); - - if( manufacturer.empty() && product.empty() && serial.empty() ) - return ""; - - return format("%s %s %s", manufacturer.c_str(), product.c_str(), serial.c_str() ); -} - -vector -get_sysfs_info(const string& device_path) -{ - string device_name = basename( device_path ); - - string friendly_name; - - string hardware_id; - - string sys_device_path = format( "/sys/class/tty/%s/device", device_name.c_str() ); - - if( device_name.compare(0,6,"ttyUSB") == 0 ) - { - sys_device_path = dirname( dirname( realpath( sys_device_path ) ) ); - - if( path_exists( sys_device_path ) ) - { - friendly_name = usb_sysfs_friendly_name( sys_device_path ); - - hardware_id = usb_sysfs_hw_string( sys_device_path ); - } - } - else if( device_name.compare(0,6,"ttyACM") == 0 ) - { - sys_device_path = dirname( realpath( sys_device_path ) ); - - if( path_exists( sys_device_path ) ) - { - friendly_name = usb_sysfs_friendly_name( sys_device_path ); - - hardware_id = usb_sysfs_hw_string( sys_device_path ); - } - } - else - { - // Try to read ID string of PCI device - - string sys_id_path = sys_device_path + "/id"; - - if( path_exists( sys_id_path ) ) - hardware_id = read_line( sys_id_path ); - } - - if( friendly_name.empty() ) - friendly_name = device_name; - - if( hardware_id.empty() ) - hardware_id = "n/a"; - - vector result; - result.push_back(friendly_name); - result.push_back(hardware_id); - - return result; -} - -string -read_line(const string& file) -{ - ifstream ifs(file.c_str(), ifstream::in); - - string line; - - if(ifs) - { - getline(ifs, line); - } - - return line; -} - -string -format(const char* format, ...) -{ - va_list ap; - - size_t buffer_size_bytes = 256; - - string result; - - char* buffer = (char*)malloc(buffer_size_bytes); - - if( buffer == NULL ) - return result; - - bool done = false; - - unsigned int loop_count = 0; - - while(!done) - { - va_start(ap, format); - - int return_value = vsnprintf(buffer, buffer_size_bytes, format, ap); - - if( return_value < 0 ) - { - done = true; - } - else if( return_value >= buffer_size_bytes ) - { - // Realloc and try again. - - buffer_size_bytes = return_value + 1; - - char* new_buffer_ptr = (char*)realloc(buffer, buffer_size_bytes); - - if( new_buffer_ptr == NULL ) - { - done = true; - } - else - { - buffer = new_buffer_ptr; - } - } - else - { - result = buffer; - done = true; - } - - va_end(ap); - - if( ++loop_count > 5 ) - done = true; - } - - free(buffer); - - return result; -} - -string -usb_sysfs_hw_string(const string& sysfs_path) -{ - string serial_number = read_line( sysfs_path + "/serial" ); - - if( serial_number.length() > 0 ) - { - serial_number = format( "SNR=%s", serial_number.c_str() ); - } - - string vid = read_line( sysfs_path + "/idVendor" ); - - string pid = read_line( sysfs_path + "/idProduct" ); - - return format("USB VID:PID=%s:%s %s", vid.c_str(), pid.c_str(), serial_number.c_str() ); -} - -vector -serial::list_ports() -{ - vector results; - - vector search_globs; - search_globs.push_back("/dev/ttyACM*"); - search_globs.push_back("/dev/ttyS*"); - search_globs.push_back("/dev/ttyUSB*"); - search_globs.push_back("/dev/tty.*"); - search_globs.push_back("/dev/cu.*"); - - vector devices_found = glob( search_globs ); - - vector::iterator iter = devices_found.begin(); - - while( iter != devices_found.end() ) - { - string device = *iter++; - - vector sysfs_info = get_sysfs_info( device ); - - string friendly_name = sysfs_info[0]; - - string hardware_id = sysfs_info[1]; - - PortInfo device_entry; - device_entry.port = device; - device_entry.description = friendly_name; - device_entry.hardware_id = hardware_id; - - results.push_back( device_entry ); - - } - - return results; -} - -#endif // defined(__linux__) diff --git a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_osx.cc b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_osx.cc deleted file mode 100644 index 333c55c..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_osx.cc +++ /dev/null @@ -1,286 +0,0 @@ -#if defined(__APPLE__) - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "serial/serial.h" - -using serial::PortInfo; -using std::string; -using std::vector; - -#define HARDWARE_ID_STRING_LENGTH 128 - -string cfstring_to_string( CFStringRef cfstring ); -string get_device_path( io_object_t& serial_port ); -string get_class_name( io_object_t& obj ); -io_registry_entry_t get_parent_iousb_device( io_object_t& serial_port ); -string get_string_property( io_object_t& device, const char* property ); -uint16_t get_int_property( io_object_t& device, const char* property ); -string rtrim(const string& str); - -string -cfstring_to_string( CFStringRef cfstring ) -{ - char cstring[MAXPATHLEN]; - string result; - - if( cfstring ) - { - Boolean success = CFStringGetCString( cfstring, - cstring, - sizeof(cstring), - kCFStringEncodingASCII ); - - if( success ) - result = cstring; - } - - return result; -} - -string -get_device_path( io_object_t& serial_port ) -{ - CFTypeRef callout_path; - string device_path; - - callout_path = IORegistryEntryCreateCFProperty( serial_port, - CFSTR(kIOCalloutDeviceKey), - kCFAllocatorDefault, - 0 ); - - if (callout_path) - { - if( CFGetTypeID(callout_path) == CFStringGetTypeID() ) - device_path = cfstring_to_string( static_cast(callout_path) ); - - CFRelease(callout_path); - } - - return device_path; -} - -string -get_class_name( io_object_t& obj ) -{ - string result; - io_name_t class_name; - kern_return_t kern_result; - - kern_result = IOObjectGetClass( obj, class_name ); - - if( kern_result == KERN_SUCCESS ) - result = class_name; - - return result; -} - -io_registry_entry_t -get_parent_iousb_device( io_object_t& serial_port ) -{ - io_object_t device = serial_port; - io_registry_entry_t parent = 0; - io_registry_entry_t result = 0; - kern_return_t kern_result = KERN_FAILURE; - string name = get_class_name(device); - - // Walk the IO Registry tree looking for this devices parent IOUSBDevice. - while( name != "IOUSBDevice" ) - { - kern_result = IORegistryEntryGetParentEntry( device, - kIOServicePlane, - &parent ); - - if(kern_result != KERN_SUCCESS) - { - result = 0; - break; - } - - device = parent; - - name = get_class_name(device); - } - - if(kern_result == KERN_SUCCESS) - result = device; - - return result; -} - -string -get_string_property( io_object_t& device, const char* property ) -{ - string property_name; - - if( device ) - { - CFStringRef property_as_cfstring = CFStringCreateWithCString ( - kCFAllocatorDefault, - property, - kCFStringEncodingASCII ); - - CFTypeRef name_as_cfstring = IORegistryEntryCreateCFProperty( - device, - property_as_cfstring, - kCFAllocatorDefault, - 0 ); - - if( name_as_cfstring ) - { - if( CFGetTypeID(name_as_cfstring) == CFStringGetTypeID() ) - property_name = cfstring_to_string( static_cast(name_as_cfstring) ); - - CFRelease(name_as_cfstring); - } - - if(property_as_cfstring) - CFRelease(property_as_cfstring); - } - - return property_name; -} - -uint16_t -get_int_property( io_object_t& device, const char* property ) -{ - uint16_t result = 0; - - if( device ) - { - CFStringRef property_as_cfstring = CFStringCreateWithCString ( - kCFAllocatorDefault, - property, - kCFStringEncodingASCII ); - - CFTypeRef number = IORegistryEntryCreateCFProperty( device, - property_as_cfstring, - kCFAllocatorDefault, - 0 ); - - if(property_as_cfstring) - CFRelease(property_as_cfstring); - - if( number ) - { - if( CFGetTypeID(number) == CFNumberGetTypeID() ) - { - bool success = CFNumberGetValue( static_cast(number), - kCFNumberSInt16Type, - &result ); - - if( !success ) - result = 0; - } - - CFRelease(number); - } - - } - - return result; -} - -string rtrim(const string& str) -{ - string result = str; - - string whitespace = " \t\f\v\n\r"; - - std::size_t found = result.find_last_not_of(whitespace); - - if (found != std::string::npos) - result.erase(found+1); - else - result.clear(); - - return result; -} - -vector -serial::list_ports(void) -{ - vector devices_found; - CFMutableDictionaryRef classes_to_match; - io_iterator_t serial_port_iterator; - io_object_t serial_port; - mach_port_t master_port; - kern_return_t kern_result; - - kern_result = IOMasterPort(MACH_PORT_NULL, &master_port); - - if(kern_result != KERN_SUCCESS) - return devices_found; - - classes_to_match = IOServiceMatching(kIOSerialBSDServiceValue); - - if (classes_to_match == NULL) - return devices_found; - - CFDictionarySetValue( classes_to_match, - CFSTR(kIOSerialBSDTypeKey), - CFSTR(kIOSerialBSDAllTypes) ); - - kern_result = IOServiceGetMatchingServices(master_port, classes_to_match, &serial_port_iterator); - - if (KERN_SUCCESS != kern_result) - return devices_found; - - while ( (serial_port = IOIteratorNext(serial_port_iterator)) ) - { - string device_path = get_device_path( serial_port ); - io_registry_entry_t parent = get_parent_iousb_device( serial_port ); - IOObjectRelease(serial_port); - - if( device_path.empty() ) - continue; - - PortInfo port_info; - port_info.port = device_path; - port_info.description = "n/a"; - port_info.hardware_id = "n/a"; - - string device_name = rtrim( get_string_property( parent, "USB Product Name" ) ); - string vendor_name = rtrim( get_string_property( parent, "USB Vendor Name") ); - string description = rtrim( vendor_name + " " + device_name ); - if( !description.empty() ) - port_info.description = description; - - string serial_number = rtrim(get_string_property( parent, "USB Serial Number" ) ); - uint16_t vendor_id = get_int_property( parent, "idVendor" ); - uint16_t product_id = get_int_property( parent, "idProduct" ); - - if( vendor_id && product_id ) - { - char cstring[HARDWARE_ID_STRING_LENGTH]; - - if(serial_number.empty()) - serial_number = "None"; - - int ret = snprintf( cstring, HARDWARE_ID_STRING_LENGTH, "USB VID:PID=%04x:%04x SNR=%s", - vendor_id, - product_id, - serial_number.c_str() ); - - if( (ret >= 0) && (ret < HARDWARE_ID_STRING_LENGTH) ) - port_info.hardware_id = cstring; - } - - devices_found.push_back(port_info); - } - - IOObjectRelease(serial_port_iterator); - return devices_found; -} - -#endif // defined(__APPLE__) diff --git a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_win.cc b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_win.cc deleted file mode 100644 index 7da40c4..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_win.cc +++ /dev/null @@ -1,152 +0,0 @@ -#if defined(_WIN32) - -/* - * Copyright (c) 2014 Craig Lilley - * This software is made available under the terms of the MIT licence. - * A copy of the licence can be obtained from: - * http://opensource.org/licenses/MIT - */ - -#include "serial/serial.h" -#include -#include -#include -#include -#include -#include - -using serial::PortInfo; -using std::vector; -using std::string; - -static const DWORD port_name_max_length = 256; -static const DWORD friendly_name_max_length = 256; -static const DWORD hardware_id_max_length = 256; - -// Convert a wide Unicode string to an UTF8 string -std::string utf8_encode(const std::wstring &wstr) -{ - int size_needed = WideCharToMultiByte(CP_UTF8, 0, &wstr[0], (int)wstr.size(), NULL, 0, NULL, NULL); - std::string strTo( size_needed, 0 ); - WideCharToMultiByte (CP_UTF8, 0, &wstr[0], (int)wstr.size(), &strTo[0], size_needed, NULL, NULL); - return strTo; -} - -vector -serial::list_ports() -{ - vector devices_found; - - HDEVINFO device_info_set = SetupDiGetClassDevs( - (const GUID *) &GUID_DEVCLASS_PORTS, - NULL, - NULL, - DIGCF_PRESENT); - - unsigned int device_info_set_index = 0; - SP_DEVINFO_DATA device_info_data; - - device_info_data.cbSize = sizeof(SP_DEVINFO_DATA); - - while(SetupDiEnumDeviceInfo(device_info_set, device_info_set_index, &device_info_data)) - { - device_info_set_index++; - - // Get port name - - HKEY hkey = SetupDiOpenDevRegKey( - device_info_set, - &device_info_data, - DICS_FLAG_GLOBAL, - 0, - DIREG_DEV, - KEY_READ); - - TCHAR port_name[port_name_max_length]; - DWORD port_name_length = port_name_max_length; - - LONG return_code = RegQueryValueEx( - hkey, - _T("PortName"), - NULL, - NULL, - (LPBYTE)port_name, - &port_name_length); - - RegCloseKey(hkey); - - if(return_code != EXIT_SUCCESS) - continue; - - if(port_name_length > 0 && port_name_length <= port_name_max_length) - port_name[port_name_length-1] = '\0'; - else - port_name[0] = '\0'; - - // Ignore parallel ports - - if(_tcsstr(port_name, _T("LPT")) != NULL) - continue; - - // Get port friendly name - - TCHAR friendly_name[friendly_name_max_length]; - DWORD friendly_name_actual_length = 0; - - BOOL got_friendly_name = SetupDiGetDeviceRegistryProperty( - device_info_set, - &device_info_data, - SPDRP_FRIENDLYNAME, - NULL, - (PBYTE)friendly_name, - friendly_name_max_length, - &friendly_name_actual_length); - - if(got_friendly_name == TRUE && friendly_name_actual_length > 0) - friendly_name[friendly_name_actual_length-1] = '\0'; - else - friendly_name[0] = '\0'; - - // Get hardware ID - - TCHAR hardware_id[hardware_id_max_length]; - DWORD hardware_id_actual_length = 0; - - BOOL got_hardware_id = SetupDiGetDeviceRegistryProperty( - device_info_set, - &device_info_data, - SPDRP_HARDWAREID, - NULL, - (PBYTE)hardware_id, - hardware_id_max_length, - &hardware_id_actual_length); - - if(got_hardware_id == TRUE && hardware_id_actual_length > 0) - hardware_id[hardware_id_actual_length-1] = '\0'; - else - hardware_id[0] = '\0'; - - #ifdef UNICODE - std::string portName = utf8_encode(port_name); - std::string friendlyName = utf8_encode(friendly_name); - std::string hardwareId = utf8_encode(hardware_id); - #else - std::string portName = port_name; - std::string friendlyName = friendly_name; - std::string hardwareId = hardware_id; - #endif - - PortInfo port_entry; - port_entry.port = portName; - port_entry.description = friendlyName; - port_entry.hardware_id = hardwareId; - - devices_found.push_back(port_entry); - } - - SetupDiDestroyDeviceInfoList(device_info_set); - - return devices_found; -} - -#endif // #if defined(_WIN32) diff --git a/src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc b/src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc deleted file mode 100644 index 4309aa6..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc +++ /dev/null @@ -1,1066 +0,0 @@ -/* Copyright 2012 William Woodall and John Harrison - * - * Additional Contributors: Christopher Baker @bakercp - */ - -#if !defined(_WIN32) - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if defined(__linux__) -# include -#endif - -#include -#include -#include -#ifdef __MACH__ -#include -#include -#include -#endif - -#include "serial/impl/unix.h" - -#ifndef TIOCINQ -#ifdef FIONREAD -#define TIOCINQ FIONREAD -#else -#define TIOCINQ 0x541B -#endif -#endif - -#if defined(MAC_OS_X_VERSION_10_3) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_3) -#include -#endif - -using std::string; -using std::stringstream; -using std::invalid_argument; -using serial::MillisecondTimer; -using serial::Serial; -using serial::SerialException; -using serial::PortNotOpenedException; -using serial::IOException; - - -MillisecondTimer::MillisecondTimer (const uint32_t millis) - : expiry(timespec_now()) -{ - int64_t tv_nsec = expiry.tv_nsec + (millis * 1e6); - if (tv_nsec >= 1e9) { - int64_t sec_diff = tv_nsec / static_cast (1e9); - expiry.tv_nsec = tv_nsec % static_cast(1e9); - expiry.tv_sec += sec_diff; - } else { - expiry.tv_nsec = tv_nsec; - } -} - -int64_t -MillisecondTimer::remaining () -{ - timespec now(timespec_now()); - int64_t millis = (expiry.tv_sec - now.tv_sec) * 1e3; - millis += (expiry.tv_nsec - now.tv_nsec) / 1e6; - return millis; -} - -timespec -MillisecondTimer::timespec_now () -{ - timespec time; -# ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - time.tv_sec = mts.tv_sec; - time.tv_nsec = mts.tv_nsec; -# else - clock_gettime(CLOCK_MONOTONIC, &time); -# endif - return time; -} - -timespec -timespec_from_ms (const uint32_t millis) -{ - timespec time; - time.tv_sec = millis / 1e3; - time.tv_nsec = (millis - (time.tv_sec * 1e3)) * 1e6; - return time; -} - -Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate, - bytesize_t bytesize, - parity_t parity, stopbits_t stopbits, - flowcontrol_t flowcontrol) - : port_ (port), fd_ (-1), is_open_ (false), xonxoff_ (false), rtscts_ (false), - baudrate_ (baudrate), parity_ (parity), - bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol) -{ - pthread_mutex_init(&this->read_mutex, NULL); - pthread_mutex_init(&this->write_mutex, NULL); - if (port_.empty () == false) - open (); -} - -Serial::SerialImpl::~SerialImpl () -{ - close(); - pthread_mutex_destroy(&this->read_mutex); - pthread_mutex_destroy(&this->write_mutex); -} - -void -Serial::SerialImpl::open () -{ - if (port_.empty ()) { - throw invalid_argument ("Empty port is invalid."); - } - if (is_open_ == true) { - throw SerialException ("Serial port already open."); - } - - fd_ = ::open (port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (fd_ == -1) { - switch (errno) { - case EINTR: - // Recurse because this is a recoverable error. - open (); - return; - case ENFILE: - case EMFILE: - THROW (IOException, "Too many file handles open."); - default: - THROW (IOException, errno); - } - } - - reconfigurePort(); - is_open_ = true; -} - -void -Serial::SerialImpl::reconfigurePort () -{ - if (fd_ == -1) { - // Can only operate on a valid file descriptor - THROW (IOException, "Invalid file descriptor, is the serial port open?"); - } - - struct termios options; // The options for the file descriptor - - if (tcgetattr(fd_, &options) == -1) { - THROW (IOException, "::tcgetattr"); - } - - // set up raw mode / no echo / binary - options.c_cflag |= (tcflag_t) (CLOCAL | CREAD); - options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | - ISIG | IEXTEN); //|ECHOPRT - - options.c_oflag &= (tcflag_t) ~(OPOST); - options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK); -#ifdef IUCLC - options.c_iflag &= (tcflag_t) ~IUCLC; -#endif -#ifdef PARMRK - options.c_iflag &= (tcflag_t) ~PARMRK; -#endif - - // setup baud rate - bool custom_baud = false; - speed_t baud; - switch (baudrate_) { -#ifdef B0 - case 0: baud = B0; break; -#endif -#ifdef B50 - case 50: baud = B50; break; -#endif -#ifdef B75 - case 75: baud = B75; break; -#endif -#ifdef B110 - case 110: baud = B110; break; -#endif -#ifdef B134 - case 134: baud = B134; break; -#endif -#ifdef B150 - case 150: baud = B150; break; -#endif -#ifdef B200 - case 200: baud = B200; break; -#endif -#ifdef B300 - case 300: baud = B300; break; -#endif -#ifdef B600 - case 600: baud = B600; break; -#endif -#ifdef B1200 - case 1200: baud = B1200; break; -#endif -#ifdef B1800 - case 1800: baud = B1800; break; -#endif -#ifdef B2400 - case 2400: baud = B2400; break; -#endif -#ifdef B4800 - case 4800: baud = B4800; break; -#endif -#ifdef B7200 - case 7200: baud = B7200; break; -#endif -#ifdef B9600 - case 9600: baud = B9600; break; -#endif -#ifdef B14400 - case 14400: baud = B14400; break; -#endif -#ifdef B19200 - case 19200: baud = B19200; break; -#endif -#ifdef B28800 - case 28800: baud = B28800; break; -#endif -#ifdef B57600 - case 57600: baud = B57600; break; -#endif -#ifdef B76800 - case 76800: baud = B76800; break; -#endif -#ifdef B38400 - case 38400: baud = B38400; break; -#endif -#ifdef B115200 - case 115200: baud = B115200; break; -#endif -#ifdef B128000 - case 128000: baud = B128000; break; -#endif -#ifdef B153600 - case 153600: baud = B153600; break; -#endif -#ifdef B230400 - case 230400: baud = B230400; break; -#endif -#ifdef B256000 - case 256000: baud = B256000; break; -#endif -#ifdef B460800 - case 460800: baud = B460800; break; -#endif -#ifdef B500000 - case 500000: baud = B500000; break; -#endif -#ifdef B576000 - case 576000: baud = B576000; break; -#endif -#ifdef B921600 - case 921600: baud = B921600; break; -#endif -#ifdef B1000000 - case 1000000: baud = B1000000; break; -#endif -#ifdef B1152000 - case 1152000: baud = B1152000; break; -#endif -#ifdef B1500000 - case 1500000: baud = B1500000; break; -#endif -#ifdef B2000000 - case 2000000: baud = B2000000; break; -#endif -#ifdef B2500000 - case 2500000: baud = B2500000; break; -#endif -#ifdef B3000000 - case 3000000: baud = B3000000; break; -#endif -#ifdef B3500000 - case 3500000: baud = B3500000; break; -#endif -#ifdef B4000000 - case 4000000: baud = B4000000; break; -#endif - default: - custom_baud = true; - // OS X support -#if defined(MAC_OS_X_VERSION_10_4) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_4) - // Starting with Tiger, the IOSSIOSPEED ioctl can be used to set arbitrary baud rates - // other than those specified by POSIX. The driver for the underlying serial hardware - // ultimately determines which baud rates can be used. This ioctl sets both the input - // and output speed. - speed_t new_baud = static_cast (baudrate_); - if (-1 == ioctl (fd_, IOSSIOSPEED, &new_baud, 1)) { - THROW (IOException, errno); - } - // Linux Support -#elif defined(__linux__) && defined (TIOCSSERIAL) - struct serial_struct ser; - - if (-1 == ioctl (fd_, TIOCGSERIAL, &ser)) { - THROW (IOException, errno); - } - - // set custom divisor - ser.custom_divisor = ser.baud_base / static_cast (baudrate_); - // update flags - ser.flags &= ~ASYNC_SPD_MASK; - ser.flags |= ASYNC_SPD_CUST; - - if (-1 == ioctl (fd_, TIOCSSERIAL, &ser)) { - THROW (IOException, errno); - } -#else - throw invalid_argument ("OS does not currently support custom bauds"); -#endif - } - if (custom_baud == false) { -#ifdef _BSD_SOURCE - ::cfsetspeed(&options, baud); -#else - ::cfsetispeed(&options, baud); - ::cfsetospeed(&options, baud); -#endif - } - - // setup char len - options.c_cflag &= (tcflag_t) ~CSIZE; - if (bytesize_ == eightbits) - options.c_cflag |= CS8; - else if (bytesize_ == sevenbits) - options.c_cflag |= CS7; - else if (bytesize_ == sixbits) - options.c_cflag |= CS6; - else if (bytesize_ == fivebits) - options.c_cflag |= CS5; - else - throw invalid_argument ("invalid char len"); - // setup stopbits - if (stopbits_ == stopbits_one) - options.c_cflag &= (tcflag_t) ~(CSTOPB); - else if (stopbits_ == stopbits_one_point_five) - // ONE POINT FIVE same as TWO.. there is no POSIX support for 1.5 - options.c_cflag |= (CSTOPB); - else if (stopbits_ == stopbits_two) - options.c_cflag |= (CSTOPB); - else - throw invalid_argument ("invalid stop bit"); - // setup parity - options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP); - if (parity_ == parity_none) { - options.c_cflag &= (tcflag_t) ~(PARENB | PARODD); - } else if (parity_ == parity_even) { - options.c_cflag &= (tcflag_t) ~(PARODD); - options.c_cflag |= (PARENB); - } else if (parity_ == parity_odd) { - options.c_cflag |= (PARENB | PARODD); - } -#ifdef CMSPAR - else if (parity_ == parity_mark) { - options.c_cflag |= (PARENB | CMSPAR | PARODD); - } - else if (parity_ == parity_space) { - options.c_cflag |= (PARENB | CMSPAR); - options.c_cflag &= (tcflag_t) ~(PARODD); - } -#else - // CMSPAR is not defined on OSX. So do not support mark or space parity. - else if (parity_ == parity_mark || parity_ == parity_space) { - throw invalid_argument ("OS does not support mark or space parity"); - } -#endif // ifdef CMSPAR - else { - throw invalid_argument ("invalid parity"); - } - // setup flow control - if (flowcontrol_ == flowcontrol_none) { - xonxoff_ = false; - rtscts_ = false; - } - if (flowcontrol_ == flowcontrol_software) { - xonxoff_ = true; - rtscts_ = false; - } - if (flowcontrol_ == flowcontrol_hardware) { - xonxoff_ = false; - rtscts_ = true; - } - // xonxoff -#ifdef IXANY - if (xonxoff_) - options.c_iflag |= (IXON | IXOFF); //|IXANY) - else - options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY); -#else - if (xonxoff_) - options.c_iflag |= (IXON | IXOFF); - else - options.c_iflag &= (tcflag_t) ~(IXON | IXOFF); -#endif - // rtscts -#ifdef CRTSCTS - if (rtscts_) - options.c_cflag |= (CRTSCTS); - else - options.c_cflag &= (unsigned long) ~(CRTSCTS); -#elif defined CNEW_RTSCTS - if (rtscts_) - options.c_cflag |= (CNEW_RTSCTS); - else - options.c_cflag &= (unsigned long) ~(CNEW_RTSCTS); -#else -#error "OS Support seems wrong." -#endif - - // http://www.unixwiz.net/techtips/termios-vmin-vtime.html - // this basically sets the read call up to be a polling read, - // but we are using select to ensure there is data available - // to read before each call, so we should never needlessly poll - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 0; - - // activate settings - ::tcsetattr (fd_, TCSANOW, &options); - - // Update byte_time_ based on the new settings. - uint32_t bit_time_ns = 1e9 / baudrate_; - byte_time_ns_ = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_); - - // Compensate for the stopbits_one_point_five enum being equal to int 3, - // and not 1.5. - if (stopbits_ == stopbits_one_point_five) { - byte_time_ns_ += ((1.5 - stopbits_one_point_five) * bit_time_ns); - } -} - -void -Serial::SerialImpl::close () -{ - if (is_open_ == true) { - if (fd_ != -1) { - int ret; - ret = ::close (fd_); - if (ret == 0) { - fd_ = -1; - } else { - THROW (IOException, errno); - } - } - is_open_ = false; - } -} - -bool -Serial::SerialImpl::isOpen () const -{ - return is_open_; -} - -size_t -Serial::SerialImpl::available () -{ - if (!is_open_) { - return 0; - } - int count = 0; - if (-1 == ioctl (fd_, TIOCINQ, &count)) { - THROW (IOException, errno); - } else { - return static_cast (count); - } -} - -bool -Serial::SerialImpl::waitReadable (uint32_t timeout) -{ - // Setup a select call to block for serial data or a timeout - fd_set readfds; - FD_ZERO (&readfds); - FD_SET (fd_, &readfds); - timespec timeout_ts (timespec_from_ms (timeout)); - int r = pselect (fd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL); - - if (r < 0) { - // Select was interrupted - if (errno == EINTR) { - return false; - } - // Otherwise there was some error - THROW (IOException, errno); - } - // Timeout occurred - if (r == 0) { - return false; - } - // This shouldn't happen, if r > 0 our fd has to be in the list! - if (!FD_ISSET (fd_, &readfds)) { - THROW (IOException, "select reports ready to read, but our fd isn't" - " in the list, this shouldn't happen!"); - } - // Data available to read. - return true; -} - -void -Serial::SerialImpl::waitByteTimes (size_t count) -{ - timespec wait_time = { 0, static_cast(byte_time_ns_ * count)}; - pselect (0, NULL, NULL, NULL, &wait_time, NULL); -} - -size_t -Serial::SerialImpl::read (uint8_t *buf, size_t size) -{ - // If the port is not open, throw - if (!is_open_) { - throw PortNotOpenedException ("Serial::read"); - } - size_t bytes_read = 0; - - // Calculate total timeout in milliseconds t_c + (t_m * N) - long total_timeout_ms = timeout_.read_timeout_constant; - total_timeout_ms += timeout_.read_timeout_multiplier * static_cast (size); - MillisecondTimer total_timeout(total_timeout_ms); - - // Pre-fill buffer with available bytes - { - ssize_t bytes_read_now = ::read (fd_, buf, size); - if (bytes_read_now > 0) { - bytes_read = bytes_read_now; - } - } - - while (bytes_read < size) { - int64_t timeout_remaining_ms = total_timeout.remaining(); - if (timeout_remaining_ms <= 0) { - // Timed out - break; - } - // Timeout for the next select is whichever is less of the remaining - // total read timeout and the inter-byte timeout. - uint32_t timeout = std::min(static_cast (timeout_remaining_ms), - timeout_.inter_byte_timeout); - // Wait for the device to be readable, and then attempt to read. - if (waitReadable(timeout)) { - // If it's a fixed-length multi-byte read, insert a wait here so that - // we can attempt to grab the whole thing in a single IO call. Skip - // this wait if a non-max inter_byte_timeout is specified. - if (size > 1 && timeout_.inter_byte_timeout == Timeout::max()) { - size_t bytes_available = available(); - if (bytes_available + bytes_read < size) { - waitByteTimes(size - (bytes_available + bytes_read)); - } - } - // This should be non-blocking returning only what is available now - // Then returning so that select can block again. - ssize_t bytes_read_now = - ::read (fd_, buf + bytes_read, size - bytes_read); - // read should always return some data as select reported it was - // ready to read when we get to this point. - if (bytes_read_now < 1) { - // Disconnected devices, at least on Linux, show the - // behavior that they are always ready to read immediately - // but reading returns nothing. - throw SerialException ("device reports readiness to read but " - "returned no data (device disconnected?)"); - } - // Update bytes_read - bytes_read += static_cast (bytes_read_now); - // If bytes_read == size then we have read everything we need - if (bytes_read == size) { - break; - } - // If bytes_read < size then we have more to read - if (bytes_read < size) { - continue; - } - // If bytes_read > size then we have over read, which shouldn't happen - if (bytes_read > size) { - throw SerialException ("read over read, too many bytes where " - "read, this shouldn't happen, might be " - "a logical error!"); - } - } - } - return bytes_read; -} - -size_t -Serial::SerialImpl::write (const uint8_t *data, size_t length) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::write"); - } - fd_set writefds; - size_t bytes_written = 0; - - // Calculate total timeout in milliseconds t_c + (t_m * N) - long total_timeout_ms = timeout_.write_timeout_constant; - total_timeout_ms += timeout_.write_timeout_multiplier * static_cast (length); - MillisecondTimer total_timeout(total_timeout_ms); - - bool first_iteration = true; - while (bytes_written < length) { - int64_t timeout_remaining_ms = total_timeout.remaining(); - // Only consider the timeout if it's not the first iteration of the loop - // otherwise a timeout of 0 won't be allowed through - if (!first_iteration && (timeout_remaining_ms <= 0)) { - // Timed out - break; - } - first_iteration = false; - - timespec timeout(timespec_from_ms(timeout_remaining_ms)); - - FD_ZERO (&writefds); - FD_SET (fd_, &writefds); - - // Do the select - int r = pselect (fd_ + 1, NULL, &writefds, NULL, &timeout, NULL); - - // Figure out what happened by looking at select's response 'r' - /** Error **/ - if (r < 0) { - // Select was interrupted, try again - if (errno == EINTR) { - continue; - } - // Otherwise there was some error - THROW (IOException, errno); - } - /** Timeout **/ - if (r == 0) { - break; - } - /** Port ready to write **/ - if (r > 0) { - // Make sure our file descriptor is in the ready to write list - if (FD_ISSET (fd_, &writefds)) { - // This will write some - ssize_t bytes_written_now = - ::write (fd_, data + bytes_written, length - bytes_written); - // write should always return some data as select reported it was - // ready to write when we get to this point. - if (bytes_written_now < 1) { - // Disconnected devices, at least on Linux, show the - // behavior that they are always ready to write immediately - // but writing returns nothing. - throw SerialException ("device reports readiness to write but " - "returned no data (device disconnected?)"); - } - // Update bytes_written - bytes_written += static_cast (bytes_written_now); - // If bytes_written == size then we have written everything we need to - if (bytes_written == length) { - break; - } - // If bytes_written < size then we have more to write - if (bytes_written < length) { - continue; - } - // If bytes_written > size then we have over written, which shouldn't happen - if (bytes_written > length) { - throw SerialException ("write over wrote, too many bytes where " - "written, this shouldn't happen, might be " - "a logical error!"); - } - } - // This shouldn't happen, if r > 0 our fd has to be in the list! - THROW (IOException, "select reports ready to write, but our fd isn't" - " in the list, this shouldn't happen!"); - } - } - return bytes_written; -} - -void -Serial::SerialImpl::setPort (const string &port) -{ - port_ = port; -} - -string -Serial::SerialImpl::getPort () const -{ - return port_; -} - -void -Serial::SerialImpl::setTimeout (serial::Timeout &timeout) -{ - timeout_ = timeout; -} - -serial::Timeout -Serial::SerialImpl::getTimeout () const -{ - return timeout_; -} - -void -Serial::SerialImpl::setBaudrate (unsigned long baudrate) -{ - baudrate_ = baudrate; - if (is_open_) - reconfigurePort (); -} - -unsigned long -Serial::SerialImpl::getBaudrate () const -{ - return baudrate_; -} - -void -Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize) -{ - bytesize_ = bytesize; - if (is_open_) - reconfigurePort (); -} - -serial::bytesize_t -Serial::SerialImpl::getBytesize () const -{ - return bytesize_; -} - -void -Serial::SerialImpl::setParity (serial::parity_t parity) -{ - parity_ = parity; - if (is_open_) - reconfigurePort (); -} - -serial::parity_t -Serial::SerialImpl::getParity () const -{ - return parity_; -} - -void -Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits) -{ - stopbits_ = stopbits; - if (is_open_) - reconfigurePort (); -} - -serial::stopbits_t -Serial::SerialImpl::getStopbits () const -{ - return stopbits_; -} - -void -Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol) -{ - flowcontrol_ = flowcontrol; - if (is_open_) - reconfigurePort (); -} - -serial::flowcontrol_t -Serial::SerialImpl::getFlowcontrol () const -{ - return flowcontrol_; -} - -void -Serial::SerialImpl::flush () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::flush"); - } - tcdrain (fd_); -} - -void -Serial::SerialImpl::flushInput () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::flushInput"); - } - tcflush (fd_, TCIFLUSH); -} - -void -Serial::SerialImpl::flushOutput () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::flushOutput"); - } - tcflush (fd_, TCOFLUSH); -} - -void -Serial::SerialImpl::sendBreak (int duration) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::sendBreak"); - } - tcsendbreak (fd_, static_cast (duration / 4)); -} - -void -Serial::SerialImpl::setBreak (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setBreak"); - } - - if (level) { - if (-1 == ioctl (fd_, TIOCSBRK)) - { - stringstream ss; - ss << "setBreak failed on a call to ioctl(TIOCSBRK): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } else { - if (-1 == ioctl (fd_, TIOCCBRK)) - { - stringstream ss; - ss << "setBreak failed on a call to ioctl(TIOCCBRK): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } -} - -void -Serial::SerialImpl::setRTS (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setRTS"); - } - - int command = TIOCM_RTS; - - if (level) { - if (-1 == ioctl (fd_, TIOCMBIS, &command)) - { - stringstream ss; - ss << "setRTS failed on a call to ioctl(TIOCMBIS): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } else { - if (-1 == ioctl (fd_, TIOCMBIC, &command)) - { - stringstream ss; - ss << "setRTS failed on a call to ioctl(TIOCMBIC): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } -} - -void -Serial::SerialImpl::setDTR (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setDTR"); - } - - int command = TIOCM_DTR; - - if (level) { - if (-1 == ioctl (fd_, TIOCMBIS, &command)) - { - stringstream ss; - ss << "setDTR failed on a call to ioctl(TIOCMBIS): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } else { - if (-1 == ioctl (fd_, TIOCMBIC, &command)) - { - stringstream ss; - ss << "setDTR failed on a call to ioctl(TIOCMBIC): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - } -} - -bool -Serial::SerialImpl::waitForChange () -{ -#ifndef TIOCMIWAIT - -while (is_open_ == true) { - - int status; - - if (-1 == ioctl (fd_, TIOCMGET, &status)) - { - stringstream ss; - ss << "waitForChange failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - else - { - if (0 != (status & TIOCM_CTS) - || 0 != (status & TIOCM_DSR) - || 0 != (status & TIOCM_RI) - || 0 != (status & TIOCM_CD)) - { - return true; - } - } - - usleep(1000); - } - - return false; -#else - int command = (TIOCM_CD|TIOCM_DSR|TIOCM_RI|TIOCM_CTS); - - if (-1 == ioctl (fd_, TIOCMIWAIT, &command)) { - stringstream ss; - ss << "waitForDSR failed on a call to ioctl(TIOCMIWAIT): " - << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - return true; -#endif -} - -bool -Serial::SerialImpl::getCTS () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getCTS"); - } - - int status; - - if (-1 == ioctl (fd_, TIOCMGET, &status)) - { - stringstream ss; - ss << "getCTS failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - else - { - return 0 != (status & TIOCM_CTS); - } -} - -bool -Serial::SerialImpl::getDSR () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getDSR"); - } - - int status; - - if (-1 == ioctl (fd_, TIOCMGET, &status)) - { - stringstream ss; - ss << "getDSR failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - else - { - return 0 != (status & TIOCM_DSR); - } -} - -bool -Serial::SerialImpl::getRI () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getRI"); - } - - int status; - - if (-1 == ioctl (fd_, TIOCMGET, &status)) - { - stringstream ss; - ss << "getRI failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - else - { - return 0 != (status & TIOCM_RI); - } -} - -bool -Serial::SerialImpl::getCD () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getCD"); - } - - int status; - - if (-1 == ioctl (fd_, TIOCMGET, &status)) - { - stringstream ss; - ss << "getCD failed on a call to ioctl(TIOCMGET): " << errno << " " << strerror(errno); - throw(SerialException(ss.str().c_str())); - } - else - { - return 0 != (status & TIOCM_CD); - } -} - -void -Serial::SerialImpl::readLock () -{ - int result = pthread_mutex_lock(&this->read_mutex); - if (result) { - THROW (IOException, result); - } -} - -void -Serial::SerialImpl::readUnlock () -{ - int result = pthread_mutex_unlock(&this->read_mutex); - if (result) { - THROW (IOException, result); - } -} - -void -Serial::SerialImpl::writeLock () -{ - int result = pthread_mutex_lock(&this->write_mutex); - if (result) { - THROW (IOException, result); - } -} - -void -Serial::SerialImpl::writeUnlock () -{ - int result = pthread_mutex_unlock(&this->write_mutex); - if (result) { - THROW (IOException, result); - } -} - -#endif // !defined(_WIN32) diff --git a/src/perception/pbox_node_dirve/src/serial/src/impl/win.cc b/src/perception/pbox_node_dirve/src/serial/src/impl/win.cc deleted file mode 100644 index 786f4f6..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/impl/win.cc +++ /dev/null @@ -1,646 +0,0 @@ -#if defined(_WIN32) - -/* Copyright 2012 William Woodall and John Harrison */ - -#include - -#include "serial/impl/win.h" - -using std::string; -using std::wstring; -using std::stringstream; -using std::invalid_argument; -using serial::Serial; -using serial::Timeout; -using serial::bytesize_t; -using serial::parity_t; -using serial::stopbits_t; -using serial::flowcontrol_t; -using serial::SerialException; -using serial::PortNotOpenedException; -using serial::IOException; - -inline wstring -_prefix_port_if_needed(const wstring &input) -{ - static wstring windows_com_port_prefix = L"\\\\.\\"; - if (input.compare(windows_com_port_prefix) != 0) - { - return windows_com_port_prefix + input; - } - return input; -} - -Serial::SerialImpl::SerialImpl (const string &port, unsigned long baudrate, - bytesize_t bytesize, - parity_t parity, stopbits_t stopbits, - flowcontrol_t flowcontrol) - : port_ (port.begin(), port.end()), fd_ (INVALID_HANDLE_VALUE), is_open_ (false), - baudrate_ (baudrate), parity_ (parity), - bytesize_ (bytesize), stopbits_ (stopbits), flowcontrol_ (flowcontrol) -{ - if (port_.empty () == false) - open (); - read_mutex = CreateMutex(NULL, false, NULL); - write_mutex = CreateMutex(NULL, false, NULL); -} - -Serial::SerialImpl::~SerialImpl () -{ - this->close(); - CloseHandle(read_mutex); - CloseHandle(write_mutex); -} - -void -Serial::SerialImpl::open () -{ - if (port_.empty ()) { - throw invalid_argument ("Empty port is invalid."); - } - if (is_open_ == true) { - throw SerialException ("Serial port already open."); - } - - // See: https://github.com/wjwwood/serial/issues/84 - wstring port_with_prefix = _prefix_port_if_needed(port_); - LPCWSTR lp_port = port_with_prefix.c_str(); - fd_ = CreateFileW(lp_port, - GENERIC_READ | GENERIC_WRITE, - 0, - 0, - OPEN_EXISTING, - FILE_ATTRIBUTE_NORMAL, - 0); - - if (fd_ == INVALID_HANDLE_VALUE) { - DWORD create_file_err = GetLastError(); - stringstream ss; - switch (create_file_err) { - case ERROR_FILE_NOT_FOUND: - // Use this->getPort to convert to a std::string - ss << "Specified port, " << this->getPort() << ", does not exist."; - THROW (IOException, ss.str().c_str()); - default: - ss << "Unknown error opening the serial port: " << create_file_err; - THROW (IOException, ss.str().c_str()); - } - } - - reconfigurePort(); - is_open_ = true; -} - -void -Serial::SerialImpl::reconfigurePort () -{ - if (fd_ == INVALID_HANDLE_VALUE) { - // Can only operate on a valid file descriptor - THROW (IOException, "Invalid file descriptor, is the serial port open?"); - } - - DCB dcbSerialParams = {0}; - - dcbSerialParams.DCBlength=sizeof(dcbSerialParams); - - if (!GetCommState(fd_, &dcbSerialParams)) { - //error getting state - THROW (IOException, "Error getting the serial port state."); - } - - // setup baud rate - switch (baudrate_) { -#ifdef CBR_0 - case 0: dcbSerialParams.BaudRate = CBR_0; break; -#endif -#ifdef CBR_50 - case 50: dcbSerialParams.BaudRate = CBR_50; break; -#endif -#ifdef CBR_75 - case 75: dcbSerialParams.BaudRate = CBR_75; break; -#endif -#ifdef CBR_110 - case 110: dcbSerialParams.BaudRate = CBR_110; break; -#endif -#ifdef CBR_134 - case 134: dcbSerialParams.BaudRate = CBR_134; break; -#endif -#ifdef CBR_150 - case 150: dcbSerialParams.BaudRate = CBR_150; break; -#endif -#ifdef CBR_200 - case 200: dcbSerialParams.BaudRate = CBR_200; break; -#endif -#ifdef CBR_300 - case 300: dcbSerialParams.BaudRate = CBR_300; break; -#endif -#ifdef CBR_600 - case 600: dcbSerialParams.BaudRate = CBR_600; break; -#endif -#ifdef CBR_1200 - case 1200: dcbSerialParams.BaudRate = CBR_1200; break; -#endif -#ifdef CBR_1800 - case 1800: dcbSerialParams.BaudRate = CBR_1800; break; -#endif -#ifdef CBR_2400 - case 2400: dcbSerialParams.BaudRate = CBR_2400; break; -#endif -#ifdef CBR_4800 - case 4800: dcbSerialParams.BaudRate = CBR_4800; break; -#endif -#ifdef CBR_7200 - case 7200: dcbSerialParams.BaudRate = CBR_7200; break; -#endif -#ifdef CBR_9600 - case 9600: dcbSerialParams.BaudRate = CBR_9600; break; -#endif -#ifdef CBR_14400 - case 14400: dcbSerialParams.BaudRate = CBR_14400; break; -#endif -#ifdef CBR_19200 - case 19200: dcbSerialParams.BaudRate = CBR_19200; break; -#endif -#ifdef CBR_28800 - case 28800: dcbSerialParams.BaudRate = CBR_28800; break; -#endif -#ifdef CBR_57600 - case 57600: dcbSerialParams.BaudRate = CBR_57600; break; -#endif -#ifdef CBR_76800 - case 76800: dcbSerialParams.BaudRate = CBR_76800; break; -#endif -#ifdef CBR_38400 - case 38400: dcbSerialParams.BaudRate = CBR_38400; break; -#endif -#ifdef CBR_115200 - case 115200: dcbSerialParams.BaudRate = CBR_115200; break; -#endif -#ifdef CBR_128000 - case 128000: dcbSerialParams.BaudRate = CBR_128000; break; -#endif -#ifdef CBR_153600 - case 153600: dcbSerialParams.BaudRate = CBR_153600; break; -#endif -#ifdef CBR_230400 - case 230400: dcbSerialParams.BaudRate = CBR_230400; break; -#endif -#ifdef CBR_256000 - case 256000: dcbSerialParams.BaudRate = CBR_256000; break; -#endif -#ifdef CBR_460800 - case 460800: dcbSerialParams.BaudRate = CBR_460800; break; -#endif -#ifdef CBR_921600 - case 921600: dcbSerialParams.BaudRate = CBR_921600; break; -#endif - default: - // Try to blindly assign it - dcbSerialParams.BaudRate = baudrate_; - } - - // setup char len - if (bytesize_ == eightbits) - dcbSerialParams.ByteSize = 8; - else if (bytesize_ == sevenbits) - dcbSerialParams.ByteSize = 7; - else if (bytesize_ == sixbits) - dcbSerialParams.ByteSize = 6; - else if (bytesize_ == fivebits) - dcbSerialParams.ByteSize = 5; - else - throw invalid_argument ("invalid char len"); - - // setup stopbits - if (stopbits_ == stopbits_one) - dcbSerialParams.StopBits = ONESTOPBIT; - else if (stopbits_ == stopbits_one_point_five) - dcbSerialParams.StopBits = ONE5STOPBITS; - else if (stopbits_ == stopbits_two) - dcbSerialParams.StopBits = TWOSTOPBITS; - else - throw invalid_argument ("invalid stop bit"); - - // setup parity - if (parity_ == parity_none) { - dcbSerialParams.Parity = NOPARITY; - } else if (parity_ == parity_even) { - dcbSerialParams.Parity = EVENPARITY; - } else if (parity_ == parity_odd) { - dcbSerialParams.Parity = ODDPARITY; - } else if (parity_ == parity_mark) { - dcbSerialParams.Parity = MARKPARITY; - } else if (parity_ == parity_space) { - dcbSerialParams.Parity = SPACEPARITY; - } else { - throw invalid_argument ("invalid parity"); - } - - // setup flowcontrol - if (flowcontrol_ == flowcontrol_none) { - dcbSerialParams.fOutxCtsFlow = false; - dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE; - dcbSerialParams.fOutX = false; - dcbSerialParams.fInX = false; - } - if (flowcontrol_ == flowcontrol_software) { - dcbSerialParams.fOutxCtsFlow = false; - dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE; - dcbSerialParams.fOutX = true; - dcbSerialParams.fInX = true; - } - if (flowcontrol_ == flowcontrol_hardware) { - dcbSerialParams.fOutxCtsFlow = true; - dcbSerialParams.fRtsControl = RTS_CONTROL_HANDSHAKE; - dcbSerialParams.fOutX = false; - dcbSerialParams.fInX = false; - } - - // activate settings - if (!SetCommState(fd_, &dcbSerialParams)){ - CloseHandle(fd_); - THROW (IOException, "Error setting serial port settings."); - } - - // Setup timeouts - COMMTIMEOUTS timeouts = {0}; - timeouts.ReadIntervalTimeout = timeout_.inter_byte_timeout; - timeouts.ReadTotalTimeoutConstant = timeout_.read_timeout_constant; - timeouts.ReadTotalTimeoutMultiplier = timeout_.read_timeout_multiplier; - timeouts.WriteTotalTimeoutConstant = timeout_.write_timeout_constant; - timeouts.WriteTotalTimeoutMultiplier = timeout_.write_timeout_multiplier; - if (!SetCommTimeouts(fd_, &timeouts)) { - THROW (IOException, "Error setting timeouts."); - } -} - -void -Serial::SerialImpl::close () -{ - if (is_open_ == true) { - if (fd_ != INVALID_HANDLE_VALUE) { - int ret; - ret = CloseHandle(fd_); - if (ret == 0) { - stringstream ss; - ss << "Error while closing serial port: " << GetLastError(); - THROW (IOException, ss.str().c_str()); - } else { - fd_ = INVALID_HANDLE_VALUE; - } - } - is_open_ = false; - } -} - -bool -Serial::SerialImpl::isOpen () const -{ - return is_open_; -} - -size_t -Serial::SerialImpl::available () -{ - if (!is_open_) { - return 0; - } - COMSTAT cs; - if (!ClearCommError(fd_, NULL, &cs)) { - stringstream ss; - ss << "Error while checking status of the serial port: " << GetLastError(); - THROW (IOException, ss.str().c_str()); - } - return static_cast(cs.cbInQue); -} - -bool -Serial::SerialImpl::waitReadable (uint32_t /*timeout*/) -{ - THROW (IOException, "waitReadable is not implemented on Windows."); - return false; -} - -void -Serial::SerialImpl::waitByteTimes (size_t /*count*/) -{ - THROW (IOException, "waitByteTimes is not implemented on Windows."); -} - -size_t -Serial::SerialImpl::read (uint8_t *buf, size_t size) -{ - if (!is_open_) { - throw PortNotOpenedException ("Serial::read"); - } - DWORD bytes_read; - if (!ReadFile(fd_, buf, static_cast(size), &bytes_read, NULL)) { - stringstream ss; - ss << "Error while reading from the serial port: " << GetLastError(); - THROW (IOException, ss.str().c_str()); - } - return (size_t) (bytes_read); -} - -size_t -Serial::SerialImpl::write (const uint8_t *data, size_t length) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::write"); - } - DWORD bytes_written; - if (!WriteFile(fd_, data, static_cast(length), &bytes_written, NULL)) { - stringstream ss; - ss << "Error while writing to the serial port: " << GetLastError(); - THROW (IOException, ss.str().c_str()); - } - return (size_t) (bytes_written); -} - -void -Serial::SerialImpl::setPort (const string &port) -{ - port_ = wstring(port.begin(), port.end()); -} - -string -Serial::SerialImpl::getPort () const -{ - return string(port_.begin(), port_.end()); -} - -void -Serial::SerialImpl::setTimeout (serial::Timeout &timeout) -{ - timeout_ = timeout; - if (is_open_) { - reconfigurePort (); - } -} - -serial::Timeout -Serial::SerialImpl::getTimeout () const -{ - return timeout_; -} - -void -Serial::SerialImpl::setBaudrate (unsigned long baudrate) -{ - baudrate_ = baudrate; - if (is_open_) { - reconfigurePort (); - } -} - -unsigned long -Serial::SerialImpl::getBaudrate () const -{ - return baudrate_; -} - -void -Serial::SerialImpl::setBytesize (serial::bytesize_t bytesize) -{ - bytesize_ = bytesize; - if (is_open_) { - reconfigurePort (); - } -} - -serial::bytesize_t -Serial::SerialImpl::getBytesize () const -{ - return bytesize_; -} - -void -Serial::SerialImpl::setParity (serial::parity_t parity) -{ - parity_ = parity; - if (is_open_) { - reconfigurePort (); - } -} - -serial::parity_t -Serial::SerialImpl::getParity () const -{ - return parity_; -} - -void -Serial::SerialImpl::setStopbits (serial::stopbits_t stopbits) -{ - stopbits_ = stopbits; - if (is_open_) { - reconfigurePort (); - } -} - -serial::stopbits_t -Serial::SerialImpl::getStopbits () const -{ - return stopbits_; -} - -void -Serial::SerialImpl::setFlowcontrol (serial::flowcontrol_t flowcontrol) -{ - flowcontrol_ = flowcontrol; - if (is_open_) { - reconfigurePort (); - } -} - -serial::flowcontrol_t -Serial::SerialImpl::getFlowcontrol () const -{ - return flowcontrol_; -} - -void -Serial::SerialImpl::flush () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::flush"); - } - FlushFileBuffers (fd_); -} - -void -Serial::SerialImpl::flushInput () -{ - if (is_open_ == false) { - throw PortNotOpenedException("Serial::flushInput"); - } - PurgeComm(fd_, PURGE_RXCLEAR); -} - -void -Serial::SerialImpl::flushOutput () -{ - if (is_open_ == false) { - throw PortNotOpenedException("Serial::flushOutput"); - } - PurgeComm(fd_, PURGE_TXCLEAR); -} - -void -Serial::SerialImpl::sendBreak (int /*duration*/) -{ - THROW (IOException, "sendBreak is not supported on Windows."); -} - -void -Serial::SerialImpl::setBreak (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setBreak"); - } - if (level) { - EscapeCommFunction (fd_, SETBREAK); - } else { - EscapeCommFunction (fd_, CLRBREAK); - } -} - -void -Serial::SerialImpl::setRTS (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setRTS"); - } - if (level) { - EscapeCommFunction (fd_, SETRTS); - } else { - EscapeCommFunction (fd_, CLRRTS); - } -} - -void -Serial::SerialImpl::setDTR (bool level) -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::setDTR"); - } - if (level) { - EscapeCommFunction (fd_, SETDTR); - } else { - EscapeCommFunction (fd_, CLRDTR); - } -} - -bool -Serial::SerialImpl::waitForChange () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::waitForChange"); - } - DWORD dwCommEvent; - - if (!SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) { - // Error setting communications mask - return false; - } - - if (!WaitCommEvent(fd_, &dwCommEvent, NULL)) { - // An error occurred waiting for the event. - return false; - } else { - // Event has occurred. - return true; - } -} - -bool -Serial::SerialImpl::getCTS () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getCTS"); - } - DWORD dwModemStatus; - if (!GetCommModemStatus(fd_, &dwModemStatus)) { - THROW (IOException, "Error getting the status of the CTS line."); - } - - return (MS_CTS_ON & dwModemStatus) != 0; -} - -bool -Serial::SerialImpl::getDSR () -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getDSR"); - } - DWORD dwModemStatus; - if (!GetCommModemStatus(fd_, &dwModemStatus)) { - THROW (IOException, "Error getting the status of the DSR line."); - } - - return (MS_DSR_ON & dwModemStatus) != 0; -} - -bool -Serial::SerialImpl::getRI() -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getRI"); - } - DWORD dwModemStatus; - if (!GetCommModemStatus(fd_, &dwModemStatus)) { - THROW (IOException, "Error getting the status of the RI line."); - } - - return (MS_RING_ON & dwModemStatus) != 0; -} - -bool -Serial::SerialImpl::getCD() -{ - if (is_open_ == false) { - throw PortNotOpenedException ("Serial::getCD"); - } - DWORD dwModemStatus; - if (!GetCommModemStatus(fd_, &dwModemStatus)) { - // Error in GetCommModemStatus; - THROW (IOException, "Error getting the status of the CD line."); - } - - return (MS_RLSD_ON & dwModemStatus) != 0; -} - -void -Serial::SerialImpl::readLock() -{ - if (WaitForSingleObject(read_mutex, INFINITE) != WAIT_OBJECT_0) { - THROW (IOException, "Error claiming read mutex."); - } -} - -void -Serial::SerialImpl::readUnlock() -{ - if (!ReleaseMutex(read_mutex)) { - THROW (IOException, "Error releasing read mutex."); - } -} - -void -Serial::SerialImpl::writeLock() -{ - if (WaitForSingleObject(write_mutex, INFINITE) != WAIT_OBJECT_0) { - THROW (IOException, "Error claiming write mutex."); - } -} - -void -Serial::SerialImpl::writeUnlock() -{ - if (!ReleaseMutex(write_mutex)) { - THROW (IOException, "Error releasing write mutex."); - } -} - -#endif // #if defined(_WIN32) - diff --git a/src/perception/pbox_node_dirve/src/serial/src/serial.cc b/src/perception/pbox_node_dirve/src/serial/src/serial.cc deleted file mode 100644 index 37b5d70..0000000 --- a/src/perception/pbox_node_dirve/src/serial/src/serial.cc +++ /dev/null @@ -1,430 +0,0 @@ -/* Copyright 2012 William Woodall and John Harrison */ -#include - -#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__) -# include -#endif - -#if defined (__MINGW32__) -# define alloca __builtin_alloca -#endif - -#include "serial/serial.h" - -#ifdef _WIN32 -#include "serial/impl/win.h" -#else -#include "serial/impl/unix.h" -#endif - -using std::invalid_argument; -using std::min; -using std::numeric_limits; -using std::vector; -using std::size_t; -using std::string; - -using serial::Serial; -using serial::SerialException; -using serial::IOException; -using serial::bytesize_t; -using serial::parity_t; -using serial::stopbits_t; -using serial::flowcontrol_t; - -class Serial::ScopedReadLock { -public: - ScopedReadLock(SerialImpl *pimpl) : pimpl_(pimpl) { - this->pimpl_->readLock(); - } - ~ScopedReadLock() { - this->pimpl_->readUnlock(); - } -private: - // Disable copy constructors - ScopedReadLock(const ScopedReadLock&); - const ScopedReadLock& operator=(ScopedReadLock); - - SerialImpl *pimpl_; -}; - -class Serial::ScopedWriteLock { -public: - ScopedWriteLock(SerialImpl *pimpl) : pimpl_(pimpl) { - this->pimpl_->writeLock(); - } - ~ScopedWriteLock() { - this->pimpl_->writeUnlock(); - } -private: - // Disable copy constructors - ScopedWriteLock(const ScopedWriteLock&); - const ScopedWriteLock& operator=(ScopedWriteLock); - SerialImpl *pimpl_; -}; - -Serial::Serial (const string &port, uint32_t baudrate, serial::Timeout timeout, - bytesize_t bytesize, parity_t parity, stopbits_t stopbits, - flowcontrol_t flowcontrol) - : pimpl_(new SerialImpl (port, baudrate, bytesize, parity, - stopbits, flowcontrol)) -{ - pimpl_->setTimeout(timeout); -} - -Serial::~Serial () -{ - delete pimpl_; -} - -void -Serial::open () -{ - pimpl_->open (); -} - -void -Serial::close () -{ - pimpl_->close (); -} - -bool -Serial::isOpen () const -{ - return pimpl_->isOpen (); -} - -size_t -Serial::available () -{ - return pimpl_->available (); -} - -bool -Serial::waitReadable () -{ - serial::Timeout timeout(pimpl_->getTimeout ()); - return pimpl_->waitReadable(timeout.read_timeout_constant); -} - -void -Serial::waitByteTimes (size_t count) -{ - pimpl_->waitByteTimes(count); -} - -size_t -Serial::read_ (uint8_t *buffer, size_t size) -{ - return this->pimpl_->read (buffer, size); -} - -size_t -Serial::read (uint8_t *buffer, size_t size) -{ - ScopedReadLock lock(this->pimpl_); - return this->pimpl_->read (buffer, size); -} - -size_t -Serial::read (std::vector &buffer, size_t size) -{ - ScopedReadLock lock(this->pimpl_); - uint8_t *buffer_ = new uint8_t[size]; - size_t bytes_read = 0; - - try { - bytes_read = this->pimpl_->read (buffer_, size); - } - catch (const std::exception &e) { - delete[] buffer_; - throw; - } - - buffer.insert (buffer.end (), buffer_, buffer_+bytes_read); - delete[] buffer_; - return bytes_read; -} - -size_t -Serial::read (std::string &buffer, size_t size) -{ - ScopedReadLock lock(this->pimpl_); - uint8_t *buffer_ = new uint8_t[size]; - size_t bytes_read = 0; - try { - bytes_read = this->pimpl_->read (buffer_, size); - } - catch (const std::exception &e) { - delete[] buffer_; - throw; - } - buffer.append (reinterpret_cast(buffer_), bytes_read); - delete[] buffer_; - return bytes_read; -} - -string -Serial::read (size_t size) -{ - std::string buffer; - this->read (buffer, size); - return buffer; -} - -size_t -Serial::readline (string &buffer, size_t size, string eol) -{ - ScopedReadLock lock(this->pimpl_); - size_t eol_len = eol.length (); - uint8_t *buffer_ = static_cast - (alloca (size * sizeof (uint8_t))); - size_t read_so_far = 0; - while (true) - { - size_t bytes_read = this->read_ (buffer_ + read_so_far, 1); - read_so_far += bytes_read; - if (bytes_read == 0) { - break; // Timeout occured on reading 1 byte - } - if (string (reinterpret_cast - (buffer_ + read_so_far - eol_len), eol_len) == eol) { - break; // EOL found - } - if (read_so_far == size) { - break; // Reached the maximum read length - } - } - buffer.append(reinterpret_cast (buffer_), read_so_far); - return read_so_far; -} - -string -Serial::readline (size_t size, string eol) -{ - std::string buffer; - this->readline (buffer, size, eol); - return buffer; -} - -vector -Serial::readlines (size_t size, string eol) -{ - ScopedReadLock lock(this->pimpl_); - std::vector lines; - size_t eol_len = eol.length (); - uint8_t *buffer_ = static_cast - (alloca (size * sizeof (uint8_t))); - size_t read_so_far = 0; - size_t start_of_line = 0; - while (read_so_far < size) { - size_t bytes_read = this->read_ (buffer_+read_so_far, 1); - read_so_far += bytes_read; - if (bytes_read == 0) { - if (start_of_line != read_so_far) { - lines.push_back ( - string (reinterpret_cast (buffer_ + start_of_line), - read_so_far - start_of_line)); - } - break; // Timeout occured on reading 1 byte - } - if (string (reinterpret_cast - (buffer_ + read_so_far - eol_len), eol_len) == eol) { - // EOL found - lines.push_back( - string(reinterpret_cast (buffer_ + start_of_line), - read_so_far - start_of_line)); - start_of_line = read_so_far; - } - if (read_so_far == size) { - if (start_of_line != read_so_far) { - lines.push_back( - string(reinterpret_cast (buffer_ + start_of_line), - read_so_far - start_of_line)); - } - break; // Reached the maximum read length - } - } - return lines; -} - -size_t -Serial::write (const string &data) -{ - ScopedWriteLock lock(this->pimpl_); - return this->write_ (reinterpret_cast(data.c_str()), - data.length()); -} - -size_t -Serial::write (const std::vector &data) -{ - ScopedWriteLock lock(this->pimpl_); - return this->write_ (&data[0], data.size()); -} - -size_t -Serial::write (const uint8_t *data, size_t size) -{ - ScopedWriteLock lock(this->pimpl_); - return this->write_(data, size); -} - -size_t -Serial::write_ (const uint8_t *data, size_t length) -{ - return pimpl_->write (data, length); -} - -void -Serial::setPort (const string &port) -{ - ScopedReadLock rlock(this->pimpl_); - ScopedWriteLock wlock(this->pimpl_); - bool was_open = pimpl_->isOpen (); - if (was_open) close(); - pimpl_->setPort (port); - if (was_open) open (); -} - -string -Serial::getPort () const -{ - return pimpl_->getPort (); -} - -void -Serial::setTimeout (serial::Timeout &timeout) -{ - pimpl_->setTimeout (timeout); -} - -serial::Timeout -Serial::getTimeout () const { - return pimpl_->getTimeout (); -} - -void -Serial::setBaudrate (uint32_t baudrate) -{ - pimpl_->setBaudrate (baudrate); -} - -uint32_t -Serial::getBaudrate () const -{ - return uint32_t(pimpl_->getBaudrate ()); -} - -void -Serial::setBytesize (bytesize_t bytesize) -{ - pimpl_->setBytesize (bytesize); -} - -bytesize_t -Serial::getBytesize () const -{ - return pimpl_->getBytesize (); -} - -void -Serial::setParity (parity_t parity) -{ - pimpl_->setParity (parity); -} - -parity_t -Serial::getParity () const -{ - return pimpl_->getParity (); -} - -void -Serial::setStopbits (stopbits_t stopbits) -{ - pimpl_->setStopbits (stopbits); -} - -stopbits_t -Serial::getStopbits () const -{ - return pimpl_->getStopbits (); -} - -void -Serial::setFlowcontrol (flowcontrol_t flowcontrol) -{ - pimpl_->setFlowcontrol (flowcontrol); -} - -flowcontrol_t -Serial::getFlowcontrol () const -{ - return pimpl_->getFlowcontrol (); -} - -void Serial::flush () -{ - ScopedReadLock rlock(this->pimpl_); - ScopedWriteLock wlock(this->pimpl_); - pimpl_->flush (); -} - -void Serial::flushInput () -{ - ScopedReadLock lock(this->pimpl_); - pimpl_->flushInput (); -} - -void Serial::flushOutput () -{ - ScopedWriteLock lock(this->pimpl_); - pimpl_->flushOutput (); -} - -void Serial::sendBreak (int duration) -{ - pimpl_->sendBreak (duration); -} - -void Serial::setBreak (bool level) -{ - pimpl_->setBreak (level); -} - -void Serial::setRTS (bool level) -{ - pimpl_->setRTS (level); -} - -void Serial::setDTR (bool level) -{ - pimpl_->setDTR (level); -} - -bool Serial::waitForChange() -{ - return pimpl_->waitForChange(); -} - -bool Serial::getCTS () -{ - return pimpl_->getCTS (); -} - -bool Serial::getDSR () -{ - return pimpl_->getDSR (); -} - -bool Serial::getRI () -{ - return pimpl_->getRI (); -} - -bool Serial::getCD () -{ - return pimpl_->getCD (); -} diff --git a/src/perception/pbox_node_dirve/test.csv b/src/perception/pbox_node_dirve/test.csv deleted file mode 100644 index e69de29..0000000 diff --git a/src/perception/pbox_node_dirve/编译配置说明.md b/src/perception/pbox_node_dirve/编译配置说明.md deleted file mode 100644 index e46f1e2..0000000 --- a/src/perception/pbox_node_dirve/编译配置说明.md +++ /dev/null @@ -1,54 +0,0 @@ -## pbox_node_dirve ROS2 使用方法 - -### 编译 - -```bash -# 进入项目目录 -cd ~/project/zxwl/sweeper/sweeper_200 - -# 设置ROS2环境 -source /opt/ros/humble/setup.bash - -# 编译 -colcon build --packages-select pbox_node_dirve - -# 加载编译结果 -source install/setup.bash -``` - -### 运行 - -```bash -# 终端1：启动节点 -ros2 launch pbox_node pbox_node.launch.py - -# 终端2：查看话题 -ros2 topic list -ros2 topic echo /Imu04 -``` - -### 配置参数 - -编辑 `src/pbox_node/launch/pbox_node.launch.py`： - -| 参数 | 说明 | 默认值 | -| ------------------ | ---------------------- | ------------- | -| `ConnectionType` | 0=串口, 1=UDP | 1 | -| `UART_Port` | 串口设备 | /dev/ttyTHS1 | -| `UART_Baudrate` | 波特率 | 230400 | -| `UDP_Addr` | UDP地址 | 192.168.225.2 | -| `UDP_Port` | UDP端口 | 12300 | -| `MsgType` | 0=标准msg, 1=自定义msg | 1 | - -### 可用话题 - -* `/Imu04` - IMU数据 (BDDB04) -* `/Imu0A` - IMU数据 (BDDB0A) -* `/Imu8B` - IMU数据 (BDDB8B) -* `/Ins` - 组合导航数据 (BDDB0B) -* `/Gnss` - GNSS数据 (BDDB10) -* `/Odom` - 里程计数据 (BDDB1B) - ---- - -**修改内容总结：** diff --git a/src/perception/rtk_asensing/4 数据输出协议.md b/src/perception/rtk_asensing/4 数据输出协议.md deleted file mode 100644 index 4af4e34..0000000 --- a/src/perception/rtk_asensing/4 数据输出协议.md +++ /dev/null @@ -1,70 +0,0 @@ -## 4 数据输出协议 - -### 4.1 Asensing 协议输出 - -#### 4.1.1 BD DB 0B 组合导航 INS 数据 - -- 帧头：0xbd 0xdb -- 帧类型：0x0B（组合导航模式，58字节，包含姿态、角速率、加速度、温度、经纬度、速度等） -- 配置指令：`AGBDDB0B,[输出串口],[发送参数],[输出频率]` -- 返回指令：`AGBDDB0B,[输出串口],[发送参数],[输出频率],OK` - -**参数说明** - -| 参数定义 | 取值范围 | 说明 | -| -------- | -------- | ------------------------------- | -| 输出串口 | 0 | 从 UART0 输出，当前仅支持 UART0 | -| 输出串口 | 3 | 从 UART3 输出（可选） | -| 发送参数 | 0 | 关闭数据输出 | -| 发送参数 | 1 | 打开数据输出 | -| 输出频率 | 20 | 20Hz 播发（50ms 1次） | -| 输出频率 | 50 | 50Hz 播发（20ms 1次） | -| 输出频率 | 100 | 100Hz 播发（10ms 1次） | -| 输出频率 | 200 | 200Hz 播发（5ms 1次） | - -**输出格式** - -| 偏移 | 定义 | 格式 | 长度 | 系数 | 单位 | 字节序 | 说明 | -| ----- | -------- | ---- | ---- | --------- | ----- | --------- | ---------------------------------------------------------- | -| 0 | 0xBD | - | 1 | - | - | - | 帧头第一位 | -| 1 | 0xDB | - | 1 | - | - | - | 帧头第二位 | -| 2 | 0x0B | - | 1 | - | - | - | 帧类型（组合导航模式） | -| 3 | 横滚角 | I16 | 2 | 360/32768 | deg | LSB_first | 取值范围[-90,90] | -| 5 | 俯仰角 | I16 | 2 | 360/32768 | deg | LSB_first | 取值范围[-180,180] | -| 7 | 方位角 | I16 | 2 | 360/32768 | deg | LSB_first | [-180,180]，正北顺时针到正南为正0-180° | -| 9 | 陀螺x轴 | I16 | 2 | 300/32768 | deg/s | LSB_first | 取值范围[-250,250] | -| 11 | 陀螺y轴 | I16 | 2 | 300/32768 | deg/s | LSB_first | 取值范围[-250,250] | -| 13 | 陀螺z轴 | I16 | 2 | 300/32768 | deg/s | LSB_first | 取值范围[-250,250] | -| 15 | 加表x轴 | I16 | 2 | 12/32768 | g | LSB_first | 取值范围[-4,4]，与量程有关（如4g） | -| 17 | 加表y轴 | I16 | 2 | 12/32768 | g | LSB_first | 取值范围[-4,4]，与量程有关（如4g） | -| 19 | 加表z轴 | I16 | 2 | 12/32768 | g | LSB_first | 取值范围[-4,4]，与量程有关（如4g） | -| 21 | 纬度 | I32 | 4 | 1.00E-07 | deg | LSB_first | [-90,90]，正为N | -| 25 | 经度 | I32 | 4 | 1.00E-07 | deg | LSB_first | [-180,180]，正为E | -| 29 | 高度 | I32 | 4 | 1.00E-03 | m | LSB_first | 海拔高度 | -| 33 | 北向速度 | I16 | 2 | 1e2/32768 | m/s | LSB_first | 北向速度分量 | -| 35 | 东向速度 | I16 | 2 | 1e2/32768 | m/s | LSB_first | 东向速度分量 | -| 37 | 地向速度 | I16 | 2 | 1e2/32768 | m/s | LSB_first | 地向速度分量 | -| 39 | 状态 | U8 | 1 | - | - | - | bit0=位置，bit1=速度，bit2=姿态，bit3=航向角；1=完成初对准 | -| 40-45 | 保留 | - | 6 | - | - | - | 预留字段 | -| 46 | Data1 | I16 | 2 | - | - | LSB_first | 轮循数据，参考轮询数据表 | -| 48 | Data2 | I16 | 2 | - | - | LSB_first | 轮循数据，参考轮询数据表 | -| 50 | Data3 | I16 | 2 | - | - | LSB_first | 轮循数据，参考轮询数据表 | -| 52 | 时间 | U32 | 4 | 2.50E-04 | 秒 | LSB_first | GPS时间（周内秒），毫秒精度 | -| 56 | Type | U8 | 1 | - | - | - | 轮循数据类型，参考轮询数据表 | -| 57 | 校验位 | U8 | 1 | - | - | - | 异或校验，包含0~56的数据 | -| 58 | GPS周 | U32 | 4 | 1 | - | LSB_first | GPS周数 | -| 62 | 校验位 | U8 | 1 | - | - | - | 异或校验，包含0~61的数据 | - -**轮询数据表** - -| 偏移 | 名称 | 单位 | Data1 | Data2 | Data3 | 计算公式及备注 | -| ---- | ------------ | ---- | -------------- | -------------- | ------------- | ------------------------------ | -| 0 | 定位信息精度 | m | latstd（纬度） | lonstd（经度） | hstd（海拔） | 精度=e^(data/100)，e取2.718281 | -| 1 | 定速信息精度 | m/s | vnstd（北向） | vestd（东向） | vdstd（地向） | 精度=e^(data/100)，e取2.718281 | -| 2 | 姿态信息精度 | deg | Rollstd | Pitchstd | Yawstd | 精度=e^(data/100)，e取2.718281 | -| 22 | 设备内部温度 | ℃ | 温度 | - | - | 温度=数据×(200/32768) | -| 32 | GNSS状态 | - | 定位状态 | 收星数 | 定向状态 | 位置状态参考附录一 | -| 33 | 轮速状态 | - | - | 轮速有无标志 | - | Data2≠0=有轮速，连续=0=无轮速 | -| 49 | IMU状态 | - | 故障标志位 | - | - | Data1=1=故障，0=正常 | - -#### \ No newline at end of file diff --git a/src/perception/rtk_asensing/CMakeLists.txt b/src/perception/rtk_asensing/CMakeLists.txt deleted file mode 100644 index 9698d0f..0000000 --- a/src/perception/rtk_asensing/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtk_asensing) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sweeper_interfaces REQUIRED) -find_package(logger REQUIRED) -find_package(imu_msgs REQUIRED) - -include_directories(include/rtk_asensing) - -add_executable(rtk_asensing_node src/rtk_asensing_node.cpp) -ament_target_dependencies( - rtk_asensing_node - rclcpp - std_msgs - sweeper_interfaces - logger - imu_msgs) - -install(TARGETS rtk_asensing_node DESTINATION lib/${PROJECT_NAME}) - -# 安装启动文件 -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp deleted file mode 100644 index 090d480..0000000 --- a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp +++ /dev/null @@ -1,403 +0,0 @@ -#ifndef __ASENSING_PROTOCOL_H__ -#define __ASENSING_PROTOCOL_H__ - -#include -#include -#include -#include - -#include "logger/logger.h" - -// Asensing协议4数据帧定义 -// 帧头: 0xBD 0xDB -// 帧类型: 0x0B (组合导航模式，58字节数据) -// 数据长度: 58字节 (不含帧头3字节) - -#pragma pack(push, 1) // 1字节对齐，避免结构体填充 - -// Asensing协议4组合导航数据帧 -struct AsensingInsData -{ - // 原始帧数据 - uint8_t frame_header[2]; // 偏移0-1: 帧头 0xBD 0xDB - uint8_t frame_type; // 偏移2: 帧类型 0x0B - int16_t roll; // 偏移3-4: 横滚角, 系数 360/32768, 单位 deg - int16_t pitch; // 偏移5-6: 俯仰角, 系数 360/32768, 单位 deg - int16_t yaw; // 偏移7-8: 方位角, 系数 360/32768, 单位 deg - int16_t gyro_x; // 偏移9-10: 陀螺x轴, 系数 300/32768, 单位 deg/s - int16_t gyro_y; // 偏移11-12: 陀螺y轴, 系数 300/32768, 单位 deg/s - int16_t gyro_z; // 偏移13-14: 陀螺z轴, 系数 300/32768, 单位 deg/s - int16_t accel_x; // 偏移15-16: 加表x轴, 系数 12/32768, 单位 g - int16_t accel_y; // 偏移17-18: 加表y轴, 系数 12/32768, 单位 g - int16_t accel_z; // 偏移19-20: 加表z轴, 系数 12/32768, 单位 g - int32_t latitude; // 偏移21-24: 纬度, 系数 1e-7, 单位 deg - int32_t longitude; // 偏移25-28: 经度, 系数 1e-7, 单位 deg - int32_t altitude; // 偏移29-32: 高度, 系数 1e-3, 单位 m - int16_t vel_north; // 偏移33-34: 北向速度, 系数 1e2/32768, 单位 m/s - int16_t vel_east; // 偏移35-36: 东向速度, 系数 1e2/32768, 单位 m/s - int16_t vel_down; // 偏移37-38: 地向速度, 系数 1e2/32768, 单位 m/s - uint8_t status; // 偏移39: 状态位 - uint8_t reserved[6]; // 偏移40-45: 保留字段 - int16_t data1; // 偏移46-47: 轮循数据1 - int16_t data2; // 偏移48-49: 轮循数据2 - int16_t data3; // 偏移50-51: 轮循数据3 - uint32_t gps_time; // 偏移52-55: GPS时间（周内秒）, 系数 2.5e-4, 单位 秒 - uint8_t pollute_type; // 偏移56: 轮循数据类型 - uint8_t check1; // 偏移57: 校验位1 (异或校验，包含0-57的数据) - - // 解析后的轮循数据 - uint32_t gps_week; // GPS周数 - double gps_time_ms; // GPS时间（毫秒） - double temperature; // 温度 (℃) - uint8_t position_type; // 定位状态 - uint8_t numsv; // 收星数 - uint8_t heading_type; // 定向状态 - uint8_t wheel_speed_status; // 轮速状态 - - // 精度数据 - double latitude_std; // 纬度标准差 - double longitude_std; // 经度标准差 - double altitude_std; // 高度标准差 - double north_velocity_std; // 北向速度标准差 - double east_velocity_std; // 东向速度标准差 - double ground_velocity_std; // 地向速度标准差 - double roll_std; // 横滚角标准差 - double pitch_std; // 俯仰角标准差 - double azimuth_std; // 方位角标准差 -}; - -#pragma pack(pop) - -// 从字节流中读取指定类型的数据 (小端序) -template -T toValue(const uint8_t* data, int& index) -{ - T result = *(T*)(data + index); - index += sizeof(T); - return result; -} - -// Asensing协议数据解析类 -class AsensingProtocol -{ -public: - AsensingProtocol() : is_synced_(false), frame_index_(0), m_lengthImu(58) {} - - // 重置解析状态 - void reset() - { - is_synced_ = false; - frame_index_ = 0; - memset(&raw_data_, 0, sizeof(raw_data_)); - } - - // 尝试从串口数据中解析出一帧完整的数据 - // 返回值: 1=成功解析一帧, 0=数据不足, -1=校验失败 - int feedData(const uint8_t* data, int len) - { - for (int i = 0; i < len; i++) - { - if (!is_synced_) - { - // 寻找帧头 0xBD 0xDB - if (frame_index_ == 0 && data[i] == 0xBD) - { - frame_buf_[frame_index_++] = data[i]; - } - else if (frame_index_ == 1 && data[i] == 0xDB) - { - frame_buf_[frame_index_++] = data[i]; - LOG_INFO("[DEBUG-PROTO] 找到帧头 BD DB"); - } - else if (frame_index_ == 1 && data[i] != 0xDB) - { - frame_index_ = 0; - if (data[i] == 0xBD) - { - frame_buf_[frame_index_++] = data[i]; - } - } - else - { - frame_index_ = 0; - } - - // 帧头匹配成功，继续接收后续数据 - if (frame_index_ == 2) - { - is_synced_ = true; - } - } - else - { - // 已同步帧头，继续接收数据 - frame_buf_[frame_index_++] = data[i]; - - // 完整帧长 = 帧头2字节 + 类型1字节 + 数据58字节 + 校验1字节 = 62字节 - // 实际数据到偏移57是check1(1字节) - // 所以完整帧长 = 62字节 - if (frame_index_ >= 62) - { - // 打印帧数据用于调试 - LOG_INFO("[DEBUG-PROTO] 收到帧数据, 帧类型: 0x%02X", frame_buf_[2]); - - // 检查帧类型 - if (frame_buf_[2] != 0x0B) - { - LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B)", frame_buf_[2]); - // 帧类型不匹配，重新同步 - is_synced_ = false; - frame_index_ = 0; - continue; - } - - // 验证校验位1 (异或校验，包含0-57的数据，共58字节) - uint8_t xor_check = 0; - for (int j = 0; j < m_lengthImu; j++) - { - xor_check ^= frame_buf_[j]; - } - - if (xor_check != frame_buf_[m_lengthImu]) - { - LOG_INFO("[DEBUG-PROTO] 校验失败: 计算=0x%02X, 接收=0x%02X", xor_check, frame_buf_[m_lengthImu]); - // 校验失败，重新同步 - is_synced_ = false; - frame_index_ = 0; - continue; - } - - LOG_INFO("[DEBUG-PROTO] 校验通过! 开始解析数据"); - - // 校验通过，解析数据 - int sub_index = 3; - - // 横滚角、俯仰角、方位角 - raw_data_.roll = toValue(frame_buf_, sub_index); - raw_data_.pitch = toValue(frame_buf_, sub_index); - raw_data_.yaw = toValue(frame_buf_, sub_index); - - // 陀螺 - raw_data_.gyro_x = toValue(frame_buf_, sub_index); - raw_data_.gyro_y = toValue(frame_buf_, sub_index); - raw_data_.gyro_z = toValue(frame_buf_, sub_index); - - // 加速度 - raw_data_.accel_x = toValue(frame_buf_, sub_index); - raw_data_.accel_y = toValue(frame_buf_, sub_index); - raw_data_.accel_z = toValue(frame_buf_, sub_index); - - // 经纬度、高度 - raw_data_.latitude = toValue(frame_buf_, sub_index); - raw_data_.longitude = toValue(frame_buf_, sub_index); - raw_data_.altitude = toValue(frame_buf_, sub_index); - - // 速度 - raw_data_.vel_north = toValue(frame_buf_, sub_index); - raw_data_.vel_east = toValue(frame_buf_, sub_index); - raw_data_.vel_down = toValue(frame_buf_, sub_index); - - // 状态 - raw_data_.status = frame_buf_[sub_index++]; - - // 保留字段 - sub_index += 4; - sub_index += 2; - - // 轮循数据 - int16_t temp[3]; - temp[0] = toValue(frame_buf_, sub_index); - temp[1] = toValue(frame_buf_, sub_index); - temp[2] = toValue(frame_buf_, sub_index); - - // GPS时间 - raw_data_.gps_time = toValue(frame_buf_, sub_index); - raw_data_.gps_time_ms = raw_data_.gps_time / 4000.0; - - // 轮循数据类型 - raw_data_.pollute_type = frame_buf_[sub_index++]; - - // 处理轮循数据 - processPollData(temp, raw_data_.pollute_type); - - // 校验位1 - raw_data_.check1 = frame_buf_[sub_index++]; - - // GPS周数 - raw_data_.gps_week = toValue(frame_buf_, sub_index); - - // 重置解析状态 - is_synced_ = false; - frame_index_ = 0; - - return 1; // 成功解析一帧 - } - } - } - - return 0; // 数据不足 - } - - // 获取解析后的数据 - const AsensingInsData& getData() const { return raw_data_; } - - // 获取解析后的物理量（已转换单位） - - // 获取横滚角 (deg) - double getRoll() const { return static_cast(raw_data_.roll) * 360.0 / 32768.0; } - - // 获取俯仰角 (deg) - double getPitch() const { return static_cast(raw_data_.pitch) * 360.0 / 32768.0; } - - // 获取方位角 (deg) - double getYaw() const { return static_cast(raw_data_.yaw) * 360.0 / 32768.0; } - - // 获取陀螺x轴 (deg/s) - double getGyroX() const { return static_cast(raw_data_.gyro_x) * 300.0 / 32768.0; } - - // 获取陀螺y轴 (deg/s) - double getGyroY() const { return static_cast(raw_data_.gyro_y) * 300.0 / 32768.0; } - - // 获取陀螺z轴 (deg/s) - double getGyroZ() const { return static_cast(raw_data_.gyro_z) * 300.0 / 32768.0; } - - // 获取加速度x轴 (g) - double getAccelX() const { return static_cast(raw_data_.accel_x) * 12.0 / 32768.0; } - - // 获取加速度y轴 (g) - double getAccelY() const { return static_cast(raw_data_.accel_y) * 12.0 / 32768.0; } - - // 获取加速度z轴 (g) - double getAccelZ() const { return static_cast(raw_data_.accel_z) * 12.0 / 32768.0; } - - // 获取纬度 (deg) - double getLatitude() const { return static_cast(raw_data_.latitude) * 1e-7; } - - // 获取经度 (deg) - double getLongitude() const { return static_cast(raw_data_.longitude) * 1e-7; } - - // 获取高度 (m) - double getAltitude() const { return static_cast(raw_data_.altitude) * 1e-3; } - - // 获取北向速度 (m/s) - double getVelNorth() const { return static_cast(raw_data_.vel_north) * 100.0 / 32768.0; } - - // 获取东向速度 (m/s) - double getVelEast() const { return static_cast(raw_data_.vel_east) * 100.0 / 32768.0; } - - // 获取地向速度 (m/s) - double getVelDown() const { return static_cast(raw_data_.vel_down) * 100.0 / 32768.0; } - - // 获取地面速度 (m/s) - double getGroundSpeed() const - { - double vn = getVelNorth(); - double ve = getVelEast(); - return std::sqrt(vn * vn + ve * ve); - } - - // 获取合速度 (m/s) - double getTotalSpeed() const - { - double vn = getVelNorth(); - double ve = getVelEast(); - double vd = getVelDown(); - return std::sqrt(vn * vn + ve * ve + vd * vd); - } - - // 获取GPS时间（周内秒）(s) - double getGpsTime() const { return raw_data_.gps_time_ms; } - - // 获取GPS周数 - uint32_t getGpsWeek() const { return raw_data_.gps_week; } - - // 获取定位状态 - // bit0=位置, bit1=速度, bit2=姿态, bit3=航向角; 1=完成初对准 - uint8_t getStatus() const { return raw_data_.status; } - - // 获取位置定位状态 - bool isPositionValid() const { return (raw_data_.status & 0x01) != 0; } - - // 获取速度定位状态 - bool isVelocityValid() const { return (raw_data_.status & 0x02) != 0; } - - // 获取姿态定位状态 - bool isAttitudeValid() const { return (raw_data_.status & 0x04) != 0; } - - // 获取航向角定位状态 - bool isHeadingValid() const { return (raw_data_.status & 0x08) != 0; } - - // 获取轮循数据类型 - uint8_t getPollutionType() const { return raw_data_.pollute_type; } - - // 获取温度 (℃) - double getTemperature() const { return raw_data_.temperature; } - - // 获取定位状态 - uint8_t getPositionType() const { return raw_data_.position_type; } - - // 获取收星数 - uint8_t getSatCount() const { return raw_data_.numsv; } - - // 获取定向状态 - uint8_t getHeadingType() const { return raw_data_.heading_type; } - - // 获取轮速状态 - uint8_t getWheelSpeedStatus() const { return raw_data_.wheel_speed_status; } - - // 获取IMU状态 - bool isImuNormal() const - { - if (raw_data_.pollute_type == 49) - { - return (raw_data_.data1 & 0xFF) == 0; - } - return true; // 默认正常 - } - -private: - // 处理轮循数据 - void processPollData(int16_t temp[3], uint8_t type) - { - switch (type) - { - case 0: // 定位精度 - raw_data_.latitude_std = exp(temp[0] / 100.0); - raw_data_.longitude_std = exp(temp[1] / 100.0); - raw_data_.altitude_std = exp(temp[2] / 100.0); - break; - case 1: // 定速精度 - raw_data_.north_velocity_std = exp(temp[0] / 100.0); - raw_data_.east_velocity_std = exp(temp[1] / 100.0); - raw_data_.ground_velocity_std = exp(temp[2] / 100.0); - break; - case 2: // 姿态精度 - raw_data_.roll_std = exp(temp[0] / 100.0); - raw_data_.pitch_std = exp(temp[1] / 100.0); - raw_data_.azimuth_std = exp(temp[2] / 100.0); - break; - case 22: // 温度 - raw_data_.temperature = temp[0] * 200.0 / 32768.0; - break; - case 32: // GNSS状态 - raw_data_.position_type = static_cast(temp[0] & 0xFF); - raw_data_.numsv = static_cast((temp[1] >> 8) & 0xFF); - raw_data_.heading_type = static_cast(temp[2] & 0xFF); - break; - case 33: // 轮速状态 - raw_data_.wheel_speed_status = static_cast((temp[1] >> 8) & 0xFF); - break; - default: - break; - } - } - -private: - AsensingInsData raw_data_; // 解析后的数据 - uint8_t frame_buf_[128]; // 帧缓冲区（足够大） - bool is_synced_; // 帧同步状态 - int frame_index_; // 当前帧索引 - int m_lengthImu; // 数据长度（不含帧头） -}; - -#endif diff --git a/src/perception/rtk_asensing/launch/rtk_asensing.launch.py b/src/perception/rtk_asensing/launch/rtk_asensing.launch.py deleted file mode 100644 index a4ee952..0000000 --- a/src/perception/rtk_asensing/launch/rtk_asensing.launch.py +++ /dev/null @@ -1,15 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription( - [ - Node( - package="rtk_asensing", - executable="rtk_asensing_node", - name="rtk_asensing_node", - output="screen", - ), - ] - ) diff --git a/src/perception/rtk_asensing/package.xml b/src/perception/rtk_asensing/package.xml deleted file mode 100644 index c254fbd..0000000 --- a/src/perception/rtk_asensing/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - rtk_asensing - 0.0.0 - Asensing RTK protocol 4 data parser - zxwl - TODO: License declaration - - ament_cmake - - rclcpp - std_msgs - sweeper_interfaces - logger - - ament_lint_auto - ament_lint_common - - - ament_cmake - - \ No newline at end of file diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp deleted file mode 100644 index fee895e..0000000 --- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#include -#include - -#include - -#include "asensing_protocol.hpp" -#include "imu_msgs/msg/gnss.hpp" -#include "logger/logger.h" -#include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/rtk.hpp" - -class RtkAsensingNode : public rclcpp::Node -{ - public: - RtkAsensingNode(std::string name) : Node(name) - { - LOG_INFO("%s节点已经启动.", name.c_str()); - - // 创建发布者 - rtk_publisher_ = this->create_publisher("rtk_message", 10); - - // 订阅 /Gnss topic - gnss_sub_ = this->create_subscription( - "/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1)); - - LOG_INFO("已订阅 /Gnss topic"); - } - - ~RtkAsensingNode() {} - - private: - void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg) - { - try - { - auto rtk_msg = sweeper_interfaces::msg::Rtk(); - - // 从 /Gnss 消息中获取数据 - rtk_msg.lat = msg->latitude; - rtk_msg.lon = msg->longitude; - - // 航向角转换：将 yaw 转换为 0~360 度（正北0，东90，南180，西270） - double heading = msg->yaw; - if (heading < 0) - { - heading += 360.0; - } - rtk_msg.head = heading; - - // 速度 - rtk_msg.speed = std::sqrt(msg->hor_vel * msg->hor_vel + msg->ver_vel * msg->ver_vel); - - // 位置质量转换（使用 flags_pos 字段） - rtk_msg.p_quality = convertFixType(msg->flags_pos); - - // 定向质量转换（使用 flags_attitude 字段） - rtk_msg.h_quality = convertFixType(msg->flags_attitude); - - LOG_INFO("Asensing RTK: lat:%.9f, lon:%.9f, heading:%.2f, speed:%.2f, p_quality:%d, h_quality:%d", - rtk_msg.lat, rtk_msg.lon, rtk_msg.head, rtk_msg.speed, rtk_msg.p_quality, rtk_msg.h_quality); - - rtk_publisher_->publish(rtk_msg); - } - catch (const std::exception& e) - { - LOG_ERROR("gnss_callback异常：%s", e.what()); - } - } - - // 将 Asensing fix 类型转换为 GPGGA 格式 - uint8_t convertFixType(uint8_t asensing_fix) - { - // Asensing 类型定义: - // 0=无解, 1=位置固定, 8=多普勒速度, 16=单点定位, - // 17=伪距差分, 18=SBAS, 32=L1浮点, 34=窄巷浮点, - // 48=L1固定, 49=宽巷固定, 50=窄巷固定 - // - // GPGGA 格式: - // 0=无效, 1=GPS单点, 2=DGPS, 4=RTK固定, 5=RTK浮点 - if (asensing_fix == 0) - { - return 0; // 无解 -> 无效 - } - else if (asensing_fix == 1 || asensing_fix == 16) - { - return 1; // 位置固定/单点 -> GPS单点 - } - else if (asensing_fix == 17 || asensing_fix == 18 || asensing_fix == 8) - { - return 2; // 差分/SBAS/多普勒 -> DGPS - } - else if (asensing_fix == 32 || asensing_fix == 34) - { - return 5; // L1浮点/窄巷浮点 -> RTK浮点 - } - else if (asensing_fix == 48 || asensing_fix == 49 || asensing_fix == 50) - { - return 4; // L1固定/宽巷固定/窄巷固定 -> RTK固定 - } - else - { - return 0; // 其他视为无效 - } - } - - // ROS发布者 - rclcpp::Publisher::SharedPtr rtk_publisher_; - - // ROS订阅者 - rclcpp::Subscription::SharedPtr gnss_sub_; -}; - -int main(int argc, char** argv) -{ - // 初始化日志系统 - logger::Logger::Init("rtk_asensing", "./nodes_log"); - - rclcpp::init(argc, argv); - - // 创建节点 - auto node = std::make_shared("rtk_asensing_node"); - - // 运行节点 - rclcpp::spin(node); - - rclcpp::shutdown(); - - // 关闭日志系统 - logger::Logger::Shutdown(); - - return 0; -}