HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of ssh://192.168.10.50:2222/HiveCoreRD/hivecore_robot_drivers

Browse Source
This commit is contained in:
NuoDaJia
2025-10-18 16:26:22 +08:00
parent 492edcbf64 75150a4a49
commit 753a0baa9a
21 changed files with 694 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
dist_dev/launch/dist_dev.launch.py
View File

@@ -6,10 +6,13 @@ from launch.conditions import IfCondition
def generate_launch_description():
return LaunchDescription([
ExecuteProcess(
cmd=["ros2","launch","livox_ros_driver2","msg_MID360_launch.py"],
output="screen"
),
# 等待一段时间确保相机启动完成
ExecuteProcess(
cmd=['sleep', '3'],
cmd=['sleep', '2'],
output='screen'
),
Node(

4
dist_dev/src/dist_dev_node.cpp
View File

@@ -7,7 +7,7 @@
#include <message_filters/synchronizer.h>
#include"dist_dev/dist_dev_node.hpp"
namespace dist_dev{
DistSub::DistSub() : Node("DistSub") {
DistSub::DistSub() : Node("dist_dev_node") {
dist_msg= std::make_shared<dist_dev::msg::DistMsg>();
dist_pub = this->create_publisher<dist_dev::msg::DistMsg>("/dist_msg", 10);
}
@@ -29,7 +29,7 @@ void DistSub::pub_msg(const DistCfg& cfg){
dist_msg->points= sensor_msgs::msg::PointCloud2();
dist_pub->publish(*dist_msg);
pub_cnt+=1;
std::cout<<"pub msg["<<pub_cnt<<"]:"<<std::endl;
std::cout<<"pub msg["<<pub_cnt<<"]:"<<dist_arr[0]<<","<<dist_arr[1]<<std::endl;
}
}

5
dist_dev/src/ks104.cpp
View File

@@ -7,9 +7,10 @@ static int uart_send(const char *port, int baud, const u8 *tx_data, int tx_len,u
// 1. 打开串口（非阻塞模式）
int fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd < 0) {
perror("open port failed");
printf("open com:%s error\n",port);
return -1;
}
/// printf("now config:%s\n",port);
// 2. 配置串口参数（8N1）
struct termios options;
tcgetattr(fd, &options);
@@ -54,7 +55,7 @@ static int uart_send(const char *port, int baud, const u8 *tx_data, int tx_len,u
return total_read;
}
static int ks104_send(const u8 *tx,u8* rx){
int ret=uart_send("/dev/ttyUSB0",115200,tx,3,rx,128,100);
int ret=uart_send("/dev/ttyS0",115200,tx,3,rx,128,100);
return ret;
}
int ks104_dist(u8 addr){

6
dist_dev/src/main.cpp
View File

@@ -22,7 +22,7 @@ int main(int argc, char* argv[]) {
return -1;
}
//超声测距
cur_node->dist_arr.resize(4);
cur_node->dist_arr.resize(2);
rclcpp::TimerBase::SharedPtr timer_=cur_node->create_wall_timer(std::chrono::milliseconds(100), [=]() {
//ls.header.frame_id="ultralsonic";
@@ -30,8 +30,8 @@ int main(int argc, char* argv[]) {
#if 1
cur_node->dist_arr[0]=ks104_dist(0xd0);
cur_node->dist_arr[1]=ks104_dist(0xd2);
cur_node->dist_arr[2]=ks104_dist(0xd4);
cur_node->dist_arr[3]=ks104_dist(0xd6);
// cur_node->dist_arr[2]=ks104_dist(0xd4);
// cur_node->dist_arr[3]=ks104_dist(0xd6);
cur_node->pub_msg(cfg0);
#endif
});

15
img_dev/launch/img_dev.launch.py
View File

@@ -7,22 +7,15 @@ from launch.conditions import IfCondition
def generate_launch_description():
return LaunchDescription([
# 启动orbbec_camera
ExecuteProcess(
cmd=[
'ros2',
'launch',
'orbbec_camera',
'multi_camera.launch.py'
],
output='screen'
),
#ExecuteProcess(
# cmd=['ros2','launch', 'orbbec_camera', 'multi_camera.launch.py'],
# output='screen'),
ExecuteProcess(
cmd=[
'ros2',
'launch',
'realsense2_camera',
'rs_launch.py'
'rs_multi_camera_launch.py'
],
output='screen'
),

22
img_dev/src/main.cpp
View File

@@ -18,7 +18,8 @@ static shared_ptr<ImageSubscriber> cur_node=nullptr;
ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera_01/color/image_raw","/camera_01/depth/image_raw");
ImgCfg cfg1=ImgCfg("orbbec", "myType","right","/camera_02/color/image_raw","/camera_02/depth/image_raw");
//
ImgCfg cfg2=ImgCfg("realsense", "myType","front","/camera/color/image_raw","/camera/depth/image_raw");
ImgCfg cfg2=ImgCfg("realsense", "myType","left","/camera1/camera1/rgbd","/camera1/camera1/rgbd");
ImgCfg cfg3=ImgCfg("realsense", "myType","right","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
void sync_cb0(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
cur_node->pub_msg(c_img,d_img,cfg0);
}
@@ -75,7 +76,7 @@ int main(int argc, char* argv[]) {
//////////////////奥比中光2 END///////////////////////
//////////////////realsense START///////////////////////
rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera/camera/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd1_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera1/camera1/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
if(cfg2.k_arr.size()==0||cfg2.d_arr.size()==0){
cfg2.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
cfg2.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
@@ -85,6 +86,23 @@ int main(int argc, char* argv[]) {
cur_node->pub_msg(msg.rgb,msg.depth,cfg2);
});
//////////////////realsense END///////////////////////
//////////////////realsense START///////////////////////
rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd2_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera2/camera2/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
if(cfg2.k_arr.size()==0||cfg2.d_arr.size()==0){
cfg2.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
cfg2.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
cfg2.arr_copy+=1;
cout<<"copy:"<<"k:"<<cfg2.k_arr[0]<<"d:"<<cfg2.d_arr[0]<<endl;
}
cur_node->pub_msg(msg.rgb,msg.depth,cfg3);
});
//////////////////realsense END///////////////////////
rclcpp::spin(cur_node);
rclcpp::shutdown();
return 0;

1
imu_dev/CMakeLists.txt
View File

@@ -24,6 +24,7 @@ target_link_libraries(imu_dev_node
${PROJECT_NAME}__rosidl_typesupport_c
)
install(TARGETS imu_dev_node DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights

22
imu_dev/launch/imu_dev.launch.py Normal file
View File

@@ -0,0 +1,22 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='imu',
executable='imu_node',
name='imu_node',
parameters=[{
'port_name': '/dev/ttyUSB0',
}]
),
Node(
package='imu_dev',
executable='imu_dev_node',
name='imu_dev_node',
output='screen'
)
])

2
input_dev/launch/input_dev.launch.py
View File

@@ -12,7 +12,7 @@ def generate_launch_description():
'ros2',
'launch',
'game_pad',
'gampe_pad.launch.py'
'game_pad.launch.py'
],
output='screen'
),

10
livox_ros_driver2/CMakeLists.txt
View File

@@ -256,9 +256,9 @@ else(ROS_EDITION STREQUAL "ROS2")
NO_DEFAULT_PATH
REQUIRED)
##
find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
NAMES "livox_lidar_api.h" "livox_lidar_def.h"
REQUIRED)
##find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
# NAMES "livox_lidar_api.h" "livox_lidar_def.h"
# REQUIRED)
## PCL library
link_directories(${PCL_LIBRARY_DIRS})
@@ -303,7 +303,6 @@ else(ROS_EDITION STREQUAL "ROS2")
target_include_directories(${PROJECT_NAME} PUBLIC
${PCL_INCLUDE_DIRS}
${APR_INCLUDE_DIRS}
${LIVOX_LIDAR_SDK_INCLUDE_DIR}
${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs
3rdparty
src
@@ -324,6 +323,7 @@ else(ROS_EDITION STREQUAL "ROS2")
EXECUTABLE ${PROJECT_NAME}_node
)
install(FILES lib/liblivox_lidar_sdk_shared.so DESTINATION lib)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
@@ -338,7 +338,5 @@ else(ROS_EDITION STREQUAL "ROS2")
ament_auto_package(INSTALL_TO_SHARE
config
launch
lib
)
endif()

10
livox_ros_driver2/config/MID360_config.json
View File

@@ -11,13 +11,13 @@
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.55",
"cmd_data_ip" : "192.168.1.50",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.55",
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.55",
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.55",
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
@@ -25,7 +25,7 @@
},
"lidar_configs" : [
{
"ip" : "192.168.1.191",
"ip" : "192.168.1.164",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {

BIN
livox_ros_driver2/lib/liblivox_lidar_sdk_shared.so
View File

Binary file not shown.

2
livox_ros_driver2/src/lds.cpp
View File

@@ -160,7 +160,7 @@ void Lds::StoragePointData(PointFrame* frame) {
uint8_t index = 0;
int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index);
if (ret != 0) {
printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle);
printf("Storage point data failed, lidar type:%u, handle:%u.%s\n", lidar_point.lidar_type, lidar_point.handle,IpNumToString(lidar_point.handle).c_str());
continue;
}
PushLidarData(&lidar_point, index, base_time);

33
motor_dev/CMakeLists.txt Normal file
View File

@@ -0,0 +1,33 @@
cmake_minimum_required(VERSION 3.8)
project(motor_dev)
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)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
include_directories(include ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp)
add_executable(motor_dev_node src/main.cpp src/rs485_driver.cpp)
add_dependencies(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
target_link_libraries(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp )
ament_target_dependencies(motor_dev_node rclcpp std_msgs)
install(TARGETS motor_dev_node DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

64
motor_dev/include/motor_dev/rs485_driver.hpp Normal file
View File

@@ -0,0 +1,64 @@
#ifndef RS485_DRIVER_HPP
#define RS485_DRIVER_HPP
#include <string>
#include <vector>
#include <termios.h>
#include <mutex>
namespace motor_dev
{
class RS485Driver
{
public:
RS485Driver();
~RS485Driver();
// 打开串口
bool openPort(const std::string &port_name, int baud_rate = 115200);
// 关闭串口
void closePort();
// 发送数据
bool sendData(const std::vector<uint8_t> &data);
// 接收数据
bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
//本末
int bm_set_mode(uint8_t motor_id, uint8_t mode);
int bm_set_pos(uint8_t motor_id, float angle);
float bm_get_pos(uint8_t motor_id);
int bm_query_id();
int mt_query_id();
//脉塔
void add_crc16(std::vector<uint8_t> &data);
int mt_set_pos(uint8_t motor_id, float angle);
float mt_get_pos(uint8_t motor_id);
private:
int com_fd_;
//bool is_open_;
struct termios original_termios_;
std::mutex uart_mutex;
//size_t max_motors_;
//size_t control_index_;
// 转换波特率
speed_t convertBaudRate(int baud_rate);
// 电机协议常量
// static constexpr uint8_t START_BYTE = 0x64;
// static constexpr uint8_t STOP_BYTE = 0x55;
// static constexpr uint8_t CONTROL_CMD = 0x64;
// static constexpr uint8_t ZERO_CMD = 0x00;
// static constexpr uint8_t SPEED_CMD = 0x14;
// static constexpr uint8_t DEFAULT_TIME = 0x64;
// static constexpr uint8_t STOP_CMD = 0x02;
// static constexpr uint8_t ALL_MOTORS_CMD = 0x03;
};
}
#endif // RS485_DRIVER_HPP

5
motor_dev/msg/MotorCmd.msg Normal file
View File

@@ -0,0 +1,5 @@
string target
string type
string position
int32[] motor_id
float32[] motor_angle

21
motor_dev/package.xml Normal file
View File

@@ -0,0 +1,21 @@
<?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>motor_dev</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="you@example.com">h</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>std_msgs</depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

60
motor_dev/src/main.cpp Normal file
View File

@@ -0,0 +1,60 @@
#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"motor_dev/msg/motor_cmd.hpp"
using namespace motor_dev;
RS485Driver rs485_driver_;
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);
auto node=rclcpp::Node::make_shared("motor_dev_node");
////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
std::cout << "open RS485 port.." << std::endl;
if (!rs485_driver_.openPort("/dev/ttyUSB0", 115200)){
std::cout << "Failed to open RS485 port." << std::endl;
}
//rs485_driver_.mt_set_pos(1,3456);
//usleep(2000000);
//rs485_driver_.mt_set_pos(1,-5);
//init
/////rs485_driver_.bm_set_mode(1,3);//位置模式
auto rs485_sub_=node->create_subscription<motor_dev::msg::MotorCmd>("/motor_cmd",10,[](const motor_dev::msg::MotorCmd::SharedPtr msg){
size_t n = msg->motor_id.size();
if (msg->target == "rs485") {
for (size_t i = 0; i < n; ++i) {
int32_t id = msg->motor_id[i];
float angle =msg->motor_angle[i];
std::cout<<"recv:"<<msg->type<<","<<id<<","<<angle<<std::endl;
if(msg->type=="bm"){
rs485_driver_.bm_set_pos(id,angle);
}else if(msg->type=="mt"){
rs485_driver_.mt_set_pos(id,angle);
}
}
}else if(msg->target=="ethercat"){
//TODO
}
});
//读取电机位置
rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(200), [=]() {
//rs485_driver_.bm_set_pos(id);
//rs485_driver_.mt_get_pos(id);
///rs485_driver_.bm_query_id();
//rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95
float angle=rs485_driver_.mt_get_pos(1);
if(angle!=9999)
printf("angle:%.1f\n",angle);
// MotorPos motor_pos;
// input_msg->source = cfg.getSource();
// input_msg->type = cfg.getType();
// input_msg->position=cfg.getPosition();
// input_msg->code=key;
// input_msg->value=val;
// input_pub->publish(*input_msg);
// pub_cnt+=1;
// std::cout<<"pub msg["<<pub_cnt<<"]:"<<key<<std::endl;
});
////rs485_driver_.closePort();
rclcpp::spin(node);
rclcpp::shutdown();
}

439
motor_dev/src/rs485_driver.cpp Normal file
View File

@@ -0,0 +1,439 @@
#include "motor_dev/rs485_driver.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <chrono>
#include <thread>
#include "motor_dev/rs485_driver.hpp"
namespace motor_dev
{
const unsigned char CRC8Table[]={
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
};
/**
* @name CRC8_Table
* @brief This function is used to Get CRC8 check value.
* @param p :Array pointer. counter :Number of array elements.
* @retval crc8
*/
unsigned char CRC8_Table(unsigned char *p, int counter)
{
unsigned char crc8 = 0;
for( ; counter > 0; counter--)
{
crc8 = CRC8Table[crc8^*p];
p++;
}
return(crc8);
}
RS485Driver::RS485Driver()
{
}
RS485Driver::~RS485Driver()
{
closePort();
}
speed_t RS485Driver::convertBaudRate(int baud_rate)
{
switch (baud_rate)
{
case 9600: return B9600;
case 19200: return B19200;
case 38400: return B38400;
case 57600: return B57600;
case 115200: return B115200;
case 230400: return B230400;
case 460800: return B460800;
case 500000: return B500000;
case 576000: return B576000;
case 921600: return B921600;
default: return B0;
}
}
bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
{
// 打开串口
//com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY);
if (com_fd_ == -1)
{
std::cerr << "无法打开串口: " << port_name << std::endl;
return false;
}
// 保存原始配置
if (tcgetattr(com_fd_, &original_termios_) == -1)
{
std::cerr << "无法获取串口属性" << std::endl;
close(com_fd_);
com_fd_ = -1;
return false;
}
// 配置串口
struct termios tty;
memset(&tty, 0, sizeof(tty));
// 设置波特率
speed_t speed = convertBaudRate(baud_rate);
if (speed == B0)
{
std::cerr << "不支持的波特率: " << baud_rate << std::endl;
closePort();
return false;
}
cfsetospeed(&tty, speed);
cfsetispeed(&tty, speed);
// 8位数据位，无奇偶校验，1位停止位
tty.c_cflag &= ~PARENB; // 无奇偶校验
tty.c_cflag &= ~CSTOPB; // 1位停止位
tty.c_cflag &= ~CSIZE; // 清除数据位设置
tty.c_cflag |= CS8; // 8位数据位
// 禁用硬件流控制
tty.c_cflag &= ~CRTSCTS;
// 启用接收器，设置本地模式
tty.c_cflag |= (CLOCAL | CREAD);
// 禁用软件流控制
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
// 原始输入模式
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// 原始输出模式
tty.c_oflag &= ~OPOST;
// 设置读取超时
tty.c_cc[VTIME] = 10; // 1秒超时
tty.c_cc[VMIN] = 0;
// 应用配置
if (tcsetattr(com_fd_, TCSANOW, &tty) != 0)
{
std::cerr << "无法设置串口属性" << std::endl;
closePort();
return false;
}
std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
return true;
}
void RS485Driver::closePort()
{
if (com_fd_>0)
{
// 恢复原始配置
tcsetattr(com_fd_, TCSANOW, &original_termios_);
close(com_fd_);
com_fd_ = -1;
std::cout << "串口已关闭" << std::endl;
}
}
bool RS485Driver::sendData(const std::vector<uint8_t> &data)
{
ssize_t bytes_written = write(com_fd_, data.data(), data.size());
if (bytes_written != static_cast<ssize_t>(data.size()))
{
std::cerr << "rs485,fd:"<<com_fd_<<",发送ERR:" <<bytes_written<<"/"<<data.size()<<std::endl;
return false;
}
return true;
}
bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
{
data.clear();
uint8_t buffer[256];
ssize_t bytes_read;
auto start_time = std::chrono::steady_clock::now();
static int idx=0;
idx+=1;
while (data.size() < max_length)
{
// 检查超时
auto current_time = std::chrono::steady_clock::now();
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
current_time - start_time).count();
if (elapsed_ms >= timeout_ms)
{
break; // 超时
}
// 尝试读取数据
bytes_read = read(com_fd_, buffer, std::min(sizeof(buffer), max_length - data.size()));
if (bytes_read > 0)
{
data.insert(data.end(), buffer, buffer + bytes_read);
start_time = std::chrono::steady_clock::now(); // 重置超时计时器
}
else if (bytes_read < 0)
{
std::cerr <<idx<< "接收数据错误" << std::endl;
return false;
}
else
{
// 没有数据，短暂休眠
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
return !data.empty();
}
int RS485Driver::bm_query_id(){
std::vector<uint8_t> command;
command.push_back(0);
command.push_back(0x36);
command.push_back(1);
command.push_back(0x1c);
command.push_back(4);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
for(auto d:command)
printf("%02x ",d);
std::cout<<std::endl;
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
bool ret=receiveData(data, 20, 1000);
if(ret){
printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
}
}
return 0;
}
int RS485Driver::bm_set_mode(uint8_t motor_id, uint8_t mode){
std::vector<uint8_t> command;
if(mode>3)
mode=3;
command.push_back(motor_id);
command.push_back(0xa0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(mode);
return 0;
}
int RS485Driver::bm_set_pos(uint8_t motor_id, float angle){
std::vector<uint8_t> command;
if(angle>360.0)
angle=360.0;
int32_t pos=angle*32767/360.0;
uint8_t high_byte = (pos >> 8) & 0xFF; // 高8位
uint8_t low_byte = pos & 0xFF; // 低8位
command.push_back(motor_id);
command.push_back(0x64);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(0);
command.push_back(0);
command.push_back(0x64);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
bool ret = sendData(command);
if(!ret)
printf("bm send err!\n");
return 0;
}
float RS485Driver::bm_get_pos(uint8_t motor_id){
std::vector<uint8_t> command;
command.push_back(motor_id);
command.push_back(0x74);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
command.push_back(0);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
bool ret = sendData(command);
if(ret){
std::vector<uint8_t> data;
if(ret){
//uint8_t pos = data[7];
//float angle = pos * 360.0 /255.0;
float angle=0;
return angle;
}
}
return -1;
}
//mt
void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
uint16_t crc = 0xFFFF;
for (uint8_t byte : data) {
crc ^= byte;
for (int i = 0; i < 8; i++) {
if (crc & 0x0001) {
crc >>= 1;
crc ^= 0xA001;
} else {
crc >>= 1;
}
}
}
data.push_back(static_cast<uint8_t>(crc & 0xFF));
data.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF));
}
//
int RS485Driver::mt_set_pos(uint8_t motor_id, float angle){
std::lock_guard<std::mutex> lock(uart_mutex);
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
int32_t pos = static_cast<int32_t>(angle * 100);
packet.push_back(0xa4);
packet.push_back(0);//方向
packet.push_back(0x10);
packet.push_back(0x00);//速度
packet.push_back((uint8_t)(pos));//位置,pos的低8位
packet.push_back((uint8_t)(pos>>8));//位置，pos的高8位
packet.push_back((uint8_t)(pos>>16));
packet.push_back((uint8_t)(pos>>24));
add_crc16(packet);
// printf("send:");
// for(auto d:packet)
// printf("%02x ",d);
// std::cout<<std::endl;
bool ret=sendData(packet);
if(!ret)
printf("send error\n");
return 0;
}
float RS485Driver::mt_get_pos(uint8_t motor_id){
std::lock_guard<std::mutex> lock(uart_mutex);
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(motor_id);
packet.push_back(0x08);
//
packet.push_back(0x92);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
add_crc16(packet);
// printf("send:");
// for(auto d:packet)
// printf("%02x ",d);
// std::cout<<std::endl;
bool ret=sendData(packet);
if(ret){
#if 1
std::vector<uint8_t> data;
data.clear();
ret=receiveData(data, 20, 10);
// printf("recv:");
// for(auto d:data)
// printf("%02x ",d);
// printf("\n");
if(ret){
if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
///printf("pos:%d\n",pos);
float angle = pos /100.0;
return angle;
}
return 9999;
}
#else
static uint8_t com_buf[128];
memset(com_buf,0,128);
ret = read(com_fd_, com_buf, 128);
///bool ret=receiveData(data, 20, 1000);
printf("recv:");
for(int i=0;i<ret;i++)
printf("%02x ",com_buf[i]);
printf("\n");
#endif
}
return 9999;
}
int RS485Driver::mt_query_id(){
std::vector<uint8_t> packet;
packet.push_back(0x3e);
packet.push_back(0xcd);
packet.push_back(0x08);
//
packet.push_back(0x79);
packet.push_back(0);
packet.push_back(1);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
packet.push_back(0);
add_crc16(packet);
// for(auto d:packet)
// printf("%02x ",d);
// std::cout<<std::endl;
bool ret=sendData(packet);
if(ret){
std::vector<uint8_t> data;
ret=receiveData(data, 20, 1000);
if(ret){
uint8_t id= data[10];
return id;
}
}
return -1;
}
}

0
orbbec/orbbec_camera/.make_deb.sh Normal file → Executable file
View File

2
orbbec/orbbec_camera/launch/multi_camera.launch.py
View File

@@ -16,7 +16,7 @@ def generate_launch_description():
),
launch_arguments={
'camera_name': 'camera_01',
'usb_port': '2-3',
'usb_port': '1-5',
'device_num': '2',
'sync_mode': 'standalone',
'enable_left_ir': 'true',