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

adapte robot

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-16 09:01:58 +08:00
parent 89b839c2cc
commit dedc31a46e
14 changed files with 50 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

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);

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',