diff --git a/src/perception/pbox_node_dirve/.gitignore b/src/perception/pbox_node_dirve/.gitignore new file mode 100644 index 0000000..685f1f9 --- /dev/null +++ b/src/perception/pbox_node_dirve/.gitignore @@ -0,0 +1,16 @@ +# 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 new file mode 100644 index 0000000..3c690ec --- /dev/null +++ b/src/perception/pbox_node_dirve/README.md @@ -0,0 +1,55 @@ +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 new file mode 100644 index 0000000..cdcc569 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt @@ -0,0 +1,51 @@ +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 new file mode 100644 index 0000000..c0a7a5f --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg @@ -0,0 +1,53 @@ + +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 new file mode 100644 index 0000000..adf6643 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg @@ -0,0 +1,39 @@ +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 new file mode 100644 index 0000000..fad581e --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000..7cd41b5 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg @@ -0,0 +1,26 @@ +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 new file mode 100644 index 0000000..ae3e507 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg @@ -0,0 +1,12 @@ +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 new file mode 100644 index 0000000..c34639b --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg @@ -0,0 +1,14 @@ +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 new file mode 100644 index 0000000..649e3cc --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/ImuInitial.msg @@ -0,0 +1,10 @@ +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 new file mode 100644 index 0000000..0c4aa41 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg @@ -0,0 +1,45 @@ +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 new file mode 100644 index 0000000..80b7b79 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/imu_msgs/package.xml @@ -0,0 +1,23 @@ + + + + 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 new file mode 100644 index 0000000..d2f543b --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt @@ -0,0 +1,56 @@ +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 new file mode 100644 index 0000000..93f34bd --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/include/AGPbox.hpp @@ -0,0 +1,71 @@ +#ifndef _AG_Pbox_HPP_ +#define _AG_Pbox_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "protocol_asensing.h" +#include "TinyLog.h" +using namespace std; +using namespace std::chrono_literals; +using namespace serial; + + +#define UART_PORT "/dev/ttyUSB0" +#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 new file mode 100644 index 0000000..dd64b86 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/include/TinyLog.h @@ -0,0 +1,49 @@ + +#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 new file mode 100644 index 0000000..a34dcb0 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/include/common.h @@ -0,0 +1,250 @@ +#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 new file mode 100644 index 0000000..0b28675 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/include/protocol_asensing.h @@ -0,0 +1,53 @@ +#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 new file mode 100644 index 0000000..bee68b0 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/launch/pbox_node.launch.py @@ -0,0 +1,59 @@ +"""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": 1}, + + #串口设备串 defaule: /dev/ttyUSB0 + {"UART_Port": "/dev/ttyUSB0"}, + #串口波特率 default: 115200 + {"UART_Baudrate": 230400}, + #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]) \ No newline at end of file diff --git a/src/perception/pbox_node_dirve/src/pbox_node/package.xml b/src/perception/pbox_node_dirve/src/pbox_node/package.xml new file mode 100644 index 0000000..e48b46e --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/package.xml @@ -0,0 +1,25 @@ + + + + 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 new file mode 100644 index 0000000..5ba9d04 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp @@ -0,0 +1,224 @@ +#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); + timer_ = this->create_wall_timer(1ms, std::bind(&AGPbox::timer_callback, this)); + 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(); + } +} + +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(0); + imu_ser.setTimeout(to); + imu_ser.setFlowcontrol(serial::flowcontrol_t::flowcontrol_hardware); + 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) + { + // for(int i = 0; i < m_read.size();i++) + { + // fprintf(m_logFd , "%02X" , m_read.c_str()[i]); + 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 new file mode 100644 index 0000000..15b0b92 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp @@ -0,0 +1,245 @@ + +#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 new file mode 100644 index 0000000..9ce7f3b --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp @@ -0,0 +1,51 @@ +#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 new file mode 100644 index 0000000..3d8f804 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp @@ -0,0 +1,43 @@ +// #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 new file mode 100644 index 0000000..0438472 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_04.cpp @@ -0,0 +1,133 @@ +// #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) + { + + uint32_t dataSize = getDataSize() - index; + uint32_t protocolSize = checkXor(&sub_address[index] , dataSize); + if(protocolSize == 44) + { + 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.imu_time_stamp = (double)tmpU32 / 4000; + // status + m_pubAsensingMsg.status = toValue(sub_address, sub_index); + + /* 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 += protocolSize; + } + 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 new file mode 100644 index 0000000..3c4d59c --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0A.cpp @@ -0,0 +1,192 @@ +// #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 new file mode 100644 index 0000000..37748aa --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_0B.cpp @@ -0,0 +1,195 @@ +// #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 new file mode 100644 index 0000000..a4eef06 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_10.cpp @@ -0,0 +1,174 @@ +// #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 new file mode 100644 index 0000000..aaab889 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_1B.cpp @@ -0,0 +1,197 @@ +// #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 new file mode 100644 index 0000000..15ac80d --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol/decode_8B.cpp @@ -0,0 +1,173 @@ +// #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 new file mode 100644 index 0000000..649f68b --- /dev/null +++ b/src/perception/pbox_node_dirve/src/pbox_node/src/protocol_asensing.cpp @@ -0,0 +1,178 @@ +#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); +} +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 new file mode 100644 index 0000000..aa10d4d --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/CMakeLists.txt @@ -0,0 +1,65 @@ +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 new file mode 100644 index 0000000..e172072 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/Makefile @@ -0,0 +1,62 @@ +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 new file mode 100644 index 0000000..eb4b279 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/README.md @@ -0,0 +1,58 @@ +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 new file mode 100644 index 0000000..c884b65 Binary files /dev/null and b/src/perception/pbox_node_dirve/src/serial/README.pdf 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 new file mode 100644 index 0000000..90c9e02 Binary files /dev/null and b/src/perception/pbox_node_dirve/src/serial/images/install_Serial_ubuntu.png 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 new file mode 100644 index 0000000..0fb38f2 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/include/serial/impl/unix.h @@ -0,0 +1,221 @@ +/*! + * \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 new file mode 100644 index 0000000..2c0c6cd --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/include/serial/impl/win.h @@ -0,0 +1,207 @@ +/*! + * \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 new file mode 100644 index 0000000..a165785 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/include/serial/serial.h @@ -0,0 +1,775 @@ +/*! + * \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 new file mode 100644 index 0000000..f3be96b --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/include/serial/v8stdint.h @@ -0,0 +1,57 @@ +// 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 new file mode 100644 index 0000000..f1ac9d3 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/package.xml @@ -0,0 +1,30 @@ + + + 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 new file mode 100644 index 0000000..9779d5e --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_linux.cc @@ -0,0 +1,335 @@ +#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 new file mode 100644 index 0000000..333c55c --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_osx.cc @@ -0,0 +1,286 @@ +#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 new file mode 100644 index 0000000..7da40c4 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/impl/list_ports/list_ports_win.cc @@ -0,0 +1,152 @@ +#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 new file mode 100644 index 0000000..4309aa6 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc @@ -0,0 +1,1066 @@ +/* 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 new file mode 100644 index 0000000..786f4f6 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/impl/win.cc @@ -0,0 +1,646 @@ +#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 new file mode 100644 index 0000000..37b5d70 --- /dev/null +++ b/src/perception/pbox_node_dirve/src/serial/src/serial.cc @@ -0,0 +1,430 @@ +/* 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 new file mode 100644 index 0000000..e69de29 diff --git a/src/perception/pbox_node_dirve/编译配置说明.md b/src/perception/pbox_node_dirve/编译配置说明.md new file mode 100644 index 0000000..10403c3 --- /dev/null +++ b/src/perception/pbox_node_dirve/编译配置说明.md @@ -0,0 +1,55 @@ + +## 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/ttyUSB0 | +| `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) + +--- + +**修改内容总结：**