pbox_node_dirve ROS2
This commit is contained in:
parent
06b873920b
commit
49993af206
16
src/perception/pbox_node_dirve/.gitignore
vendored
Normal file
16
src/perception/pbox_node_dirve/.gitignore
vendored
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
# Prerequisites
|
||||||
|
*.o
|
||||||
|
*.i
|
||||||
|
**/**Release**/
|
||||||
|
**/**Debug**/
|
||||||
|
Firmware/
|
||||||
|
M4_BL/combine.sh
|
||||||
|
M4_BL/config.txt
|
||||||
|
*.rej
|
||||||
|
**/**install**/
|
||||||
|
**/**log**/
|
||||||
|
**/**build**/
|
||||||
|
**/**devel**/
|
||||||
|
**/**vscode**/
|
||||||
|
|
||||||
|
|
||||||
55
src/perception/pbox_node_dirve/README.md
Normal file
55
src/perception/pbox_node_dirve/README.md
Normal file
@ -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帧解析 时间戳异常问题
|
||||||
51
src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt
Normal file
51
src/perception/pbox_node_dirve/src/imu_msgs/CMakeLists.txt
Normal file
@ -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(<dependency> 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()
|
||||||
53
src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg
Normal file
53
src/perception/pbox_node_dirve/src/imu_msgs/msg/ASENSING.msg
Normal file
@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
39
src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg
Normal file
39
src/perception/pbox_node_dirve/src/imu_msgs/msg/Gnss.msg
Normal file
@ -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
|
||||||
2
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg
Normal file
2
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu.msg
Normal file
@ -0,0 +1,2 @@
|
|||||||
|
std_msgs/Header header
|
||||||
|
ASENSING imu_msg
|
||||||
26
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg
Normal file
26
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu04.msg
Normal file
@ -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
|
||||||
12
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg
Normal file
12
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu0A.msg
Normal file
@ -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
|
||||||
14
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg
Normal file
14
src/perception/pbox_node_dirve/src/imu_msgs/msg/Imu8B.msg
Normal file
@ -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
|
||||||
@ -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
|
||||||
45
src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg
Normal file
45
src/perception/pbox_node_dirve/src/imu_msgs/msg/Odom.msg
Normal file
@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
23
src/perception/pbox_node_dirve/src/imu_msgs/package.xml
Normal file
23
src/perception/pbox_node_dirve/src/imu_msgs/package.xml
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>imu_msgs</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="shixiaowu@todo.todo">shixiaowu</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<build_depend>rosidl_default_generators</build_depend>
|
||||||
|
|
||||||
|
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||||
|
|
||||||
|
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
56
src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt
Normal file
56
src/perception/pbox_node_dirve/src/pbox_node/CMakeLists.txt
Normal file
@ -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()
|
||||||
@ -0,0 +1,71 @@
|
|||||||
|
#ifndef _AG_Pbox_HPP_
|
||||||
|
#define _AG_Pbox_HPP_
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <vector>
|
||||||
|
#include <memory>
|
||||||
|
#include <cstring>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <serial/serial.h>
|
||||||
|
|
||||||
|
#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
|
||||||
@ -0,0 +1,49 @@
|
|||||||
|
|
||||||
|
#ifndef __TINYLOG_H__
|
||||||
|
#define __TINYLOG_H__
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
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
|
||||||
250
src/perception/pbox_node_dirve/src/pbox_node/include/common.h
Normal file
250
src/perception/pbox_node_dirve/src/pbox_node/include/common.h
Normal file
@ -0,0 +1,250 @@
|
|||||||
|
#ifndef __COMMON_H__
|
||||||
|
#define __COMMON_H__
|
||||||
|
|
||||||
|
// #include <qglobal.h>
|
||||||
|
#include <string>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <unistd.h>
|
||||||
|
// #include <QString>
|
||||||
|
// #include <QColor>
|
||||||
|
#include <string.h>
|
||||||
|
#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;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˢ<EFBFBD>¼<EFBFBD><C2BC>
|
||||||
|
|
||||||
|
const std::string G_DOWN_SOURCETABLE_FINISHED = "download source table finished";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
enum class ConnectType
|
||||||
|
{
|
||||||
|
eNone = 0,
|
||||||
|
eTxt = 1,//<2F>ı<EFBFBD>
|
||||||
|
eVideo = 2,//ý<><C3BD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD>USB<53><42><EFBFBD><EFBFBD>ͷ
|
||||||
|
eSerial = 3,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
eCan = 4,//can<61><6E><EFBFBD><EFBFBD>
|
||||||
|
eUdp = 5,//<2F><>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
eTcp = 6,//<2F><>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
eSomeip = 7,//<2F><>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
eDoip = 8,//<2F><>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
eNtrip = 9
|
||||||
|
};
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
enum class ProtocolType
|
||||||
|
{
|
||||||
|
eNone = 0,
|
||||||
|
eIns = 1,//ins
|
||||||
|
eGnss = 2,//gnss
|
||||||
|
ePoll = 3,//<2F><><EFBFBD><EFBFBD>
|
||||||
|
eVideo = 4
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class LogType {
|
||||||
|
eNone = 0,
|
||||||
|
eText,//<2F><>ͨ<EFBFBD>ı<EFBFBD>
|
||||||
|
eVideoRecordStart,//<2F><>ʼ¼<CABC><C2BC><EFBFBD><EFBFBD>Ƶ
|
||||||
|
eVideoRecordStop,//<2F><><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>Ƶ
|
||||||
|
eSerialPortConnect,//<2F><><EFBFBD><EFBFBD>
|
||||||
|
eSerialPort,//<2F><><EFBFBD><EFBFBD>
|
||||||
|
eSerialPortClose,//<2F><><EFBFBD><EFBFBD>
|
||||||
|
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
|
||||||
|
};
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// ״̬<D7B4>ռ<EFBFBD>ö<EFBFBD><C3B6>ֵ
|
||||||
|
/// </summary>
|
||||||
|
enum class StatusUi
|
||||||
|
{
|
||||||
|
eNone,
|
||||||
|
eSerial,
|
||||||
|
eCan,
|
||||||
|
eUdp,
|
||||||
|
eTcp,
|
||||||
|
ePosLoss,//RTK
|
||||||
|
eBiasDiffuse,//<2F><>ƫ<EFBFBD><C6AB>ɢ
|
||||||
|
eSizeDiffuse,//<2F>˱۷<CBB1>ɢ
|
||||||
|
eAngleDiffuse,//<2F><>̬<EFBFBD><CCAC>ɢ
|
||||||
|
eRtkOff,//<2F><><EFBFBD><EFBFBD><EFBFBD>ʱ
|
||||||
|
eWheelSpeedLoss,//<2F><><EFBFBD>ٶ<EFBFBD>ʧ
|
||||||
|
eGnssLoss//GNSS<53><53>ʧ
|
||||||
|
};
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD>״ֵ̬
|
||||||
|
/// </summary>
|
||||||
|
enum class StatusType
|
||||||
|
{
|
||||||
|
eNone,
|
||||||
|
eOpenOrValid,
|
||||||
|
eError,
|
||||||
|
eCloseOrOver
|
||||||
|
};
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC>key<65><79>ip<69><70>ַ+<2B><><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><>ת<EFBFBD><D7AA><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>С<EFBFBD><D0A1>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="temp"></param>
|
||||||
|
/// <param name="length"></param>
|
||||||
|
void reverseData(uint8_t* temp, int length);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʵ<D7AA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>Դ<EFBFBD><D4B4><EFBFBD>С<EFBFBD><D0A1>ת<EFBFBD><D7AA>
|
||||||
|
/// </summary>
|
||||||
|
/// <typeparam name="T"></typeparam>
|
||||||
|
/// <param name="add"></param>
|
||||||
|
/// <param name="index"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
template<typename T>
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʵ<D7AA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
/// <typeparam name="T"></typeparam>
|
||||||
|
/// <param name="add"></param>
|
||||||
|
/// <param name="index"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
template<typename T>
|
||||||
|
T toValue(const uint8_t* add, int& index)
|
||||||
|
{
|
||||||
|
T result = *(T*)(add + index);
|
||||||
|
index += sizeof(T);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תʵ<D7AA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
/// <typeparam name="T"></typeparam>
|
||||||
|
/// <param name="add"></param>
|
||||||
|
/// <param name="index"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
template<typename T>
|
||||||
|
T toValue(const char* add, int& index)
|
||||||
|
{
|
||||||
|
T result = *(T*)(add + index);
|
||||||
|
index += sizeof(T);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
template<typename T>
|
||||||
|
std::string toBytes(const T& data)
|
||||||
|
{
|
||||||
|
return std::string((char*)&data, sizeof(T));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20>ַ<EFBFBD><D6B7><EFBFBD>0~F/f)תʮ<D7AA><CAAE><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="data"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
char convertToBit(char data);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת16<31><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="data"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
// QString convertToHex(const std::string& data);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 16<31><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>硰FF"->255
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="origin_data"></param>
|
||||||
|
/// <returns></returns>
|
||||||
|
// 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
|
||||||
@ -0,0 +1,53 @@
|
|||||||
|
#ifndef __PROTOCOL_ASENSING_H__
|
||||||
|
#define __PROTOCOL_ASENSING_H__
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <memory>
|
||||||
|
#include <functional>
|
||||||
|
#include <vector>
|
||||||
|
#include <map>
|
||||||
|
#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<std::string, int> protocolLengthMap;
|
||||||
|
static std::map<std::string , ProtocolAsensing*> protocolMap;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@ -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])
|
||||||
25
src/perception/pbox_node_dirve/src/pbox_node/package.xml
Normal file
25
src/perception/pbox_node_dirve/src/pbox_node/package.xml
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>pbox_node</name>
|
||||||
|
<version>3.0.1</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="yfsw@asensing.com">yfsw@asensing</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<buildtool_depend>serial</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>imu_msgs</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<depend>nav_msgs</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
224
src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp
Normal file
224
src/perception/pbox_node_dirve/src/pbox_node/src/AGPbox.cpp
Normal file
@ -0,0 +1,224 @@
|
|||||||
|
#include <chrono>
|
||||||
|
#include "AGPbox.hpp"
|
||||||
|
#include <utility>
|
||||||
|
#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<int>("MsgType", 1);
|
||||||
|
this->declare_parameter<int>("ConnectionType", 0);
|
||||||
|
/* serial port */
|
||||||
|
this->declare_parameter<std::string>("UART_Port", UART_PORT);
|
||||||
|
this->declare_parameter<int>("UART_Baudrate", UART_BAUDRATE);
|
||||||
|
/* UDP */
|
||||||
|
this->declare_parameter<std::string>("UDP_Addr", "192.168.119.133");
|
||||||
|
this->declare_parameter<int>("UDP_Port", 12300);
|
||||||
|
this->declare_parameter<int>("USB_LatencyTime" , 16);
|
||||||
|
/* log */
|
||||||
|
this->declare_parameter<std::string>("LogInfo", "");
|
||||||
|
this->declare_parameter<int>("LogLevel", 1);
|
||||||
|
/* range */
|
||||||
|
this->declare_parameter<float>("Grange04", 250.0);
|
||||||
|
this->declare_parameter<float>("Arange04", 4.0);
|
||||||
|
this->declare_parameter<float>("Grange0B", 4.0);
|
||||||
|
this->declare_parameter<float>("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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
245
src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp
Normal file
245
src/perception/pbox_node_dirve/src/pbox_node/src/TinyLog.cpp
Normal file
@ -0,0 +1,245 @@
|
|||||||
|
|
||||||
|
#include "TinyLog.h"
|
||||||
|
|
||||||
|
#include <stdarg.h>
|
||||||
|
#include <thread>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <string.h>
|
||||||
|
#include <atomic>
|
||||||
|
#include <sstream>
|
||||||
|
#include <chrono>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
#ifdef _WIN32
|
||||||
|
#include <direct.h>
|
||||||
|
#else
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
int TinyLog::storageLevel = TinyLog::INFO; /* storage to file , default INFO */
|
||||||
|
|
||||||
|
int TinyLog::singleMaxSize = 10485760; /* unit<69><74>Byte<74><65>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<<log<<std::endl;
|
||||||
|
|
||||||
|
if (level >= storageLevel)
|
||||||
|
{
|
||||||
|
std::string filename = storageDir;
|
||||||
|
if (fileSize(filename.c_str()) > singleMaxSize)
|
||||||
|
{
|
||||||
|
std::chrono::milliseconds now = std::chrono::duration_cast<std::chrono::milliseconds>(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");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
51
src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp
Normal file
51
src/perception/pbox_node_dirve/src/pbox_node/src/common.cpp
Normal file
@ -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;
|
||||||
|
// }
|
||||||
43
src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp
Normal file
43
src/perception/pbox_node_dirve/src/pbox_node/src/main.cpp
Normal file
@ -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<AGPbox>();
|
||||||
|
node->init_pbox_node();
|
||||||
|
/* create publisher */
|
||||||
|
#ifdef PROTOCOL_BDDB0A
|
||||||
|
auto publish0A = std::make_shared<Decode_0A>(node);
|
||||||
|
#endif
|
||||||
|
#ifdef PROTOCOL_BDDB04
|
||||||
|
auto publish04 = std::make_shared<Decode_04>(node);
|
||||||
|
#endif
|
||||||
|
#ifdef PROTOCOL_BDDB8B
|
||||||
|
auto publish8B = std::make_shared<Decode_8B>(node);
|
||||||
|
#endif
|
||||||
|
#ifdef PROTOCOL_BDDB0B
|
||||||
|
auto publish0B = std::make_shared<Decode_0B>(node);
|
||||||
|
#endif
|
||||||
|
#ifdef PROTOCOL_BDDB10
|
||||||
|
auto publish10 = std::make_shared<Decode_10>(node);
|
||||||
|
#endif
|
||||||
|
#ifdef PROTOCOL_BDDB1B
|
||||||
|
auto publish1B = std::make_shared<Decode_1B>(node);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//循环节点
|
||||||
|
rclcpp::spin(node);
|
||||||
|
rclcpp::shutdown();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@ -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<rclcpp::Node> 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<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
m_pubAsensingMsg.gy = (toValue<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
m_pubAsensingMsg.gz = (toValue<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
// acc
|
||||||
|
m_pubAsensingMsg.ax = (toValue<int16_t>(sub_address, sub_index)) * m_arange / 32768.0;
|
||||||
|
m_pubAsensingMsg.ay = (toValue<int16_t>(sub_address, sub_index)) * m_arange / 32768.0;
|
||||||
|
m_pubAsensingMsg.az = (toValue<int16_t>(sub_address, sub_index)) * m_arange / 32768.0;
|
||||||
|
// temp
|
||||||
|
middle = toValue<int16_t>(sub_address, sub_index);
|
||||||
|
m_pubAsensingMsg.temperature = middle * 200.0 / 32768;
|
||||||
|
// time
|
||||||
|
tmpU32 = toValue<uint32_t>(sub_address, sub_index);
|
||||||
|
m_pubAsensingMsg.imu_time_stamp = (double)tmpU32 / 4000;
|
||||||
|
// status
|
||||||
|
m_pubAsensingMsg.status = toValue<uint8_t>(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<imu_msgs::msg::Imu04>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<imu_msgs::msg::Imu04>(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<sensor_msgs::msg::Imu>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<sensor_msgs::msg::Imu>(m_topicName.c_str(), 500);
|
||||||
|
}
|
||||||
|
sub_index = 3;
|
||||||
|
pubMsg.angular_velocity.x = (toValue<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
pubMsg.angular_velocity.y = (toValue<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
pubMsg.angular_velocity.z = (toValue<int16_t>(sub_address, sub_index)) * m_grange / 32768.0;
|
||||||
|
|
||||||
|
pubMsg.linear_acceleration.x = (toValue<int16_t>(sub_address, sub_index)) * m_arange / 32768.0;
|
||||||
|
pubMsg.linear_acceleration.y = (toValue<int16_t>(sub_address, sub_index)) * m_arange / 32768.0;
|
||||||
|
pubMsg.linear_acceleration.z = (toValue<int16_t>(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<rclcpp::Node> 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;
|
||||||
|
};
|
||||||
@ -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<rclcpp::Node> 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<float>(sub_address, sub_index);
|
||||||
|
imu0a_data.gyro_y = toValue<float>(sub_address, sub_index);
|
||||||
|
imu0a_data.gyro_z = toValue<float>(sub_address, sub_index);
|
||||||
|
// acc
|
||||||
|
imu0a_data.acc_x = toValue<float>(sub_address, sub_index);
|
||||||
|
imu0a_data.acc_y = toValue<float>(sub_address, sub_index);
|
||||||
|
imu0a_data.acc_z = toValue<float>(sub_address, sub_index);
|
||||||
|
|
||||||
|
// temp
|
||||||
|
middle = toValue<int16_t>(sub_address, sub_index);
|
||||||
|
imu0a_data.temperature = middle * 200.0 / 32768;
|
||||||
|
|
||||||
|
tmpU32 = toValue<uint32_t>(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<imu_msgs::msg::Imu0A>(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<imu_msgs::msg::ImuInitial>(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<sensor_msgs::msg::Imu>::SharedPtr publisher;
|
||||||
|
// if(!isCreate)
|
||||||
|
// {
|
||||||
|
// isCreate = true;
|
||||||
|
// publisher = m_node->create_publisher<sensor_msgs::msg::Imu>(m_topicName.c_str(), 500);
|
||||||
|
// }
|
||||||
|
// /* frame BDDB0A not have roll pitch yaw */
|
||||||
|
|
||||||
|
// pubMsg.angular_velocity.x = toValue<float>(sub_address, sub_index);
|
||||||
|
// pubMsg.angular_velocity.y = toValue<float>(sub_address, sub_index);
|
||||||
|
// pubMsg.angular_velocity.z = toValue<float>(sub_address, sub_index);
|
||||||
|
|
||||||
|
// pubMsg.linear_acceleration.x = toValue<float>(sub_address, sub_index);
|
||||||
|
// pubMsg.linear_acceleration.y = toValue<float>(sub_address, sub_index);
|
||||||
|
// pubMsg.linear_acceleration.z = toValue<float>(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<rclcpp::Node> m_node;
|
||||||
|
imu_msgs::msg::ImuInitial m_msg;
|
||||||
|
imu_msgs::msg::Imu0A m_msg0A;
|
||||||
|
rclcpp::Publisher<imu_msgs::msg::ImuInitial>::SharedPtr m_pub;
|
||||||
|
rclcpp::Publisher<imu_msgs::msg::Imu0A>::SharedPtr m_pub0A;
|
||||||
|
|
||||||
|
/* use ros msg or asensing msg */
|
||||||
|
int m_msgSwitch = 1;
|
||||||
|
int m_protocolType;
|
||||||
|
bool isCreate = false;
|
||||||
|
};
|
||||||
@ -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<rclcpp::Node> 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<int16_t>(data, sub_index)) * 360.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.pitch = (toValue<int16_t>(data, sub_index)) * 360.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.azimuth = (toValue<int16_t>(data, sub_index)) * 360.0 / 32768;
|
||||||
|
|
||||||
|
/* gx gy gz , ax ay az */
|
||||||
|
m_imuMsg.imu_msg.x_angular_velocity = (toValue<int16_t>(data, sub_index)) * 300.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.y_angular_velocity = (toValue<int16_t>(data, sub_index)) * 300.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.z_angular_velocity = (toValue<int16_t>(data, sub_index)) * 300.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.x_acc = (toValue<int16_t>(data, sub_index)) * 12.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.y_acc = (toValue<int16_t>(data, sub_index)) * 12.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.z_acc = (toValue<int16_t>(data, sub_index)) * 12.0 / 32768;
|
||||||
|
|
||||||
|
m_imuMsg.imu_msg.latitude = (toValue<int32_t>(data, sub_index)) * 0.0000001;
|
||||||
|
m_imuMsg.imu_msg.longitude = (toValue<int32_t>(data, sub_index)) * 0.0000001;
|
||||||
|
m_imuMsg.imu_msg.altitude = (toValue<int32_t>(data, sub_index)) * 0.001;
|
||||||
|
m_imuMsg.imu_msg.north_velocity = (toValue<int16_t>(data, sub_index)) * 100.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.east_velocity = (toValue<int16_t>(data, sub_index)) * 100.0 / 32768;
|
||||||
|
m_imuMsg.imu_msg.ground_velocity = (toValue<int16_t>(data, sub_index)) * 100.0 / 32768;
|
||||||
|
|
||||||
|
m_imuMsg.imu_msg.ins_status = data[sub_index ++];
|
||||||
|
|
||||||
|
sub_index += 4;
|
||||||
|
sub_index += 2;
|
||||||
|
temp[0] = toValue<int16_t>(data, sub_index);
|
||||||
|
temp[1] = toValue<int16_t>(data, sub_index);
|
||||||
|
temp[2] = toValue<int16_t>(data, sub_index);
|
||||||
|
m_ts = (toValue<uint32_t>(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<uint32_t>(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<imu_msgs::msg::Imu>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<imu_msgs::msg::Imu>(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<sensor_msgs::msg::Imu>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<sensor_msgs::msg::Imu>(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<rclcpp::Node> 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;
|
||||||
|
};
|
||||||
@ -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<rclcpp::Node> 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<int32_t>(data, sub_index)) * 0.0000001;
|
||||||
|
m_gnssMsg.lon_sigma = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.latitude = (toValue<int32_t>(data, sub_index)) * 0.0000001;
|
||||||
|
m_gnssMsg.lat_sigma = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.altitude = (toValue<int32_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.alt_sigma = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
|
||||||
|
// m_gnssMsg.gps_fix = toValue<uint16_t>(data , sub_index); // sAcc
|
||||||
|
sub_index += 2;
|
||||||
|
|
||||||
|
// m_gnssMsg.rtk_age = toValue<uint16_t>(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<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.track_angle = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.ver_vel = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.latency_vel = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.base_length = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
|
||||||
|
m_gnssMsg.yaw = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.yaw_sigma = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.pitch = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
m_gnssMsg.pitch_sigma = (toValue<int16_t>(data, sub_index)) * 0.001;
|
||||||
|
|
||||||
|
std::string timeStr;
|
||||||
|
timeStr += std::to_string(((toValue<uint16_t>(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<uint16_t>(data, sub_index)*0.001);
|
||||||
|
m_gnssMsg.utc_time = timeStr;
|
||||||
|
|
||||||
|
m_gnssMsg.ts_pos = toValue<uint32_t>(data, sub_index);
|
||||||
|
m_gnssMsg.ts_vel = toValue<uint32_t>(data, sub_index);
|
||||||
|
m_gnssMsg.ts_heading = toValue<uint32_t>(data, sub_index);
|
||||||
|
m_gnssMsg.state = data[sub_index++];
|
||||||
|
m_gnssMsg.num_master = data[sub_index++];
|
||||||
|
sub_index++;
|
||||||
|
sub_index++;
|
||||||
|
m_gnssMsg.gdop = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.pdop = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.hdop = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.htdop = (toValue<int16_t>(data, sub_index)) * 0.01;
|
||||||
|
m_gnssMsg.tdop = (toValue<int16_t>(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<imu_msgs::msg::Gnss>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<imu_msgs::msg::Gnss>(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<sensor_msgs::msg::NavSatFix>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<sensor_msgs::msg::NavSatFix>(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<rclcpp::Node> m_node;
|
||||||
|
/* msg */
|
||||||
|
sensor_msgs::msg::NavSatFix m_rosGnssMsg;
|
||||||
|
imu_msgs::msg::Gnss m_gnssMsg;
|
||||||
|
|
||||||
|
/* range */
|
||||||
|
float m_ts = 0.0;
|
||||||
|
};
|
||||||
@ -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<rclcpp::Node> 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<int32_t>(data,sub_index)) * (1.0e-9);
|
||||||
|
m_OdomMsg.q1_x = (toValue<int32_t>(data,sub_index)) * (1.0e-9);
|
||||||
|
m_OdomMsg.q2_y = (toValue<int32_t>(data,sub_index)) * (1.0e-9);
|
||||||
|
m_OdomMsg.q3_z = (toValue<int32_t>(data,sub_index)) * (1.0e-9);
|
||||||
|
m_OdomMsg.pos_x = (toValue<int32_t>(data,sub_index)) * (1.0e-3);
|
||||||
|
m_OdomMsg.pos_y = (toValue<int32_t>(data,sub_index)) * (1.0e-3);
|
||||||
|
m_OdomMsg.pos_z = (toValue<int32_t>(data,sub_index)) * (1.0e-3);
|
||||||
|
m_OdomMsg.vel_x = (toValue<int16_t>(data,sub_index)) * 100 / 32768.0;
|
||||||
|
m_OdomMsg.vel_y = (toValue<int16_t>(data,sub_index)) * 100 / 32768.0;
|
||||||
|
m_OdomMsg.vel_z = (toValue<int16_t>(data,sub_index)) * 100 / 32768.0;
|
||||||
|
m_OdomMsg.vel = (toValue<int16_t>(data,sub_index)) * 100 / 32768.0;
|
||||||
|
m_OdomMsg.ang_vel_x = (toValue<int16_t>(data,sub_index)) * 300 / 32768.0;
|
||||||
|
m_OdomMsg.ang_vel_y = (toValue<int16_t>(data,sub_index)) * 300 / 32768.0;
|
||||||
|
m_OdomMsg.ang_vel_z = (toValue<int16_t>(data,sub_index)) * 300 / 32768.0;
|
||||||
|
m_OdomMsg.acc_x = (toValue<int16_t>(data,sub_index)) * 12.0 / 32768.0;
|
||||||
|
m_OdomMsg.acc_y = (toValue<int16_t>(data,sub_index)) * 12.0 / 32768.0;
|
||||||
|
m_OdomMsg.acc_z = (toValue<int16_t>(data,sub_index)) * 12.0 / 32768.0;
|
||||||
|
m_OdomMsg.status = data[sub_index++];
|
||||||
|
m_OdomMsg.sensor_status = (toValue<uint32_t>(data,sub_index));
|
||||||
|
temp[0] = (toValue<int16_t>(data,sub_index));
|
||||||
|
temp[1] = (toValue<int16_t>(data,sub_index));
|
||||||
|
temp[2] = (toValue<int16_t>(data,sub_index));
|
||||||
|
poll_type = data[sub_index++];
|
||||||
|
routing_cnt = data[sub_index++];
|
||||||
|
ts_ms = (toValue<int32_t>(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<imu_msgs::msg::Odom>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<imu_msgs::msg::Odom>(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<nav_msgs::msg::Odometry>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<nav_msgs::msg::Odometry>(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<rclcpp::Node> m_node;
|
||||||
|
/* msg */
|
||||||
|
nav_msgs::msg::Odometry m_rosPboxOdom;
|
||||||
|
imu_msgs::msg::Odom m_OdomMsg;
|
||||||
|
|
||||||
|
};
|
||||||
@ -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<rclcpp::Node> 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<uint8_t>(sub_address, sub_index);
|
||||||
|
pubMsg.data_length = toValue<uint16_t>(sub_address, sub_index);
|
||||||
|
pubMsg.frame_count = toValue<uint32_t>(sub_address, sub_index);
|
||||||
|
pubMsg.serial_number = toValue<uint8_t>(sub_address, sub_index);
|
||||||
|
|
||||||
|
// gyro
|
||||||
|
pubMsg.gx = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.gy = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.gz = toValue<float>(sub_address, sub_index);
|
||||||
|
|
||||||
|
// acc
|
||||||
|
pubMsg.ax = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.ay = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.az = toValue<float>(sub_address, sub_index);
|
||||||
|
|
||||||
|
// temp
|
||||||
|
middle = toValue<int16_t>(sub_address, sub_index);
|
||||||
|
pubMsg.temperature = middle * 200.0 / 32768;
|
||||||
|
|
||||||
|
sub_index++;
|
||||||
|
pubMsg.status = toValue<uint8_t>(sub_address, sub_index);
|
||||||
|
|
||||||
|
{
|
||||||
|
/* Asensing msg type */
|
||||||
|
static bool isCreate = false;
|
||||||
|
|
||||||
|
static rclcpp::Publisher<imu_msgs::msg::Imu8B>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<imu_msgs::msg::Imu8B>(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<sensor_msgs::msg::Imu>::SharedPtr publisher;
|
||||||
|
if(!isCreate)
|
||||||
|
{
|
||||||
|
isCreate = true;
|
||||||
|
publisher = m_node->create_publisher<sensor_msgs::msg::Imu>(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<float>(sub_address, sub_index);
|
||||||
|
pubMsg.angular_velocity.y = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.angular_velocity.z = toValue<float>(sub_address, sub_index);
|
||||||
|
|
||||||
|
pubMsg.linear_acceleration.x = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.linear_acceleration.y = toValue<float>(sub_address, sub_index);
|
||||||
|
pubMsg.linear_acceleration.z = toValue<float>(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<rclcpp::Node> 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;
|
||||||
|
};
|
||||||
@ -0,0 +1,178 @@
|
|||||||
|
#include "protocol_asensing.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <iostream>
|
||||||
|
#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<std::string, int> ProtocolAsensing::protocolLengthMap{};
|
||||||
|
std::map<std::string , ProtocolAsensing*> 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<std::string, int>(protocolFlag, length));
|
||||||
|
protocolMap.insert(std::pair<std::string, ProtocolAsensing*>(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;
|
||||||
|
}
|
||||||
65
src/perception/pbox_node_dirve/src/serial/CMakeLists.txt
Normal file
65
src/perception/pbox_node_dirve/src/serial/CMakeLists.txt
Normal file
@ -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()
|
||||||
62
src/perception/pbox_node_dirve/src/serial/Makefile
Normal file
62
src/perception/pbox_node_dirve/src/serial/Makefile
Normal file
@ -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
|
||||||
58
src/perception/pbox_node_dirve/src/serial/README.md
Normal file
58
src/perception/pbox_node_dirve/src/serial/README.md
Normal file
@ -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**:
|
||||||
|
|
||||||
|
<img title="" src="file:///home/lee/serial/images/install_Serial_ubuntu.png" alt="" data-align="inline">
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
也可以不用 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 <wjwwood@gmail.com>
|
||||||
|
John Harrison <ash.gti@gmail.com>
|
||||||
|
|
||||||
|
### Contact
|
||||||
|
|
||||||
|
William Woodall <william@osrfoundation.org>
|
||||||
BIN
src/perception/pbox_node_dirve/src/serial/README.pdf
Normal file
BIN
src/perception/pbox_node_dirve/src/serial/README.pdf
Normal file
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 141 KiB |
@ -0,0 +1,221 @@
|
|||||||
|
/*!
|
||||||
|
* \file serial/impl/unix.h
|
||||||
|
* \author William Woodall <wjwwood@gmail.com>
|
||||||
|
* \author John Harrison <ash@greaterthaninfinity.com>
|
||||||
|
* \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 <pthread.h>
|
||||||
|
|
||||||
|
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)
|
||||||
@ -0,0 +1,207 @@
|
|||||||
|
/*!
|
||||||
|
* \file serial/impl/windows.h
|
||||||
|
* \author William Woodall <wjwwood@gmail.com>
|
||||||
|
* \author John Harrison <ash@greaterthaninfinity.com>
|
||||||
|
* \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)
|
||||||
@ -0,0 +1,775 @@
|
|||||||
|
/*!
|
||||||
|
* \file serial/serial.h
|
||||||
|
* \author William Woodall <wjwwood@gmail.com>
|
||||||
|
* \author John Harrison <ash.gti@gmail.com>
|
||||||
|
* \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 <limits>
|
||||||
|
#include <vector>
|
||||||
|
#include <string>
|
||||||
|
#include <cstring>
|
||||||
|
#include <sstream>
|
||||||
|
#include <exception>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <serial/v8stdint.h>
|
||||||
|
|
||||||
|
#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<uint32_t>::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<uint8_t> &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<string> containing the lines.
|
||||||
|
*
|
||||||
|
* \throw serial::PortNotOpenedException
|
||||||
|
* \throw serial::SerialException
|
||||||
|
*/
|
||||||
|
std::vector<std::string>
|
||||||
|
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<uint8_t> &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<PortInfo>
|
||||||
|
list_ports();
|
||||||
|
|
||||||
|
} // namespace serial
|
||||||
|
|
||||||
|
#endif
|
||||||
@ -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 <stddef.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#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 <stdint.h>
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif // V8STDINT_H_
|
||||||
30
src/perception/pbox_node_dirve/src/serial/package.xml
Normal file
30
src/perception/pbox_node_dirve/src/serial/package.xml
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package>
|
||||||
|
<name>serial</name>
|
||||||
|
<version>1.2.1</version>
|
||||||
|
<description>
|
||||||
|
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.
|
||||||
|
</description>
|
||||||
|
|
||||||
|
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
|
||||||
|
|
||||||
|
<license>MIT</license>
|
||||||
|
|
||||||
|
<url type="website">http://wjwwood.github.com/serial/</url>
|
||||||
|
<url type="repository">https://github.com/wjwwood/serial</url>
|
||||||
|
<url type="bugtracker">https://github.com/wjwwood/serial/issues</url>
|
||||||
|
|
||||||
|
<author email="wjwwood@gmail.com">William Woodall</author>
|
||||||
|
<author email="ash.gti@gmail.com">John Harrison</author>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<test_depend>boost</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
|
||||||
|
</package>
|
||||||
@ -0,0 +1,335 @@
|
|||||||
|
#if defined(__linux__)
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
|
||||||
|
* 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 <vector>
|
||||||
|
#include <string>
|
||||||
|
#include <sstream>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <cstdio>
|
||||||
|
#include <cstdarg>
|
||||||
|
#include <cstdlib>
|
||||||
|
|
||||||
|
#include <glob.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#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<string> glob(const vector<string>& 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<string> 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<string>
|
||||||
|
glob(const vector<string>& patterns)
|
||||||
|
{
|
||||||
|
vector<string> 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<string>::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<string>
|
||||||
|
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<string> 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<PortInfo>
|
||||||
|
serial::list_ports()
|
||||||
|
{
|
||||||
|
vector<PortInfo> results;
|
||||||
|
|
||||||
|
vector<string> 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<string> devices_found = glob( search_globs );
|
||||||
|
|
||||||
|
vector<string>::iterator iter = devices_found.begin();
|
||||||
|
|
||||||
|
while( iter != devices_found.end() )
|
||||||
|
{
|
||||||
|
string device = *iter++;
|
||||||
|
|
||||||
|
vector<string> 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__)
|
||||||
@ -0,0 +1,286 @@
|
|||||||
|
#if defined(__APPLE__)
|
||||||
|
|
||||||
|
#include <sys/param.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include <CoreFoundation/CoreFoundation.h>
|
||||||
|
#include <IOKit/IOKitLib.h>
|
||||||
|
#include <IOKit/serial/IOSerialKeys.h>
|
||||||
|
#include <IOKit/IOBSD.h>
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#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<CFStringRef>(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<CFStringRef>(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<CFNumberRef>(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<PortInfo>
|
||||||
|
serial::list_ports(void)
|
||||||
|
{
|
||||||
|
vector<PortInfo> 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__)
|
||||||
@ -0,0 +1,152 @@
|
|||||||
|
#if defined(_WIN32)
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2014 Craig Lilley <cralilley@gmail.com>
|
||||||
|
* 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 <tchar.h>
|
||||||
|
#include <windows.h>
|
||||||
|
#include <setupapi.h>
|
||||||
|
#include <initguid.h>
|
||||||
|
#include <devguid.h>
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
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<PortInfo>
|
||||||
|
serial::list_ports()
|
||||||
|
{
|
||||||
|
vector<PortInfo> 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)
|
||||||
1066
src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc
Normal file
1066
src/perception/pbox_node_dirve/src/serial/src/impl/unix.cc
Normal file
File diff suppressed because it is too large
Load Diff
646
src/perception/pbox_node_dirve/src/serial/src/impl/win.cc
Normal file
646
src/perception/pbox_node_dirve/src/serial/src/impl/win.cc
Normal file
@ -0,0 +1,646 @@
|
|||||||
|
#if defined(_WIN32)
|
||||||
|
|
||||||
|
/* Copyright 2012 William Woodall and John Harrison */
|
||||||
|
|
||||||
|
#include <sstream>
|
||||||
|
|
||||||
|
#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<size_t>(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<DWORD>(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<DWORD>(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)
|
||||||
|
|
||||||
430
src/perception/pbox_node_dirve/src/serial/src/serial.cc
Normal file
430
src/perception/pbox_node_dirve/src/serial/src/serial.cc
Normal file
@ -0,0 +1,430 @@
|
|||||||
|
/* Copyright 2012 William Woodall and John Harrison */
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#if !defined(_WIN32) && !defined(__OpenBSD__) && !defined(__FreeBSD__)
|
||||||
|
# include <alloca.h>
|
||||||
|
#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<uint8_t> &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<const char*>(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<uint8_t*>
|
||||||
|
(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<const char*>
|
||||||
|
(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<const char*> (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<string>
|
||||||
|
Serial::readlines (size_t size, string eol)
|
||||||
|
{
|
||||||
|
ScopedReadLock lock(this->pimpl_);
|
||||||
|
std::vector<std::string> lines;
|
||||||
|
size_t eol_len = eol.length ();
|
||||||
|
uint8_t *buffer_ = static_cast<uint8_t*>
|
||||||
|
(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<const char*> (buffer_ + start_of_line),
|
||||||
|
read_so_far - start_of_line));
|
||||||
|
}
|
||||||
|
break; // Timeout occured on reading 1 byte
|
||||||
|
}
|
||||||
|
if (string (reinterpret_cast<const char*>
|
||||||
|
(buffer_ + read_so_far - eol_len), eol_len) == eol) {
|
||||||
|
// EOL found
|
||||||
|
lines.push_back(
|
||||||
|
string(reinterpret_cast<const char*> (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<const char*> (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<const uint8_t*>(data.c_str()),
|
||||||
|
data.length());
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t
|
||||||
|
Serial::write (const std::vector<uint8_t> &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 ();
|
||||||
|
}
|
||||||
0
src/perception/pbox_node_dirve/test.csv
Normal file
0
src/perception/pbox_node_dirve/test.csv
Normal file
|
|
55
src/perception/pbox_node_dirve/编译配置说明.md
Normal file
55
src/perception/pbox_node_dirve/编译配置说明.md
Normal file
@ -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)
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
**修改内容总结：**
|
||||||
Loading…
Reference in New Issue
Block a user