diff --git a/hi-pilot/README.md b/hi-pilot/README.md index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..c32fb4b38c46dacee9a3ade90efc17ff508df727 100644 --- a/hi-pilot/README.md +++ b/hi-pilot/README.md @@ -0,0 +1,127 @@ +# 基于openEuler Embedded和海鸥派开发自动驾驶小车 + +## 目录 + + +- [hi-pilot](#) + - [项目结构](#to-do-list) + - [开发环境搭建](#) + - [x86环境]() + - [海鸥派环境]() + - [硬件组装]() + - [镜像烧录]() + - [系统配置]() + - [项目复现]() + - [ROS2软件包编译]() + - [可执行文件传输]() + - [上位机观测]() + - [车端运行]() + - [遥控移动]() + - [2D雷达数据]() + - [cart建图]() + - [导航]() + + + +# 项目结构 + +## ROS2 package结构 + +# 开发环境搭建 +所需资源来自上交大openEuler镜像站 [hieulerpi1-ros](https://mirror.sjtu.edu.cn/openeuler/openEuler-24.03-LTS/embedded_img/aarch64/hieulerpi1-ros/) +# Windows-海鸥派系统烧录工具 +ToolPlatform软件 +# openEuler-x86开发环境 +> 开发用x86环境部署于WSL2-openEuler-23.04中，该步骤于WSL2中完成 +1. 下载SDK工具链sh文件，添加执行权限 +``` +wget https://mirror.sjtu.edu.cn/openeuler/openEuler-24.03-LTS/embedded_img/aarch64/hieulerpi1-ros/openeuler-glibc-x86_64-openeuler-image-aarch64-hieulerpi1-toolchain-24.03-LTS.sh +chmod +x *.sh +``` +2. 安装SDK工具链 + +> 在容器中，用户名变为hieulerpi + +# 硬件组装 +将海鸥派与小车驱动板通过电源与RS485连接 + +# 镜像烧录 +在Windows系统下，下载ToolPlatform软件，将海鸥派调试USB-C串口与PC连接（需安装CH340驱动），海鸥派插入网线与PC连接至同一局域网下 + +1. PC与板端配置中，选择海鸥派对应串口，服务器IP选择局域网IP，板端IP末尾为0，网关为路由器网关，传输方式选择网口 + +2. 界面中间选择标签页“烧写eMMC”，浏览并打开参考分区表文件partTable.xml + + 列表中需包含4个项目，均可从 [上交大openEuler镜像站]() 中下载，对应参考如下 + + |名称|文件|器件类型|文件系统|开始地址|长度| + |---|-|-|-|-|-| + |u-boot|boot_image.bin|emmc|none|0|1M| + |env|boot_env.bin|emmc|none|1M|1M| + |kernel|kernel-pi|emmc|none|2M|16M| + |rootfs|*.rootfs.ext4|emmc|ext3/4|18M|-| +3. 随后点击“烧写”，按住板子UP的同时短按RST，底部输出有进度条即表示已开始烧写固件 + + 固件写入过程可能会耗时多达40分钟，请耐心等待 +4. 固件烧录后系统自动重启（也可手动复位），首次启动会要求设置复杂密码，用户名为默认root，请务必牢记 + +# 系统配置 +- 修改串口控制台超时 +目的: 避免控制台5分钟无操作自动登出 +修改文件/etc/profile，将字段TMOUT修改值为0 + +- 分区大小扩展 +目的: 系统烧录后系统分区大小仅为8GB,扩展根目录所在分区为最大存储大小 +执行`resize2fs /dev/root` + +- eth0/eth1有线联网 +修改网络控制配置文件`/etc/network/interfaces` +修改文件内eth0、eth1相关字段 + ``` + # eth0-静态ip + auto eth0 + iface eth0 inet static + address 192.168.2.124 + netmask 255.255.255.0 + network 192.168.2.0 + gateway 192.168.2.1 + + # eth1-DHCP + auto eth1 + iface eth1 inet dhcp + ``` + +- WiFi联网 +存在问题:WiFi断开后,串口控制台会输出网卡错误信息,且不会自动重连 + +# 构建运行 +# ROS2软件包编译构建 +拉取本仓库，在此 ws_eulercar 目录下，执行命令 + +``` +colcon build --cmake-args -DBUILD_TESTING=False +``` + +等待编译构建通过 + +# 编译后可执行文件传输 +```bash +# 于交叉编译环境 openEuler x86 中 +scp -r ws_eulercar root@IP:/root/ +# 将ROS工作空间传输至海鸥派中，将IP替换为海鸥派网卡IP +``` +由于交叉编译环境与运行环境中ROS2工作空间不同，需修改生成文件中的相关目录字段 + +进入ws_eulercar/install目录，使用如下脚本： +``` +cd ws_eulercar/install +find ./ -type f -exec sed -i 's@/opt/buildtools/nativesdk/sysroots/x86_64-openeulersdk-linux/usr/bin/python3@/usr/bin/python3@g' {} + +``` +# 车端运行 +1. ssh 登入eulerpi +2. 执行以下脚本（每次新开终端/编译变更时均需要） +```bash +source /etc/profile.d/ros/setup.bash # 设置系统中ROS2环境变量 +source /root/ws_eulercar/install/setup.sh # 设置eulercar的ROS2环境变量 +``` +## 遥控 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/CMakeLists.txt b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..fef3b4974936d510669275f06ffb27b762360baf --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_hieuler_robot) + +# 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(actionlib REQUIRED) +#find_package(actionlib_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(serial REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Position.msg" + ) + +include_directories( + include +) +set(hieuler_robot_node_SRCS + src/hieuler_robot.cpp + src/Quaternion_Solution.cpp +) + +add_executable(hieuler_robot_node src/hieuler_robot.cpp src/Quaternion_Solution.cpp) +ament_target_dependencies(hieuler_robot_node tf2_ros tf2 rclcpp std_msgs nav_msgs std_srvs sensor_msgs serial tf2_geometry_msgs) + +install(TARGETS +hieuler_robot_node + +DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY launch urdf + DESTINATION share/${PROJECT_NAME} +) +ament_package() \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/README.md b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/README.md new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/Quaternion_Solution.h b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/Quaternion_Solution.h new file mode 100644 index 0000000000000000000000000000000000000000..466146e25067d0acfbdf6c0f17874e5a36572af9 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/Quaternion_Solution.h @@ -0,0 +1,13 @@ +#ifndef __QUATERNION_SOLUTION_H_ +#define __QUATERNION_SOLUTION_H_ +#include "hieuler_robot.h" +#define SAMPLING_FREQ 20.0f // 采样频率 +#define PARAMETER1 1.5F +#define PARAMETER2 0.5F +#define PARAMETER3 0x5f375a86 +#define PARAMETER4 1 +#define PARAMETER 1.0f +float InvSqrt(float number); +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az); + +#endif \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/hieuler_robot.h b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/hieuler_robot.h new file mode 100644 index 0000000000000000000000000000000000000000..0c985a3f45b0cb47057e4bfd1645f34514e96a65 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/include/hieuler_robot.h @@ -0,0 +1,187 @@ +#ifndef __HIEULER_ROBOT_H_ +#define __HIEULER_ROBOT_H_ +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2/LinearMath/Transform.h" +#include "tf2/LinearMath/Quaternion.h" +#include +#include +#include +using namespace std; +#define FRAME_HEADER 0X7B //帧头 +#define FRAME_TAIL 0X7D //帧尾 +#define INTERVAL_HEADER 0X7C //Frame_header //间隔帧头 +#define INTERVAL_TAIL 0X7F //Frame_tail //间隔帧尾 + +#define RECEIVE_DATA_SIZE 24 //下位机发送过来的数据的长度 +#define SEND_DATA_SIZE 11 //TROS向下位机发送的数据的长度 +#define SEND_DATA_CHECK 1 +#define READ_DATA_CHECK 0 +#define DEL 8 +#define BAUD_RATE 9600 +#define MESSAGE_LENGTH 10 +#define GYROSCOPE_RATIO 0.00026644f +#define ACCEl_RATIO 1671.84f +#define MAGNIFICATION 1000 +#define SAMPLING_TIME 0.048 +#define COVARIANCE_6 1e6 +#define COVARIANCE_F6 1e-6 +#define COVARIANCE_12 1e12 +#define COVARIANCE_F5 1e-5 +#define COVARIANCE_F3 1e-3 +#define TIMEOUT 2000 +#define SENDMESSAGE_BUF 100 +#define COVARIANCE_O 0 +#define COVARIANCE_F 4 +#define COVARIANCE_S 7 +#define COVARIANCE_E 8 +#define COVARIANCE_FT 14 +#define COVARIANCE_TO 21 +#define COVARIANCE_FE 28 +#define COVARIANCE_TF 35 +#define CHECK_NUM 9 +#define VEL_MAX 300 +#define VEL_MIN -300 + +extern sensor_msgs::msg::Imu g_mpuData; //外部变量，IMU话题数据 +//协方差矩阵，用于里程计话题数据，用于robt_pose_ekf功能包 +const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9 }; + +const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e3 }; + +const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, + 0, 1e-3, 1e-9, 0, 0, 0, + 0, 0, 1e6, 0, 0, 0, + 0, 0, 0, 1e6, 0, 0, + 0, 0, 0, 0, 1e6, 0, + 0, 0, 0, 0, 0, 1e-9} ; + +//速度、位置数据结构体 +typedef struct RobotPositionDate +{ + float x; + float y; + float z; +}RobPosData; + +//IMU数据结构体 +typedef struct ImuMpuData +{ + short xAcceleData; + short yAcceleData; + short zAcceleData; + short xGyrosData; + short yGyrosData; + short zGyrosData; +}Mpu6050Data; + +//ROS向下位机发送数据的结构体 +typedef union ToUploadData +{ + unsigned char buffer[SEND_DATA_SIZE]; + unsigned char headBuf; + float xSpeed; + float ySpeed; + float zSpeed; + unsigned char endBuf; +}UploadData; + + +//下位机向ROS发送数据的结构体 +typedef struct ReceiveMcuData +{ + uint8_t rx[RECEIVE_DATA_SIZE]; + uint8_t flagStop; + unsigned char frameHeader; + float xSpeed; + float ySpeed; + float zSpeed; + unsigned char frameTail; +}RecData; + +//机器人底盘类，使用构造函数初始化数据和发布话题等 +class ControlRobot : public rclcpp::Node +{ + public: + ControlRobot(); + ~ControlRobot(); ///析构函数 + void CycleControl(); //循环控制代码 + void UpdateTf(geometry_msgs::msg::TransformStamped::SharedPtr OdomTf); + void PublishOdom(); //发布里程计话题 + serial::Serial StmSerial; //声明串口对象 + + private: + int count = 0; + int recvCount = 0; + int sendCount = 0; + rclcpp::TimerBase::SharedPtr serial_timer_; + rclcpp::Subscription::SharedPtr CmdVelSub; + rclcpp::Publisher::SharedPtr VoltagePublisher; + rclcpp::Publisher::SharedPtr OdomPublisher; + rclcpp::Publisher::SharedPtr ImuPublisher; + rclcpp::Publisher::SharedPtr TestPublisher; + rclcpp::Publisher::SharedPtr TfPub; + std::shared_ptr TfBro; + rclcpp::TimerBase::SharedPtr TestTimer; + rclcpp::TimerBase::SharedPtr OdomTimer; + rclcpp::TimerBase::SharedPtr ImuTimer; + rclcpp::TimerBase::SharedPtr VoltageTimer; + rclcpp::TimerBase::SharedPtr RobotposeTimer; + rclcpp::TimerBase::SharedPtr RobotvelTimer; + void CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr twistAux); + void SerialSendCallback(); + void PublishImuSensor(); //发布IMU传感器话题 + void PublishVoltage(); //发布电源电压话题 + short VelLimit(short velocity); + bool GetSensorData(); + short DataTrans(uint8_t dataHigh, uint8_t dataLow); + float OdomTrans(uint8_t dataHigh, uint8_t dataLow); + unsigned char CheckSum(unsigned char Count_Number, unsigned char mode); + string usartPortName, robotFrameId, gyroFrameId, odomFrameId; //定义相关变量 + std::string cmd_vel; + float powerVoltage; //电源电压 + int serialBaudRate; //串口通信波特率 + RecData ReceiveData; //串口接收数据结构体 + UploadData SendStr; //串口发送数据结构体 + RobPosData RobotPos; //机器人的位置 + RobPosData RobotVel; //机器人的速度 + Mpu6050Data MpuData; //IMU数据 +}; +#endif \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/launch/ros2_hieuler_robot.launch.py b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/launch/ros2_hieuler_robot.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..60ea3873a844fe140f4c3bcd4a737616e2d04f70 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/launch/ros2_hieuler_robot.launch.py @@ -0,0 +1,60 @@ +import os +from pathlib import Path +import launch +from launch.actions import SetEnvironmentVariable +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import(DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import PushRosNamespace +from launch_ros.actions import Node +from launch.conditions import UnlessCondition + +def generate_launch_description(): + hieuler_robot = Node( + package='ros2_hieuler_robot', + executable='hieuler_robot_node', + name='hieuler_robot', + parameters=[{'usartPortName':'/dev/ttyAMA3', + 'serialBaudRate':9600, + 'robotFrameId':'base_footprint', + 'odomFrameId':'odom', + 'cmd_vel':'cmd_vel',}], + ) + + sllidar_dir = get_package_share_directory('wr_ls_udp') + sllidar_launch_dir = os.path.join(sllidar_dir, 'launch') + sllidar = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(sllidar_launch_dir, 'wr_ls1207de_udp_with_1_lidar_launch.py')), + ) + + astra_camera_dir = get_package_share_directory('astra_camera') + astra_camera_launch_dir = os.path.join(astra_camera_dir, 'launch') + astra_camera_dabai = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(astra_camera_launch_dir, 'dabai.launch.py')), + ) + + ## urdf + package_dir = get_package_share_directory('ros2_hieuler_robot') + urdf_path = os.path.join(package_dir, 'urdf', 'hieuler.urdf') + with open(urdf_path, 'r') as infp: + robot_desc = infp.read() + + robot_state_publisher_node = Node( + package = 'robot_state_publisher', + executable = 'robot_state_publisher', + parameters=[ + {'robot_description': robot_desc}, + {'use_sim_time': False}], + output = 'screen' + ) + + ld = LaunchDescription() + ld.add_action(robot_state_publisher_node) + #ld.add_action(sllidar) + #ld.add_action(astra_camera_dabai) + ld.add_action(hieuler_robot) + + return ld \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/msg/Position.msg b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/msg/Position.msg new file mode 100644 index 0000000000000000000000000000000000000000..2fe26971dc4d97a2757bd07af5648103393f5572 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/msg/Position.msg @@ -0,0 +1,3 @@ +float32 angle_x +float32 angle_y +float32 distance diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/package.xml b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..bf175e6e1e60c8de9a6480732c1356f6e498aa10 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/package.xml @@ -0,0 +1,37 @@ + + + + ros2_hieuler_robot + 0.0.0 + ROS2 ros2_control_robot for control_robot + control + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + rosidl_default_generators + rosidl_interface_packages + rosidl_default_runtime + ament_index_cpp + geometry_msgs + rclcpp + rclcpp_action + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_ros + rclpy + serial + nav2_msgs + nav_msgs + tf2_geometry_msgs + boost + + ament_cmake + + \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/Quaternion_Solution.cpp b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/Quaternion_Solution.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7a70c0260a592a4e468b0b681c52628656d8ffbe --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/Quaternion_Solution.cpp @@ -0,0 +1,87 @@ +#include "Quaternion_Solution.h" + +/************************************** +功能: 平方根倒数 求四元数用到 +***************************************/ +float InvSqrt(float number) +{ + volatile long i; + volatile float x, y; + volatile const float f = PARAMETER1; + x = number * PARAMETER2; + y = number; + i = * (( long * ) &y); + i = PARAMETER3 - ( i >> PARAMETER4 ); + y = * (( float * ) &i); + y = y * ( f - ( x * y * y ) ); + return y; +} + +/************************************** +功能: 四元数解算 +***************************************/ +volatile float twoKp = PARAMETER; // 2 * 比例增益 (Kp) +volatile float twoKi = 0.0f; // 2 * 积分增益 (Ki) +volatile float q0 = PARAMETER, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数 +volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // Ki缩放的积分误差项 +void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az) +{ + float recipNorm; + float halfVx, halfVy, halfVz; + float halfEx, halfEy, halfEz; + float qa, qb, qc; + // 仅在加速度计测量有效时计算反馈 + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模 + recipNorm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + // 把四元数换算成方向余弦中的第三行的三个元素 + halfVx = q1 * q3 - q0 * q2; + halfVy = q0 * q1 + q2 * q3; + halfVz = q0 * q0 - PARAMETER2 + q3 * q3; + //误差是估计的重力方向和测量的重力方向的交叉乘积之和 + halfEx = (ay * halfVz - az * halfVy); + halfEy = (az * halfVx - ax * halfVz); + halfEz = (ax * halfVy - ay * halfVx); + // 计算并应用积分反馈 + if(twoKi > 0.0f) { + integralFBx += twoKi * halfEx * (PARAMETER / SAMPLING_FREQ); + integralFBy += twoKi * halfEy * (PARAMETER / SAMPLING_FREQ); + integralFBz += twoKi * halfEz * (PARAMETER / SAMPLING_FREQ); + gx += integralFBx; + gy += integralFBy; + gz += integralFBz; + } + else { + integralFBx = 0.0f; + integralFBy = 0.0f; + integralFBz = 0.0f; + } + gx += twoKp * halfEx; + gy += twoKp * halfEy; + gz += twoKp * halfEz; + } + // 四元数积分变化率 + gx *= (PARAMETER2 * (PARAMETER / SAMPLING_FREQ)); + gy *= (PARAMETER2 * (PARAMETER / SAMPLING_FREQ)); + gz *= (PARAMETER2 * (PARAMETER / SAMPLING_FREQ)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + // 归一化四元数 + recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + g_mpuData.orientation.w = q0; + g_mpuData.orientation.x = q1; + g_mpuData.orientation.y = q2; + g_mpuData.orientation.z = q3; +} diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/hieuler_robot.cpp b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/hieuler_robot.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ae460ae4cdcff28fa178f7aeea989b49a711daaf --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/src/hieuler_robot.cpp @@ -0,0 +1,445 @@ +#include "hieuler_robot.h" +#include "rclcpp/rclcpp.hpp" +#include "Quaternion_Solution.h" + +#include + +using std::placeholders::_1; +using namespace std; +using namespace std::chrono_literals; + +sensor_msgs::msg::Imu g_mpuData; +rclcpp::Node::SharedPtr NodeHandle = nullptr; + +short ControlRobot::DataTrans(uint8_t dataHigh, uint8_t dataLow) +{ + short dataTransition = 0; + dataTransition |= dataHigh << DEL; + dataTransition |= dataLow; + return dataTransition; +} + +float ControlRobot::OdomTrans(uint8_t dataHigh, uint8_t dataLow) +{ + float dataReturn; + short dataTransition = 0; + dataTransition |= dataHigh << DEL; + dataTransition |= dataLow; + dataReturn = (dataTransition / MAGNIFICATION) + (dataTransition % MAGNIFICATION) * 0.001; + return dataReturn; +} + +unsigned char ControlRobot::CheckSum(unsigned char Count_Number, unsigned char mode) +{ + unsigned char checkSum = 0; + int k; + /*接收模式*/ + if (mode == 0) + { + for (k = 0; k < Count_Number; k++) + { + checkSum = checkSum ^ ReceiveData.rx[k]; + } + } + /*发送模式*/ + if (mode == 1) + { + for (k = 0; k < Count_Number; k++) + { + checkSum = checkSum ^ SendStr.buffer[k]; + } + } + return checkSum; +} + +short ControlRobot::VelLimit(short velocity) +{ + return velocity = velocity > VEL_MAX ? VEL_MAX : (velocity < VEL_MIN ? VEL_MIN : velocity); +} + +void ControlRobot::SerialSendCallback() +{ + unsigned char buffer[16]; + RCLCPP_INFO(rclcpp::get_logger("teleop_node"), "serial_test_cb"); + + buffer[0] = FRAME_HEADER; + for(int i=1;i<10;i++) + buffer[i] = i; + buffer[10] = FRAME_TAIL; + + StmSerial.write(SendStr.buffer, SEND_DATA_SIZE); +} + +void ControlRobot::CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr twistAux) +{ + short transition; + SendStr.buffer[0] = FRAME_HEADER; + SendStr.buffer[1] = 0; + SendStr.buffer[2] = 0; + transition = 0; + transition = twistAux->linear.x * MAGNIFICATION; + //transition = VelLimit(transition); + SendStr.buffer[4] = transition; + SendStr.buffer[3] = transition >> DEL; + transition = 0; + transition = twistAux->linear.y * MAGNIFICATION; + //transition = VelLimit(transition); + SendStr.buffer[6] = transition; + SendStr.buffer[5] = transition >> DEL; + transition = 0; + transition = twistAux->angular.z * MAGNIFICATION; + SendStr.buffer[8] = transition; + SendStr.buffer[7] = transition >> DEL; + SendStr.buffer[9] = CheckSum(CHECK_NUM, SEND_DATA_CHECK); + if ((SendStr.buffer[9] == FRAME_HEADER) && (transition != 0)) + { + SendStr.buffer[4] = SendStr.buffer[4] ^ 1; + SendStr.buffer[9] = SendStr.buffer[9] ^ 1; + } + SendStr.buffer[10] = FRAME_TAIL; + + // for(int i=0;i<11;i++) + // SendStr.buffer[i] = i; + + RCLCPP_INFO(this->get_logger(), "recv topic(linear.x:%f linear.y:%f angular.z:%f)", twistAux->linear.x, twistAux->linear.y, twistAux->angular.z); + + char debugBuffer[SEND_DATA_SIZE * 5] = {0}; + char *debugBufferPtr = debugBuffer; + for (size_t i = 0; i < SEND_DATA_SIZE; ++i) + { + debugBufferPtr += sprintf(debugBufferPtr, "0x%02x ", SendStr.buffer[i]); + } + debugBuffer[SEND_DATA_SIZE * 5 - 1] = '\0'; + RCLCPP_INFO(this->get_logger(), "SendData:%s", debugBuffer); + try + { + sendCount++; + // StmSerial.write(SendStr.buffer, SEND_DATA_SIZE); + RCLCPP_INFO(this->get_logger(), "---sendCount--%d:", sendCount); + } + catch (serial::IOException &e) + { + RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port!")); + } +} + +void ControlRobot::PublishImuSensor() +{ + sensor_msgs::msg::Imu ImuDataPub; + ImuDataPub.header.stamp = rclcpp::Node::now(); + ImuDataPub.header.frame_id = gyroFrameId; + ImuDataPub.orientation.x = g_mpuData.orientation.x; + ImuDataPub.orientation.y = g_mpuData.orientation.y; + ImuDataPub.orientation.z = g_mpuData.orientation.z; + ImuDataPub.orientation.w = g_mpuData.orientation.w; + ImuDataPub.orientation_covariance[COVARIANCE_O] = COVARIANCE_6; + ImuDataPub.orientation_covariance[COVARIANCE_F] = COVARIANCE_6; + ImuDataPub.orientation_covariance[COVARIANCE_E] = COVARIANCE_F6; + ImuDataPub.angular_velocity.x = g_mpuData.angular_velocity.x; + ImuDataPub.angular_velocity.y = g_mpuData.angular_velocity.y; + ImuDataPub.angular_velocity.z = g_mpuData.angular_velocity.z; + ImuDataPub.angular_velocity_covariance[COVARIANCE_O] = COVARIANCE_6; + ImuDataPub.angular_velocity_covariance[COVARIANCE_F] = COVARIANCE_6; + ImuDataPub.angular_velocity_covariance[COVARIANCE_E] = COVARIANCE_F6; + ImuDataPub.linear_acceleration.x = g_mpuData.linear_acceleration.x; + ImuDataPub.linear_acceleration.y = g_mpuData.linear_acceleration.y; + ImuDataPub.linear_acceleration.z = g_mpuData.linear_acceleration.z; + ImuDataPub.linear_acceleration_covariance[COVARIANCE_O] = COVARIANCE_6; + ImuDataPub.linear_acceleration_covariance[COVARIANCE_F] = COVARIANCE_6; + ImuDataPub.linear_acceleration_covariance[COVARIANCE_E] = COVARIANCE_F6; + ImuPublisher->publish(ImuDataPub); +} + +void ControlRobot::PublishOdom() +{ + tf2::Quaternion q; + q.setRPY(0, 0, RobotPos.z); + geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q); + nav_msgs::msg::Odometry odom; + odom.header.stamp = rclcpp::Node::now(); + odom.header.frame_id = odomFrameId; + odom.child_frame_id = robotFrameId; + odom.pose.pose.position.x = RobotPos.x; + odom.pose.pose.position.y = RobotPos.y; + odom.pose.pose.position.z = 0; + odom.pose.pose.orientation = odom_quat; + odom.pose.covariance[COVARIANCE_O] = COVARIANCE_F5; + odom.pose.covariance[COVARIANCE_S] = COVARIANCE_F5; + odom.pose.covariance[COVARIANCE_FT] = COVARIANCE_12; + odom.pose.covariance[COVARIANCE_TO] = COVARIANCE_12; + odom.pose.covariance[COVARIANCE_FE] = COVARIANCE_12; + odom.pose.covariance[COVARIANCE_TF] = COVARIANCE_F3; + odom.twist.twist.linear.x = RobotVel.x; + odom.twist.twist.linear.y = RobotVel.y; + odom.twist.twist.angular.z = RobotVel.z; + odom.twist.covariance[COVARIANCE_O] = COVARIANCE_F5; + odom.twist.covariance[COVARIANCE_S] = COVARIANCE_F5; + odom.twist.covariance[COVARIANCE_FT] = COVARIANCE_12; + odom.twist.covariance[COVARIANCE_TO] = COVARIANCE_12; + odom.twist.covariance[COVARIANCE_FE] = COVARIANCE_12; + odom.twist.covariance[COVARIANCE_TF] = COVARIANCE_F3; + geometry_msgs::msg::TransformStamped OdomTf; + OdomTf.header = odom.header; + OdomTf.child_frame_id = odom.child_frame_id; + OdomTf.header.stamp = rclcpp::Node::now(); + OdomTf.transform.translation.x = odom.pose.pose.position.x; + OdomTf.transform.translation.y = odom.pose.pose.position.y; + OdomTf.transform.translation.z = odom.pose.pose.position.z; + OdomTf.transform.rotation = odom.pose.pose.orientation; + TfBro->sendTransform(OdomTf); + OdomPublisher->publish(odom); +} +void ControlRobot::PublishVoltage() +{ + std_msgs::msg::Float32 voltageMsgs; // 定义电源电压发布话题的数据类型 + static int countVoltage = 0; + if (countVoltage++ > 10) + { + countVoltage = 0; + powerVoltage = powerVoltage < 20 ? 20 : powerVoltage > 24 ? 24 + : powerVoltage; + voltageMsgs.data = (powerVoltage - 20.0) * 25; // 电量百分比转换 + VoltagePublisher->publish(voltageMsgs); // 发布电源电压话题单位：V 伏特 + } +} + +bool ControlRobot::GetSensorData() +{ + unsigned short transition = 0; + uint8_t Usart_Receive[1] = {0}; + // RCLCPP_INFO(this->get_logger(), "before serial read"); + StmSerial.read(Usart_Receive, sizeof(Usart_Receive)); + // RCLCPP_INFO(this->get_logger(), "after serial read"); + ReceiveData.rx[count] = Usart_Receive[0]; + // 确保数组第一个数据为FRAME_HEADER + if (Usart_Receive[0] == FRAME_HEADER || count > 0) + count++; + else + count = 0; + if (count == RECEIVE_DATA_SIZE) // 验证数据包的长度 + { + count = 0; // 为串口数据重新填入数组做准备 + if (ReceiveData.rx[RECEIVE_DATA_SIZE - 1] != FRAME_TAIL) // 验证数据包的帧尾 + { + return false; + } + + unsigned char check_sum = 0; + for (unsigned char k = 0; k < (RECEIVE_DATA_SIZE - 2); k++) + { + check_sum = check_sum ^ ReceiveData.rx[k]; + } + // 数据异或位校验计算 + if (ReceiveData.rx[RECEIVE_DATA_SIZE - 2] != check_sum) + { + return false; + } + // recvCount++; + // RCLCPP_INFO(this->get_logger(), "---recvCount--%d:", recvCount); + + char debugBuffer[RECEIVE_DATA_SIZE * 5] = {0}; + char *debugBufferPtr = debugBuffer; + for (size_t i = 0; i < RECEIVE_DATA_SIZE; ++i) + { + debugBufferPtr += sprintf(debugBufferPtr, "0x%02x ", ReceiveData.rx[i]); + } + debugBuffer[RECEIVE_DATA_SIZE * 5 - 1] = '\0'; + RCLCPP_INFO(this->get_logger(), "ReceiveData:%s", debugBuffer); + + RobotVel.x = OdomTrans(ReceiveData.rx[2], ReceiveData.rx[3]); + RobotVel.y = OdomTrans(ReceiveData.rx[4], ReceiveData.rx[5]); + RobotVel.z = OdomTrans(ReceiveData.rx[6], ReceiveData.rx[7]); + + MpuData.xAcceleData = DataTrans(ReceiveData.rx[8], ReceiveData.rx[9]); + MpuData.yAcceleData = DataTrans(ReceiveData.rx[10], ReceiveData.rx[11]); + MpuData.zAcceleData = DataTrans(ReceiveData.rx[12], ReceiveData.rx[13]); + + MpuData.xGyrosData = DataTrans(ReceiveData.rx[14], ReceiveData.rx[15]); + MpuData.yGyrosData = DataTrans(ReceiveData.rx[16], ReceiveData.rx[17]); + MpuData.zGyrosData = DataTrans(ReceiveData.rx[18], ReceiveData.rx[19]); + + g_mpuData.angular_velocity.x = MpuData.xGyrosData * GYROSCOPE_RATIO; + g_mpuData.angular_velocity.y = MpuData.yGyrosData * GYROSCOPE_RATIO; + g_mpuData.angular_velocity.z = MpuData.zGyrosData * GYROSCOPE_RATIO; + g_mpuData.linear_acceleration.x = MpuData.xAcceleData / ACCEl_RATIO; + g_mpuData.linear_acceleration.y = MpuData.yAcceleData / ACCEl_RATIO; + g_mpuData.linear_acceleration.z = MpuData.zAcceleData / ACCEl_RATIO; + // 获取电池电压 + transition |= ReceiveData.rx[20] << DEL | ReceiveData.rx[21]; + // 单位转换毫伏(mv)->伏(v) + powerVoltage = transition / MAGNIFICATION + (transition % MAGNIFICATION) * 0.001; + + return true; + } + return false; +} + +void ControlRobot::CycleControl() +{ + count = 0; + while (rclcpp::ok()) + { + if (true == GetSensorData()) + { + RobotPos.x += (RobotVel.x * cos(RobotPos.z) - RobotVel.y * sin(RobotPos.z)) * SAMPLING_TIME; + RobotPos.y += (RobotVel.x * sin(RobotPos.z) + RobotVel.y * cos(RobotPos.z)) * SAMPLING_TIME; + RobotPos.z += RobotVel.z * SAMPLING_TIME; + Quaternion_Solution(g_mpuData.angular_velocity.x, g_mpuData.angular_velocity.y, g_mpuData.angular_velocity.z, + g_mpuData.linear_acceleration.x, g_mpuData.linear_acceleration.y, g_mpuData.linear_acceleration.z); + // PublishImuSensor(); + // PublishOdom(); + // PublishVoltage(); // 发布电量话题 + rclcpp::spin_some(this->get_node_base_interface()); + } + } +} + +ControlRobot::ControlRobot() + : rclcpp::Node("hieuler_robot") +{ + memset(&RobotPos, 0, sizeof(RobotPos)); + memset(&RobotVel, 0, sizeof(RobotVel)); + memset(&ReceiveData, 0, sizeof(ReceiveData)); + memset(&SendStr, 0, sizeof(SendStr)); + memset(&MpuData, 0, sizeof(MpuData)); + + int serialBaudRate = BAUD_RATE; + this->declare_parameter("serialBaudRate", BAUD_RATE); + this->declare_parameter("usartPortName", "/dev/ttyAMA3"); + this->declare_parameter("cmd_vel", "cmd_vel"); + this->declare_parameter("odomFrameId", "odom"); + this->declare_parameter("robotFrameId", "base_footprint"); + this->declare_parameter("gyroFrameId", "imu_link"); + this->get_parameter("serialBaudRate", serialBaudRate); + this->get_parameter("usartPortName", usartPortName); + this->get_parameter("cmd_vel", cmd_vel); + this->get_parameter("odomFrameId", odomFrameId); + this->get_parameter("robotFrameId", robotFrameId); + this->get_parameter("gyroFrameId", gyroFrameId); + serial_timer_ = this->create_wall_timer( + 500ms, std::bind(&ControlRobot::SerialSendCallback, this) + ); + VoltagePublisher = create_publisher("powerVoltage", 1); + OdomPublisher = create_publisher("odom", MESSAGE_LENGTH); + ImuPublisher = create_publisher("imu", MESSAGE_LENGTH); + TfBro = std::make_shared(this); + CmdVelSub = create_subscription( + cmd_vel, 2, std::bind(&ControlRobot::CmdVelCallback, this, _1)); + + RCLCPP_INFO(this->get_logger(), "eulercar usartPortName:%s", usartPortName.c_str()); + RCLCPP_INFO(this->get_logger(), "eulercar serialBaudRate:%d", serialBaudRate); + try + { + StmSerial.setPort(usartPortName); + StmSerial.setBaudrate(serialBaudRate); + serial::Timeout _time = serial::Timeout::simpleTimeout(TIMEOUT); + StmSerial.setTimeout(_time); + RCLCPP_INFO(this->get_logger(), "open robot serial port"); + StmSerial.open(); + } + catch (serial::IOException &e) + { + RCLCPP_ERROR(this->get_logger(), "robot can not open serial port, Please check the serial port cable! "); + } + if (StmSerial.isOpen()) + { + RCLCPP_INFO(this->get_logger(), "robot serial port opened"); + } +} + +ControlRobot::~ControlRobot() +{ + SendStr.buffer[0] = FRAME_HEADER; + SendStr.buffer[1] = 0; + SendStr.buffer[2] = 0; + SendStr.buffer[4] = 0; + SendStr.buffer[3] = 0; + SendStr.buffer[6] = 0; + SendStr.buffer[5] = 0; + SendStr.buffer[8] = 0; + SendStr.buffer[7] = 0; + SendStr.buffer[9] = CheckSum(CHECK_NUM, SEND_DATA_CHECK); + SendStr.buffer[10] = FRAME_TAIL; + try + { + StmSerial.write(SendStr.buffer, SEND_DATA_SIZE); + } + catch (serial::IOException &e) + { + RCLCPP_ERROR(this->get_logger(), "Unable to send data through serial port"); + } + StmSerial.close(); +} + +void sigintHandler(int sig) +{ + sig = sig; + + serial::Serial carserial; + carserial.setPort("/dev/ttyAMA3"); + carserial.setBaudrate(BAUD_RATE); // 设置波特率 + serial::Timeout timeOut = serial::Timeout::simpleTimeout(2000); // 超时等待 + carserial.setTimeout(timeOut); + try + { + carserial.open(); + } + catch (serial::IOException &e) + { + printf("robot can not open serial port, Please check the serial port cable! "); + std::cerr << "I/O Exception: " << e.what() << std::endl; + } + + // 如果串口打开，则驱动读取数据的线程 + if (carserial.isOpen()) + { + // 程序退出时自动停车 + unsigned char buffer[SEND_DATA_SIZE] = {0}; + buffer[0] = FRAME_HEADER; + buffer[1] = 0; + buffer[2] = 0; + buffer[4] = 0; + buffer[3] = 0; + buffer[6] = 0; + buffer[5] = 0; + buffer[8] = 0; + buffer[7] = 0; + for (int k = 0; k < (SEND_DATA_SIZE-2); k++) + { + buffer[9] = buffer[9] ^ buffer[k]; + } + buffer[10] = FRAME_TAIL; + try + { + carserial.write(buffer, SEND_DATA_SIZE); + } + catch (serial::IOException &e) + { + printf("Unable to send data through serial port"); + std::cerr << "I/O Exception: " << e.what() << std::endl; + } + carserial.close(); + } + printf("Shutting down"); + // 关闭ROS2接口，清除资源 + rclcpp::shutdown(); +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + // 创建信号处理函数 + // signal(SIGINT, sigintHandler); + + auto node = std::make_shared(); + + ControlRobot RobotControl; + // RobotControl.CycleControl(); + + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + + return 0; +} \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/urdf/hieuler.urdf b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/urdf/hieuler.urdf new file mode 100644 index 0000000000000000000000000000000000000000..f214ac06a8570a3405a958433e1f9c39891e512e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/control/ros2_hieuler_robot/urdf/hieuler.urdf @@ -0,0 +1,156 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/README.md b/hi-pilot/ws_eulercar/src/official/urdf/README.md new file mode 100644 index 0000000000000000000000000000000000000000..fa2c70496490e9c0cd396ad2d9d52a8bc775ed36 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/README.md @@ -0,0 +1,10 @@ +# URDF + +This contains packages for working with URDF files. +See the ROS wiki for API documentation and tutorials. + +* [`urdf`](http://wiki.ros.org/urdf) +* [`urdf_parser_plugin`](http://wiki.ros.org/urdf_parser_plugin) + +This was originally part of the [`ros/robot_model`](https://github.com/ros/robot_model) repository. +It has been moved to this repo as described by [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195) diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/CHANGELOG.rst b/hi-pilot/ws_eulercar/src/official/urdf/urdf/CHANGELOG.rst new file mode 100644 index 0000000000000000000000000000000000000000..f03b9cf155c8fb24effdfe85476d670034a347de --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/CHANGELOG.rst @@ -0,0 +1,182 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package urdf +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.6.0 (2022-03-01) +------------------ +* Install headers to include/${PROJECT_NAME} (`#31 `_) +* Contributors: Shane Loretz + +2.5.3 (2022-01-13) +------------------ +* Add linter tests and fix errors (`#30 `_) +* Add in a Doxyfile to predefine macros. (`#28 `_) +* Contributors: Chris Lalancette, Jacob Perron + +2.5.2 (2021-05-12) +------------------ + +2.5.1 (2020-12-10) +------------------ +* Work around Windows min/max bug. (`#21 `_) +* Contributors: Chris Lalancette + +2.5.0 (2020-10-05) +------------------ +* Enable -Wall -Wextra -Wpedantic (`#20 `_) +* Add dependency on TinyXML2 (`#19 `_) +* Remove TinyXML dependency from urdf. (`#17 `_) +* Make urdf plugable and revive urdf_parser_plugin (`#13 `_) +* Contributors: Audrow Nash, Chris Lalancette, Shane Loretz + +2.4.0 (2020-05-26) +------------------ +* Deprecate methods that require tinyxml (`#12 `_) +* Contributors: Dan Rose + +2.3.0 (2020-04-28) +------------------ +* Export targets in a addition to include directories / libraries. (`#10 `_) +* Force explicit language for uncrustify. (`#9 `_) +* Code style only: wrap after open parenthesis if not in one line. (`#8 `_) +* Contributors: Dirk Thomas + +2.2.0 (2018-11-20) +------------------ +* Update to remove unneeded console_bridge dependency. (`#4 `_) +* Contributors: Mikael Arguedas + +2.1.0 (2018-06-25) +------------------ +* Update project URLs (`#2 `_) +* Contributors: Mikael Arguedas + +2.0.0 (2017-12-08) +------------------ +* Ported to ROS 2 +* Added Windows compatibility +* Switched to ROS 2 code style +* Contributors: Chris Lalancette, Mikael Arguedas + +1.12.12 (2017-11-08) +-------------------- +* Switched to package format 2 and made rostest a test_depend (`#221 `_) +* Made rostest a test_depend (`#221 `_) +* Added missing dependency on tinyxml (`#213 `_) +* Contributors: Chris Lalancette, Mikael Arguedas + + +1.12.11 (2017-06-27) +-------------------- +* Shared ptr yakkety (`#207 `_) + * Forward declare urdf::Model when urdfdom version is > 0.4 + * Add test for upcasting from urdf::ModelSharedPtr to urdf::ModelInterfaceSharedPtr +* Contributors: Shane Loretz + +1.12.10 (2017-06-24) +-------------------- +* Change urdf::Model to use std::shared_ptrs in urdfdom > v0.4 (`#206 `_) +* Contributors: Dave Coleman + +1.12.9 (2017-04-26) +------------------- + +1.12.8 (2017-03-27) +------------------- +* Allow supplying NodeHandle for initParam (`#168 `_) + * Allow supplying NodeHandle for initParam using new function. + * fixed missing return statement in previous commit. +* add Chris and Shane as maintainers (`#184 `_) +* fix missed mandatory -std=c++11 flag (`#181 `_) + collada_parser,kdl_parser,urdf: add c++11 flag, + collada_parser: replace typeof with ansi __typeof\_\_ + builded/tested on gentoo + Thanks den4ix for the contribution! +* Contributors: Denis Romanchuk, Piyush Khandelwal, William Woodall + +1.12.7 (2017-01-26) +------------------- + +1.12.6 (2017-01-04) +------------------- +* Addressed gcc6 build error in the urdf package, forward port of `#156 `_ (`#173 `_) +* Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) +* Contributors: Jochen Sprickerhof, William Woodall + +1.12.5 (2016-10-27) +------------------- +* Added urdf_compatibility.h header to define SharedPtr types (`#160 `_) + This provides portability for downstream packages allowing them to use urdfdom 0.3 or 0.4. +* urdf: Explicitly cast shared_ptr to bool in unit test. (`#158 `_) +* Add smart ptr typedefs (`#153 `_) +* Addressed gcc6 build error in urdf which was related to use of the isystem flag (`#157 `_) +* Remove unneeded dependency on libpcrecpp (`#155 `_) +* Contributors: Bence Magyar, Jochen Sprickerhof, Lukas Bulwahn, Maarten de Vries, Robert Haschke + +1.12.4 (2016-08-23) +------------------- + +1.12.3 (2016-06-10) +------------------- + +1.12.2 (2016-04-12) +------------------- + +1.12.1 (2016-04-10) +------------------- + +1.11.8 (2015-09-11) +------------------- +* Removed pcre hack for newer released collada-dom. +* Fixed link order of libpcrecpp. +* Contributors: Kei Okada + +1.11.7 (2015-04-22) +------------------- +* Removed the exporting of Boost and pcre as they are not used in the headers, and added TinyXML because it is. +* Fixed a bug with pcrecpp on Ubuntu > 13.04. +* Contributors: Kei Okada, William Woodall + +1.11.6 (2014-11-30) +------------------- +* Add install for static libs needed for Android cross-compilation +* Contributors: Gary Servin + +1.11.5 (2014-07-24) +------------------- + +1.11.4 (2014-07-07) +------------------- +* moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 +* Contributors: Tully Foote + +1.11.3 (2014-06-24) +------------------- +* fix urdfdom_headers find_package re `ros/rosdistro#4633 `_ +* Contributors: Tully Foote + +1.11.2 (2014-03-22) +------------------- + +1.11.1 (2014-03-20) +------------------- + +1.11.0 (2014-02-21) +------------------- +* fix urdf files for test +* fix test at urdf +* Contributors: YoheiKakiuchi + +1.10.18 (2013-12-04) +-------------------- +* add DEPENDS for kdl_parser +* Contributors: Ioan Sucan + +1.10.16 (2013-11-18) +-------------------- +* check for CATKIN_ENABLE_TESTING +* fix for using collada_parser_plugin + +1.10.15 (2013-08-17) +-------------------- +* fix `#30 `_ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/CMakeLists.txt b/hi-pilot/ws_eulercar/src/official/urdf/urdf/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..ac749aaedc791381239f6c2f0041bccf23b9ffe7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/CMakeLists.txt @@ -0,0 +1,129 @@ +cmake_minimum_required(VERSION 3.5) +project(urdf) + +find_package(ament_cmake_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(urdf_parser_plugin REQUIRED) +find_package(urdfdom REQUIRED) +find_package(urdfdom_headers REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) + +# Find version components +if(NOT urdfdom_headers_VERSION) + set(urdfdom_headers_VERSION "0.0.0") +endif() +string(REGEX REPLACE "^([0-9]+).*" "\\1" URDFDOM_HEADERS_MAJOR_VERSION "${urdfdom_headers_VERSION}") +string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_MINOR_VERSION "${urdfdom_headers_VERSION}") +string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_REVISION_VERSION "${urdfdom_headers_VERSION}") +set(generated_install_path "${CMAKE_CURRENT_BINARY_DIR}/include") +set(generated_compat_header "${generated_install_path}/${PROJECT_NAME}/urdfdom_compatibility.h") +include_directories(${generated_install_path}) +configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY) + +# 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() + +install(FILES ${generated_compat_header} DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME}) + +add_library(${PROJECT_NAME} src/model.cpp) +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$" +) +ament_target_dependencies(${PROJECT_NAME} + urdf_parser_plugin + urdfdom + urdfdom_headers + pluginlib +) +target_link_libraries(${PROJECT_NAME} + urdfdom::urdfdom_model +) + +if(WIN32) + target_compile_definitions(${PROJECT_NAME} PRIVATE "URDF_BUILDING_DLL") +endif() + +if(APPLE) + set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") +endif() + +add_library(urdf_xml_parser SHARED + src/urdf_plugin.cpp +) +target_link_libraries(urdf_xml_parser + ${PROJECT_NAME} +) +ament_target_dependencies(urdf_xml_parser + "pluginlib" + "TinyXML2" + "urdf_parser_plugin" +) + +install(TARGETS urdf_xml_parser + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_google_benchmark REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + # This forces cppcheck to consider all files in this project to be C++, + # including the headers which end with .h, which cppcheck would normally + # consider to be C instead. + ament_cppcheck(LANGUAGE "c++") + ament_cpplint() + ament_lint_cmake() + ament_uncrustify(LANGUAGE "C++") + + pluginlib_enable_plugin_testing( + CMAKE_TARGET_VAR mock_install_target + AMENT_PREFIX_PATH_VAR mock_install_path + PLUGIN_CATEGORY "urdf_parser" + PLUGIN_DESCRIPTIONS "urdf_parser_description.xml" + PLUGIN_LIBRARIES urdf_xml_parser + ) + + ament_add_google_benchmark(plugin_overhead + test/benchmark_plugin_overhead.cpp + APPEND_ENV AMENT_PREFIX_PATH="${mock_install_path}" + ) + target_link_libraries(plugin_overhead + urdf + ) + add_dependencies(plugin_overhead "${mock_install_target}") +endif() + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME}) + +ament_export_dependencies(pluginlib) +ament_export_dependencies(urdf_parser_plugin) +ament_export_dependencies(urdfdom) +ament_export_dependencies(urdfdom_headers) + +pluginlib_export_plugin_description_file(urdf_parser_plugin "urdf_parser_description.xml") + +ament_package() diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/Doxyfile b/hi-pilot/ws_eulercar/src/official/urdf/urdf/Doxyfile new file mode 100644 index 0000000000000000000000000000000000000000..ab65704279e2f3410508f101f72d1d6311d4b22c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/Doxyfile @@ -0,0 +1,21 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "urdf" +PROJECT_NUMBER = ros2 +PROJECT_BRIEF = "C++ parser for the Unified Robot Description Format (URDF)" + +INPUT = ./include +RECURSIVE = YES +OUTPUT_DIRECTORY = docs_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO +GENERATE_XML = YES +EXCLUDE_SYMBOLS = detail details + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED += URDF_EXPORT diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/model.h b/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/model.h new file mode 100644 index 0000000000000000000000000000000000000000..b8e4282973a2d956d3d3d0b23fd402f782ffddba --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/model.h @@ -0,0 +1,84 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#ifndef URDF__MODEL_H_ +#define URDF__MODEL_H_ + +#include +#include + +#include "urdf_model/model.h" + +#include "urdf/urdfdom_compatibility.h" +#include "urdf/visibility_control.hpp" + +namespace urdf +{ + +// PIMPL Forward Declaration +class ModelImplementation; + +/// \brief Populates itself based on a robot descripton +/// +/// This class uses `urdf_parser_plugin` to parse the given robot description. +/// The chosen plugin is the one that reports the most confident score. +/// There is no way to override this choice except by uninstalling undesirable +/// parser plugins. +class Model : public ModelInterface +{ +public: + URDF_EXPORT + Model(); + + URDF_EXPORT + ~Model(); + + /// \brief Load Model given a filename + URDF_EXPORT bool initFile(const std::string & filename); + + /// \brief Load Model from a XML-string + URDF_EXPORT bool initString(const std::string & xmlstring); + +private: + std::unique_ptr impl_; +}; + +// shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for +// std::shared_ptrs in latest version + +} // namespace urdf + +#endif // URDF__MODEL_H_ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/visibility_control.hpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/visibility_control.hpp new file mode 100644 index 0000000000000000000000000000000000000000..3b5925b76ac1fcfbc1cf4036da4a15deda5565f1 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/include/urdf/visibility_control.hpp @@ -0,0 +1,76 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, Open Source Robotics Foundation, Inc. +* 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 the copyright holder 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. +*********************************************************************/ + +/* This header must be included by all urdf headers which declare symbols + * which are defined in the urdf library. When not building the urdf + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the urdf + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef URDF__VISIBILITY_CONTROL_HPP_ +#define URDF__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define URDF_EXPORT __attribute__ ((dllexport)) + #define URDF_IMPORT __attribute__ ((dllimport)) + #else + #define URDF_EXPORT __declspec(dllexport) + #define URDF_IMPORT __declspec(dllimport) + #endif + #ifdef URDF_BUILDING_LIBRARY + #define URDF_PUBLIC URDF_EXPORT + #else + #define URDF_PUBLIC URDF_IMPORT + #endif + #define URDF_PUBLIC_TYPE URDF_PUBLIC + #define URDF_LOCAL +#else + #define URDF_EXPORT __attribute__ ((visibility("default"))) + #define URDF_IMPORT + #if __GNUC__ >= 4 + #define URDF_PUBLIC __attribute__ ((visibility("default"))) + #define URDF_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define URDF_PUBLIC + #define URDF_LOCAL + #endif + #define URDF_PUBLIC_TYPE +#endif + +#endif // URDF__VISIBILITY_CONTROL_HPP_ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/mainpage.dox b/hi-pilot/ws_eulercar/src/official/urdf/urdf/mainpage.dox new file mode 100644 index 0000000000000000000000000000000000000000..9287a4522e48584575f415c175c0f0f916b68da0 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/mainpage.dox @@ -0,0 +1,159 @@ +/** +\mainpage +\htmlinclude manifest.html + +urdf::Model is a class containing robot model data structure. +Every Robot Description File (URDF) can be described as a list of Links (urdf::Model::links_) and Joints (urdf::Model::joints_). +The connection between links(nodes) and joints(edges) should define a tree (i.e. 1 parent link, 0+ children links). +\li Here is an example Robot Description Describing a Parent Link 'P', a Child Link 'C', and a Joint 'J' + @verbatim + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @endverbatim + + + +\section codeapi Code API + +The URDF parser API contains the following methods: + \li Parse and build tree from XML: urdf::Model::initXml + \li Parse and build tree from File: urdf::Model::initFile + \li Parse and build tree from String: urdf::Model::initString + \li Get Root Link: urdf::Model::getRoot + \li Get Link by name urdf::Model::getLink + \li Get all Link's urdf::Model::getLinks + \li Get Joint by name urdf::Model::getJoint + + + + + + + + +*/ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/package.xml b/hi-pilot/ws_eulercar/src/official/urdf/urdf/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..ac3e83e2005a76edf91d3918366898aa5e0d6341 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/package.xml @@ -0,0 +1,49 @@ + + urdf + 2.6.0 + + This package contains a C++ parser for the Unified Robot Description + Format (URDF), which is an XML format for representing a robot model. + The code API of the parser has been through our review process and will remain + backwards compatible in future releases. + + + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz + + BSD + + https://github.com/ros2/urdf + https://github.com/ros2/urdf/issues + + ament_cmake_ros + + pluginlib + tinyxml2_vendor + urdfdom + urdf_parser_plugin + + urdfdom_headers + + pluginlib + tinyxml2_vendor + urdfdom + + urdfdom_headers + + pluginlib + urdf_parser_plugin + urdfdom + + urdfdom_headers + + ament_cmake_google_benchmark + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/model.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/model.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0817ea154b74bb3590d01fc8391f70f6d362e35e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/model.cpp @@ -0,0 +1,170 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include "urdf/model.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "urdf_parser_plugin/parser.h" +#include "pluginlib/class_loader.hpp" + +// Windows has preprocessor defines for "max", which conflicts with +// several things (one of them being std::numeric_limits::max()). Since +// we know we aren't going to use that macros, and this is a cpp (not header +// file), we just undefine it on Windows. +#ifdef _WIN32 +#undef max +#endif + +namespace urdf +{ +class ModelImplementation final +{ +public: + ModelImplementation() + : loader_("urdf_parser_plugin", "urdf::URDFParser") + { + } + + ~ModelImplementation() = default; + + pluginlib::UniquePtr load_plugin(const std::string & plugin_name); + + // Loader used to get plugins + pluginlib::ClassLoader loader_; +}; + + +Model::Model() +: impl_(new ModelImplementation) +{ +} + +Model::~Model() +{ + clear(); + impl_.reset(); +} + +bool Model::initFile(const std::string & filename) +{ + // get the entire file + std::string xml_string; + std::fstream xml_file(filename.c_str(), std::fstream::in); + if (xml_file.is_open()) { + while (xml_file.good() ) { + std::string line; + std::getline(xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + return Model::initString(xml_string); + } else { + fprintf(stderr, "Could not open file [%s] for parsing.\n", filename.c_str()); + return false; + } +} + +pluginlib::UniquePtr +ModelImplementation::load_plugin(const std::string & plugin_name) +{ + pluginlib::UniquePtr plugin_instance; + try { + plugin_instance = loader_.createUniqueInstance(plugin_name); + } catch (const pluginlib::CreateClassException &) { + fprintf(stderr, "Failed to load urdf_parser_plugin [%s]\n", plugin_name.c_str()); + } + return plugin_instance; +} + +bool Model::initString(const std::string & data) +{ + urdf::ModelInterfaceSharedPtr model; + + size_t best_score = std::numeric_limits::max(); + auto best_plugin = pluginlib::UniquePtr{nullptr}; + std::string best_plugin_name; + + // Figure out what plugins might handle this format + for (const std::string & plugin_name : impl_->loader_.getDeclaredClasses()) { + pluginlib::UniquePtr plugin_instance = impl_->load_plugin(plugin_name); + if (!plugin_instance) { + // Debug mode + assert(plugin_instance); + // Release mode + continue; + } + size_t score = plugin_instance->might_handle(data); + if (score < best_score) { + best_score = score; + best_plugin = std::move(plugin_instance); + best_plugin_name = plugin_name; + } + } + + if (best_score >= data.size()) { + // No plugin was confident ... try urdf anyways + best_plugin_name = "urdf_xml_parser/URDFXMLParser"; + best_plugin = impl_->load_plugin(best_plugin_name); + } + + if (!best_plugin) { + fprintf(stderr, "No plugin found for given robot description.\n"); + return false; + } + + model = best_plugin->parse(data); + + // copy data from model into this object + if (!model) { + fprintf(stderr, "Failed to parse robot description using: %s\n", best_plugin_name.c_str()); + return false; + } + + this->links_ = model->links_; + this->joints_ = model->joints_; + this->materials_ = model->materials_; + this->name_ = model->name_; + this->root_link_ = model->root_link_; + return true; +} +} // namespace urdf diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/rosconsole_bridge.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/rosconsole_bridge.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d09daf9328674eb11c65697dc4b2c917e7969149 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/rosconsole_bridge.cpp @@ -0,0 +1,37 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011, Willow Garage, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +#include + +REGISTER_ROSCONSOLE_BRIDGE; diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/urdf_plugin.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/urdf_plugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3c2f389bf0239312c88be240c0eb8089bd91cd85 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/src/urdf_plugin.cpp @@ -0,0 +1,81 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Open Source Robotics Foundation, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +#include +#include +#include + +#include + +namespace urdf +{ +class URDFXMLParser final : public urdf::URDFParser +{ +public: + URDFXMLParser() = default; + + ~URDFXMLParser() = default; + + urdf::ModelInterfaceSharedPtr parse(const std::string & xml_string) override; + + size_t might_handle(const std::string & data) override; +}; + +urdf::ModelInterfaceSharedPtr URDFXMLParser::parse(const std::string & xml_string) +{ + return urdf::parseURDF(xml_string); +} + +size_t URDFXMLParser::might_handle(const std::string & data) +{ + tinyxml2::XMLDocument doc; + const tinyxml2::XMLError error = doc.Parse(data.c_str()); + if (error == tinyxml2::XML_SUCCESS) { + // Since it's an XML document it must have `` as the first tag + const tinyxml2::XMLElement * root = doc.RootElement(); + if (std::string("robot") != root->Name()) { + return data.size(); + } + } + + // Possiblities: + // 1) It is not an XML based robot description + // 2) It is an XML based robot description, but there's an XML syntax error + // 3) It is a URDF XML with correct XML syntax + return data.find(" // NOLINT +PLUGINLIB_EXPORT_CLASS(urdf::URDFXMLParser, urdf::URDFParser) diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/benchmark_plugin_overhead.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/benchmark_plugin_overhead.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d7c34f85e1de40f37a639b24cb7a30e97899ae55 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/benchmark_plugin_overhead.cpp @@ -0,0 +1,87 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Willow Garage, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +#include +#include + +#include + +#include "urdf/model.h" + +const char test_xml[] = + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + +static void BM_no_plugin(benchmark::State & state) +{ + for (auto _ : state) { + if (nullptr == urdf::parseURDF(test_xml)) { + state.SkipWithError("Failed to read xml"); + break; + } + } +} + +static void BM_with_plugin(benchmark::State & state) +{ + for (auto _ : state) { + urdf::Model m; + if (!m.initString(test_xml)) { + state.SkipWithError("Failed to read xml"); + break; + } + } +} + +BENCHMARK(BM_no_plugin); +BENCHMARK(BM_with_plugin); + +BENCHMARK_MAIN(); diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_bracket.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_bracket.urdf new file mode 100644 index 0000000000000000000000000000000000000000..d0caaa81ae2c6114779a450dbe7011dcb3fea663 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_bracket.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double.urdf new file mode 100644 index 0000000000000000000000000000000000000000..10bbd4c7b92d2f3700b97e53724bf807395c410b --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double_joint.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double_joint.urdf new file mode 100644 index 0000000000000000000000000000000000000000..2974a7aa954fc3047203c7ba0c7708fcf88cfe3e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_double_joint.urdf @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_loop.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_loop.urdf new file mode 100644 index 0000000000000000000000000000000000000000..ab210245fed904a94907bed4424ab918135ccb62 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_loop.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf new file mode 100644 index 0000000000000000000000000000000000000000..88d75b39efe3b8a414bdd6fc1626cbde4c9a6465 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf @@ -0,0 +1,3377 @@ + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + + + true + 1000.0 + + + + + + true + 1.0 + 5 + -10.0 + 1.0 + 10.0 + 1200000.0 + diagnostic + battery_state + self_test + + + + + true + 1000.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fl_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + fl_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fl_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + fl_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fr_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + fr_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fr_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + fr_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bl_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + bl_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bl_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + bl_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + br_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + br_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + br_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + br_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + base_link_geom + 100.0 + + true + 100.0 + base_bumper + + + + + + + + + + base_link + + true + 100.0 + base_link + base_pose_ground_truth + 0.01 + map + 25.7 25.7 0 + 0 0 0 + + + + true + 100.0 + plug_holder + plug_holder_pose_ground_truth + 0.01 + map + 0 0 0 + 0 0 0 + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + + -135 + 135 + + 0.05 + 10.0 + 0.01 + 20.0 + + 0.005 + true + 20.0 + base_scan + base_laser_link + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + torso_lift_link_geom + 100.0 + + true + 100.0 + torso_lift_bumper + + + + + + + true + 100.0 + torso_lift_link + imu_data + 0.01 + map + 0 0 0 + 0 0 0 + + + + + + -52143.33 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R8G8B8 + 2448 2050 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + prosilica/cam_info + prosilica/image + prosilica/image_rect + prosilica/cam_info_service + prosilica/poll + hight_def_optical_frame + 1224.5 + 1224.5 + 1025.5 + 2955 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + wide_stereo/left_image + wide_stereo_l_stereo_camera_frame + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + wide_stereo/right_image + wide_stereo_r_stereo_camera_frame + + + + + + + true + 20.0 + wide_stereo_l_sensor + wide_stereo_r_sensor + wide_stereo/raw_stereo + wide_stereo_optical_frame + 320 + 320 + 240 + 320 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + narrow_stereo/left_image + narrow_stereo_l_stereo_camera_frame + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + narrow_stereo/right_image + narrow_stereo_r_stereo_camera_frame + + + + + + + true + 20.0 + narrow_stereo_l_sensor + narrow_stereo_r_sensor + narrow_stereo/raw_stereo + narrow_stereo_optical_frame + 320 + 320 + 240 + 772.55 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + + 0.0 0.0 0.0 + false + + -80 + 80 + + 0.05 + 10.0 + 0.01 + 40.0 + + 0.005 + true + 40.0 + tilt_scan + laser_tilt_link + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_shoulder_pan_link_geom + 100.0 + + true + 100.0 + r_shoulder_pan_bumper + + + + + true + + + + + + r_shoulder_lift_link_geom + 100.0 + + true + 100.0 + r_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + r_upper_arm_link_geom + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + true + + + + true + + r_forearm_link_geom + 100.0 + + true + 100.0 + r_forearm_bumper + + + + + + true + + r_wrist_flex_link_geom + 100.0 + + true + 100.0 + r_wrist_flex_bumper + + + + + + + + + true + + r_wrist_roll_link_geom + 100.0 + + true + 100.0 + r_wrist_roll_bumper + + + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + r_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + + + r_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + + true + + r_gripper_palm_link_geom + 100.0 + + true + 100.0 + r_gripper_palm_bumper + + + + + + true + + + + r_gripper_l_finger_tip_link + r_gripper_float_link + r_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + r_gripper_r_finger_tip_link + r_gripper_float_link + r_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + r_gripper_tool_frame + r_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_shoulder_pan_link_geom + 100.0 + + true + 100.0 + l_shoulder_pan_bumper + + + + + true + + + + + + l_shoulder_lift_link_geom + 100.0 + + true + 100.0 + l_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + l_upper_arm_link_geom + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + true + + + + true + + l_forearm_link_geom + 100.0 + + true + 100.0 + l_forearm_bumper + + + + + + true + + l_wrist_flex_link_geom + 100.0 + + true + 100.0 + l_wrist_flex_bumper + + + + + + + + + true + + l_wrist_roll_link_geom + 100.0 + + true + 100.0 + l_wrist_roll_bumper + + + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + l_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + + + l_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + + true + + l_gripper_palm_link_geom + 100.0 + + true + 100.0 + l_gripper_palm_bumper + + + + + + true + + + + l_gripper_l_finger_tip_link + l_gripper_float_link + l_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + l_gripper_r_finger_tip_link + l_gripper_float_link + l_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + l_gripper_tool_frame + l_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + l_forearm_cam/image + l_forearm_cam_frame + + + + true + PR2/Blue + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + r_forearm_cam/image + r_forearm_cam_frame + + + + true + PR2/Blue + + true + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_joint2.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_joint2.urdf new file mode 100644 index 0000000000000000000000000000000000000000..be1fdbc994455ff6ae59c352baebae28e2bd80dd --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_no_joint2.urdf @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_parent_itself.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_parent_itself.urdf new file mode 100644 index 0000000000000000000000000000000000000000..5eda24d4c27e37ac41755d05e20268acea6d3a3f --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_parent_itself.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_two_trees.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_two_trees.urdf new file mode 100644 index 0000000000000000000000000000000000000000..415e74c1a649d56060e06ac3f0ef358d1424e4cf --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_pr2_desc_two_trees.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_three_links_one_joint.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_three_links_one_joint.urdf new file mode 100644 index 0000000000000000000000000000000000000000..a6766a9d196cd5842a20a76a648b0ed3ab3d0e42 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/fail_three_links_one_joint.urdf @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/no_visual.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/no_visual.urdf new file mode 100644 index 0000000000000000000000000000000000000000..0554bac5f2903fa8a5dd49cc4f27bd702b56174a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/no_visual.urdf @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/one_link.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/one_link.urdf new file mode 100644 index 0000000000000000000000000000000000000000..4d99c96387ae9a88c11779ec0d084737dd112bda --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/one_link.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc.urdf new file mode 100644 index 0000000000000000000000000000000000000000..cbc5c776969feacde2ea2f7c4b1cd0ff9eeada94 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc.urdf @@ -0,0 +1,3238 @@ + + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + true + 1.0 + 5 + + power_state + 10.0 + 87.78 + -474 + 525 + 15.52 + 16.41 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -129.998394137 + 129.998394137 + 0.08 + 10.0 + 0.01 + 20 + + 0.005 + true + 20 + base_scan + base_laser_link + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + true + + base_link_geom + 100.0 + + true + 100.0 + base_bumper + + + + + + + true + 100.0 + base_link + base_pose_ground_truth + 0.01 + map + 25.7 25.7 0 + + 0 0 0 + + + base_footprint + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + torso_lift_link_geom + 100.0 + + true + 100.0 + torso_lift_bumper + + + + + + + + -52143.33 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 100.0 + imu_link + torso_lift_imu/data + 2.89e-08 + 0 0 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R8G8B8 + 2448 2050 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + /prosilica/image_raw + /prosilica/camera_info + /prosilica/request_image + high_def_frame + 1224.5 + 1224.5 + 1025.5 + 2955 + + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + BAYER_BGGR8 + 90 + 0.1 + 100 + 25.0 + + true + 25.0 + wide_stereo/left/image_raw + wide_stereo/left/camera_info + wide_stereo_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + BAYER_BGGR8 + 90 + 0.1 + 100 + 25.0 + + true + 25.0 + wide_stereo/right/image_raw + wide_stereo/right/camera_info + wide_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/left/image_raw + narrow_stereo/left/camera_info + narrow_stereo_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/right/image_raw + narrow_stereo/right/camera_info + narrow_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 15.0 + stereo_projection_pattern_high_res_red.png + projector_wg6802418_child_frame + stereo_projection_pattern_filter.png + projector_wg6802418_controller/image + projector_wg6802418_controller/projector + 0.785398163397 + 0.4 + 10 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -79.9999999086 + 79.9999999086 + 0.08 + 10.0 + 0.01 + 40 + + 0.005 + true + 40 + tilt_scan + laser_tilt_link + + + + + + + + + + -6.05 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + true + + + + + + + true + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_bumper + + + + + + + + + + + + + + + + + true + + r_gripper_r_finger_link_geom + 100.0 + + true + r_gripper_r_finger_link + 100.0 + r_gripper_r_finger_bumper + + + + + + + + + + + + + + + + true + false + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + r_gripper_l_finger_tip_link + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + true + false + + r_gripper_r_finger_tip_link_geom + 100.0 + + true + r_gripper_r_finger_tip_link + 100.0 + r_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_pose_ground_truth + 0.0 + base_link + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + true + 0.17126 + 7.7562e-05 + 1.49095e-06 + -9.83385e-06 + 0.000197083 + -3.06125e-06 + 0.000181054 + 0.03598 + 0.0173 + -0.00164 + 0.82991 -0.157 0.790675 + 0 -0 0 + true + false + + + true + 0.17389 + 7.73841e-05 + -2.09309e-06 + -8.36228e-06 + 0.000198474 + 2.4611e-06 + 0.00018107 + 0.03576 + -0.01736 + -0.00095 + 0.82991 -0.219 0.790675 + 0 -0 0 + true + false + + + + + r_gripper_r_parallel_link + r_gripper_palm_link + r_gripper_palm_link + 0 0 -1 + 0.2 + 0.05891 -0.031 0 + + + r_gripper_l_parallel_link + r_gripper_palm_link + r_gripper_palm_link + 0 0 1 + 0.2 + 0.05891 0.031 0 + + + r_gripper_r_parallel_link + r_gripper_r_finger_tip_link + r_gripper_r_finger_tip_link + 0 0 1 + -0.018 -0.021 0 + + + r_gripper_l_parallel_link + r_gripper_l_finger_tip_link + r_gripper_l_finger_tip_link + 0 0 1 + -0.018 0.021 0 + + + r_gripper_l_finger_tip_link + r_gripper_r_finger_tip_link + r_gripper_r_finger_tip_link + 0 1 0 + + + + true + + + + true + + + + + + + + + + + + + true + + r_gripper_palm_link_geom + 100.0 + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_bumper + + + + + + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + true + + + + + + + true + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + true + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_bumper + + + + + + + + + + + + + + + + + true + + l_gripper_r_finger_link_geom + 100.0 + + true + l_gripper_r_finger_link + 100.0 + l_gripper_r_finger_bumper + + + + + + + + + + + + + + + + true + false + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + l_gripper_l_finger_tip_link + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + true + false + + l_gripper_r_finger_tip_link_geom + 100.0 + + true + l_gripper_r_finger_tip_link + 100.0 + l_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_pose_ground_truth + 0.0 + base_link + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + true + 0.17126 + 7.7562e-05 + 1.49095e-06 + -9.83385e-06 + 0.000197083 + -3.06125e-06 + 0.000181054 + 0.03598 + 0.0173 + -0.00164 + 0.82991 0.219 0.790675 + 0 -0 0 + true + false + + + true + 0.17389 + 7.73841e-05 + -2.09309e-06 + -8.36228e-06 + 0.000198474 + 2.4611e-06 + 0.00018107 + 0.03576 + -0.01736 + -0.00095 + 0.82991 0.157 0.790675 + 0 -0 0 + true + false + + + + + l_gripper_r_parallel_link + l_gripper_palm_link + l_gripper_palm_link + 0 0 -1 + 0.2 + 0.05891 -0.031 0 + + + l_gripper_l_parallel_link + l_gripper_palm_link + l_gripper_palm_link + 0 0 1 + 0.2 + 0.05891 0.031 0 + + + l_gripper_r_parallel_link + l_gripper_r_finger_tip_link + l_gripper_r_finger_tip_link + 0 0 1 + -0.018 -0.021 0 + + + l_gripper_l_parallel_link + l_gripper_l_finger_tip_link + l_gripper_l_finger_tip_link + 0 0 1 + -0.018 0.021 0 + + + l_gripper_l_finger_tip_link + l_gripper_r_finger_tip_link + l_gripper_r_finger_tip_link + 0 1 0 + + + + true + + + + true + + + + + + + + + + + + + true + + l_gripper_palm_link_geom + 100.0 + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_bumper + + + + + + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 25.0 + + true + 25.0 + l_forearm_cam/image_raw + l_forearm_cam/camera_info + l_forearm_cam_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 25.0 + + true + 25.0 + r_forearm_cam/image_raw + r_forearm_cam/camera_info + r_forearm_cam_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc_no_joint.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc_no_joint.urdf new file mode 100644 index 0000000000000000000000000000000000000000..11fc4fd991289d647a7e40b0b0c4e13a5c866b5d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/pr2_desc_no_joint.urdf @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/singularity.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/singularity.urdf new file mode 100644 index 0000000000000000000000000000000000000000..ed91f44d74b43648e448fbfd41d703b1fed54283 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/singularity.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot.urdf new file mode 100644 index 0000000000000000000000000000000000000000..731e1f30f285861cc73be9080fe977d91c96f7a9 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot.urdf @@ -0,0 +1,425 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.cpp new file mode 100644 index 0000000000000000000000000000000000000000..19330ec296ee0c402fa8047ce5c3cc1f5c53a4b7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.cpp @@ -0,0 +1,156 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 the Willow Garage 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. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include +#include +#include + +#include "gtest/gtest.h" +#include "urdf/model.h" + +int g_argc; +char ** g_argv; + +class TestParser : public testing::Test +{ +public: + bool checkModel(urdf::Model & robot) + { + // get root link + urdf::LinkConstSharedPtr root_link = robot.getRoot(); + if (!root_link) { + fprintf(stderr, "no root link %s\n", robot.getName().c_str()); + return false; + } + + // go through entire tree + return this->traverse_tree(root_link); + } + +protected: + /// constructor + // num_links starts at 1 because traverse_tree doesn't count the root node + TestParser() + : num_joints(0), num_links(1) + { + } + + /// Destructor + ~TestParser() + { + } + + bool traverse_tree(urdf::LinkConstSharedPtr link, int level = 0) + { + fprintf( + stderr, "Traversing tree at level %d, link size %lu\n", + level, link->child_links.size()); + level += 2; + bool retval = true; + for (std::vector::const_iterator child = link->child_links.begin(); + child != link->child_links.end(); child++) + { + ++num_links; + if (*child && (*child)->parent_joint) { + ++num_joints; + // check rpy + double roll, pitch, yaw; + (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw); + + if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) { + fprintf(stderr, "getRPY() returned nan!\n"); + return false; + } + // recurse down the tree + retval &= this->traverse_tree(*child, level); + } else { + fprintf(stderr, "root link: %s has a null child!\n", link->name.c_str()); + return false; + } + } + // no more children + return retval; + } + + size_t num_joints; + size_t num_links; +}; + +TEST_F(TestParser, test) { + ASSERT_GE(g_argc, 3); + std::string folder = std::string(g_argv[1]) + "/test/"; + fprintf(stderr, "Folder %s\n", folder.c_str()); + std::string file = std::string(g_argv[2]); + bool expect_success = (file.substr(0, 5) != "fail_"); + urdf::Model robot; + fprintf(stderr, "Parsing file %s, expecting %d\n", (folder + file).c_str(), expect_success); + if (!expect_success) { + ASSERT_FALSE(robot.initFile(folder + file)); + return; + } + + ASSERT_EQ(g_argc, 7); + std::string robot_name = std::string(g_argv[3]); + std::string root_name = std::string(g_argv[4]); + size_t expected_num_joints = atoi(g_argv[5]); + size_t expected_num_links = atoi(g_argv[6]); + + ASSERT_TRUE(robot.initFile(folder + file)); + + EXPECT_EQ(robot.getName(), robot_name); + urdf::LinkConstSharedPtr root = robot.getRoot(); + ASSERT_TRUE(static_cast(root)); + EXPECT_EQ(root->name, root_name); + + ASSERT_TRUE(checkModel(robot)); + EXPECT_EQ(num_joints, expected_num_joints); + EXPECT_EQ(num_links, expected_num_links); + EXPECT_EQ(robot.joints_.size(), expected_num_joints); + EXPECT_EQ(robot.links_.size(), expected_num_links); + + // test reading from parameter server + ASSERT_TRUE(robot.initParam("robot_description")); + ASSERT_FALSE(robot.initParam("robot_description_wim")); + SUCCEED(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + g_argc = argc; + g_argv = argv; + return RUN_ALL_TESTS(); +} diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.launch b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.launch new file mode 100644 index 0000000000000000000000000000000000000000..9953b55a3a14596b739d4bcce99dc289c291ab75 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/test_robot_model_parser.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/two_links_one_joint.urdf b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/two_links_one_joint.urdf new file mode 100644 index 0000000000000000000000000000000000000000..a4cf276fbd3220e112f12410a8fb092dabcbcfea --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/two_links_one_joint.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/urdfdom_compatibility.cpp b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/urdfdom_compatibility.cpp new file mode 100644 index 0000000000000000000000000000000000000000..43ba2a897db50a5923b77d64a79a9fcd8e3a5e21 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/test/urdfdom_compatibility.cpp @@ -0,0 +1,54 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017 Open Source Robotics Foundation +* 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 the Willow Garage 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. +*********************************************************************/ + +#include + +#include + +#include "urdf/model.h" + + +TEST(UrdfCompatibility, UpcastSharedPtr) { + urdf::ModelSharedPtr model(new urdf::Model); + model->name_ = "UpcastSharedPtr"; + urdf::ModelInterfaceSharedPtr interface = model; + EXPECT_EQ(std::string("UpcastSharedPtr"), interface->getName()); +} + + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdf_parser_description.xml b/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdf_parser_description.xml new file mode 100644 index 0000000000000000000000000000000000000000..c4b8b377be346b2c2feb0a96adbfcb883182862e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdf_parser_description.xml @@ -0,0 +1,7 @@ + + + + Parse models as URDF from URDF XML. + + + diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdfdom_compatibility.h.in b/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdfdom_compatibility.h.in new file mode 100644 index 0000000000000000000000000000000000000000..25afde462b3ce348d4ce5f084a8ccfaa129f3549 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf/urdfdom_compatibility.h.in @@ -0,0 +1,111 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2016, CITEC, Bielefeld University +* 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 the Willow Garage 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. +*********************************************************************/ + +/* Robert Haschke */ + +#ifndef URDF__URDFDOM_COMPATIBILITY_H_ +#define URDF__URDFDOM_COMPATIBILITY_H_ + +#define URDFDOM_HEADERS_MAJOR_VERSION @URDFDOM_HEADERS_MAJOR_VERSION@ +#define URDFDOM_HEADERS_MINOR_VERSION @URDFDOM_HEADERS_MINOR_VERSION@ +#define URDFDOM_HEADERS_REVISION_VERSION @URDFDOM_HEADERS_REVISION_VERSION@ + +// Define shared pointers for urdfdom versions less than 0.4 +// Plus define ModelSharedPtr for 0.4 +#if URDFDOM_HEADERS_MAJOR_VERSION == 0 && URDFDOM_HEADERS_MINOR_VERSION <= 4 + +#include "boost/shared_ptr.hpp" +#include "boost/weak_ptr.hpp" + +#define URDF_TYPEDEF_CLASS_POINTER(Class) \ + class Class; \ + typedef boost::shared_ptr Class ## SharedPtr; \ + typedef boost::shared_ptr Class ## ConstSharedPtr; \ + typedef boost::weak_ptr Class ## WeakPtr + +namespace urdf +{ +// These shared_ptrs were added to urdfdom in 0.4.0, +// so if urdfdom == 0.4 this is duplicate work +URDF_TYPEDEF_CLASS_POINTER(Box); +URDF_TYPEDEF_CLASS_POINTER(Collision); +URDF_TYPEDEF_CLASS_POINTER(Cylinder); +URDF_TYPEDEF_CLASS_POINTER(Geometry); +URDF_TYPEDEF_CLASS_POINTER(Inertial); + +URDF_TYPEDEF_CLASS_POINTER(Joint); +URDF_TYPEDEF_CLASS_POINTER(JointCalibration); +URDF_TYPEDEF_CLASS_POINTER(JointDynamics); +URDF_TYPEDEF_CLASS_POINTER(JointLimits); +URDF_TYPEDEF_CLASS_POINTER(JointMimic); +URDF_TYPEDEF_CLASS_POINTER(JointSafety); + +URDF_TYPEDEF_CLASS_POINTER(Link); +URDF_TYPEDEF_CLASS_POINTER(Material); +URDF_TYPEDEF_CLASS_POINTER(Mesh); +URDF_TYPEDEF_CLASS_POINTER(Sphere); +URDF_TYPEDEF_CLASS_POINTER(Visual); + +URDF_TYPEDEF_CLASS_POINTER(ModelInterface); + +// ModelSharedPtr is the only one that needs to be defined for urdfdom 0.4 +URDF_TYPEDEF_CLASS_POINTER(Model); +} // namespace urdf + +#undef URDF_TYPEDEF_CLASS_POINTER + +#else // urdfdom <= 0.4 + +#include + +#include "urdf_model/types.h" +#include "urdf_world/types.h" + +namespace urdf +{ +typedef std::shared_ptr ModelInterfaceSharedPtr; +typedef std::shared_ptr ModelInterfaceConstSharedPtr; +typedef std::weak_ptr ModelInterfaceWeakPtr; + +// Forward declaration +class Model; + +typedef std::shared_ptr ModelSharedPtr; +typedef std::shared_ptr ModelConstSharedPtr; +typedef std::weak_ptr ModelWeakPtr; +} // namespace urdf + +#endif // urdfdom > 0.4 + +#endif // URDF__URDFDOM_COMPATIBILITY_H_ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CHANGELOG.rst b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CHANGELOG.rst new file mode 100644 index 0000000000000000000000000000000000000000..d62eaca8e35a6b88778fc1f34a3e5d05b3d8c4d2 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CHANGELOG.rst @@ -0,0 +1,116 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package urdf_parser_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.6.0 (2022-03-01) +------------------ +* Install headers to include/${PROJECT_NAME} (`#31 `_) +* Contributors: Shane Loretz + +2.5.3 (2022-01-13) +------------------ +* Add linter tests and fix errors (`#30 `_) +* Contributors: Jacob Perron + +2.5.2 (2021-05-12) +------------------ +* Export urdfdom_headers as urdf_parser_plugin dependency. (`#25 `_) +* Contributors: Michel Hidalgo + +2.5.1 (2020-12-10) +------------------ + +2.5.0 (2020-10-05) +------------------ +* Make urdf plugable and revive urdf_parser_plugin (`#13 `_) +* Contributors: Shane Loretz + +2.1.0 (2018-06-25) +------------------ +* Update project URLs (`#2 `_) +* Contributors: Mikael Arguedas + +2.0.0 (2017-12-08) +------------------ + +1.12.12 (2017-11-08) +-------------------- + +1.12.11 (2017-06-27) +-------------------- + +1.12.10 (2017-06-24) +-------------------- + +1.12.9 (2017-04-26) +------------------- + +1.12.8 (2017-03-27) +------------------- +* add Chris and Shane as maintainers (`#184 `_) +* Contributors: William Woodall + +1.12.7 (2017-01-26) +------------------- + +1.12.6 (2017-01-04) +------------------- +* Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) +* Contributors: Jochen Sprickerhof + +1.12.5 (2016-10-27) +------------------- + +1.12.4 (2016-08-23) +------------------- + +1.12.3 (2016-06-10) +------------------- + +1.12.2 (2016-04-12) +------------------- + +1.12.1 (2016-04-10) +------------------- + +1.11.8 (2015-09-11) +------------------- + +1.11.7 (2015-04-22) +------------------- + +1.11.6 (2014-11-30) +------------------- + +1.11.5 (2014-07-24) +------------------- + +1.11.4 (2014-07-07) +------------------- +* moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 +* Contributors: Tully Foote + +1.11.3 (2014-06-24) +------------------- +* update usage of urdfdom_headers for indigo/trusty +* Contributors: William Woodall + +1.11.2 (2014-03-22) +------------------- + +1.11.1 (2014-03-20) +------------------- + +1.11.0 (2014-02-21) +------------------- + +1.10.18 (2013-12-04) +-------------------- +* add DEPENDS for kdl_parser +* Contributors: Ioan Sucan + +1.10.16 (2013-11-18) +-------------------- + +1.10.15 (2013-08-17) +-------------------- diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CMakeLists.txt b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..d5a0bac731ffc9df2ed789275d5806c9a2c7f6af --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(urdf_parser_plugin) + +find_package(ament_cmake_ros REQUIRED) +find_package(urdfdom_headers REQUIRED) + +# Create and export a header-only target +add_library(urdf_parser_plugin INTERFACE) +target_include_directories(urdf_parser_plugin INTERFACE + $ +) +ament_target_dependencies(urdf_parser_plugin INTERFACE + "urdfdom_headers" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_cppcheck_LANGUAGE "c++") + set(ament_cmake_uncrustify_ADDITIONAL_ARGS LANGUAGE "C++") + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS urdf_parser_plugin EXPORT urdf_parser_plugin-export) +ament_export_targets(urdf_parser_plugin-export) +ament_export_dependencies(urdfdom_headers) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +ament_package() diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/include/urdf_parser_plugin/parser.h b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/include/urdf_parser_plugin/parser.h new file mode 100644 index 0000000000000000000000000000000000000000..f7b8d2dbca6fc151c8a58a0da29f60cc1683825f --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/include/urdf_parser_plugin/parser.h @@ -0,0 +1,70 @@ +// Copyright (c) 2013, Willow Garage, Inc. +// +// 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 the Willow Garage 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 HOLDER 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. + +/* Author: Ioan Sucan */ + +#ifndef URDF_PARSER_PLUGIN__PARSER_H_ +#define URDF_PARSER_PLUGIN__PARSER_H_ + +#include + +#include "urdf_world/types.h" + +namespace urdf +{ + +/** \brief Base class for URDF parsers */ +class URDFParser +{ +public: + URDFParser() + { + } + virtual ~URDFParser() + { + } + + /// \brief Load Model from string + /// \return nullptr and write to stderr if the given string is invalid + virtual urdf::ModelInterfaceSharedPtr parse(const std::string & data) = 0; + + /// \brief Indicate if data is meant to be parsed by this parser + /// \return The position in the string that the plugin became confident the + /// data is intended to be parsed by it. + /// For example, the plugin parsing COLLADA files might return the + /// position in the string that the '' xml tag was found. + /// Smaller values are interpretted as more confidence, and the + /// plugin with the smallest value is used to parse the data. + /// If a plugin believes data is not meant for it, then it should + /// return a value greater than or equal to data.size(). + virtual size_t might_handle(const std::string & data) = 0; +}; + +} // namespace urdf + +#endif // URDF_PARSER_PLUGIN__PARSER_H_ diff --git a/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/package.xml b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..beff1033bf72aa9926fc10f279744e76853bcf09 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/official/urdf/urdf_parser_plugin/package.xml @@ -0,0 +1,31 @@ + + urdf_parser_plugin + 2.6.0 + + This package contains a C++ base class for URDF parsers. + + + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz + + BSD + + + https://github.com/ros2/urdf + https://github.com/ros2/urdf/issues + + ament_cmake_ros + + urdfdom_headers + urdfdom_headers + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/CMakeLists.txt b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..b2346ac8a727466aa31ae8dd9532cf4e49848a02 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(hieuler_cartographer) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # 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() + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/config/cartographer_2d.lua b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/config/cartographer_2d.lua new file mode 100644 index 0000000000000000000000000000000000000000..29ca76f8af8fd75934ccda44126d7f9fc4b3d507 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/config/cartographer_2d.lua @@ -0,0 +1,44 @@ +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", -- 地图帧的名称 + tracking_frame = "laser_link", -- 跟踪帧的名称 + published_frame = "base_link", -- 发布帧的名称 + odom_frame = "odom", -- 里程计帧的名称 + provide_odom_frame = true, -- 是否提供里程计帧 + publish_frame_projected_to_2d = false, -- 是否发布2d姿态 + use_pose_extrapolator = false, + use_odometry = false, -- 是否使用里程计 + use_nav_sat = false, -- 是否使用导航卫星 + use_landmarks = false, -- 是否使用地标 + num_laser_scans = 1, -- 激光雷达的数量 + num_multi_echo_laser_scans = 0, -- 多回波激光雷达的数量 + num_subdivisions_per_laser_scan = 1, -- 每个激光扫描的细分数量 + num_point_clouds = 0, -- 点云的数量 + lookup_transform_timeout_sec = 0.2, -- 查找变换的超时时间（秒） + submap_publish_period_sec = 0.3, -- 子地图发布周期（秒） + pose_publish_period_sec = 5e-3, -- 姿态发布周期（秒） + trajectory_publish_period_sec = 30e-3, -- 轨迹发布周期（秒） + rangefinder_sampling_ratio = 1., -- 测距仪采样比率 + odometry_sampling_ratio = 1., -- 里程计采样比率 + fixed_frame_pose_sampling_ratio = 1., -- 固定帧姿态采样比率 + imu_sampling_ratio = 1., -- IMU采样比率 + landmarks_sampling_ratio = 1., -- 地标采样比率 +} + +MAP_BUILDER.use_trajectory_builder_2d = true -- 是否启动2D SLAM +TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35 -- 2D轨迹构建器中子地图的范围数据数量 +TRAJECTORY_BUILDER_2D.min_range = 0.1 -- 限制在雷达最小扫描范围，比机器人半径小的都忽略 +TRAJECTORY_BUILDER_2D.max_range = 3.5 -- 限制在雷达最大扫描范围 +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. -- 限制在雷达最大扫描范围 +TRAJECTORY_BUILDER_2D.use_imu_data = false -- 是否使用IMU数据 +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 是否使用实时回环检测扫描匹配 + +TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) -- 1.0改成0.1,提高对运动的敏感度 +POSE_GRAPH.constraint_builder.min_score = 0.65 -- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。 +POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 --0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确 + +return options diff --git a/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/launch/cartographer.launch.py b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/launch/cartographer.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..a6f06151cdb30727cfab474c9c550426dcbd1033 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/launch/cartographer.launch.py @@ -0,0 +1,55 @@ +import os +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + # 定位功能包 + pkg_share = FindPackageShare(package='hieuler_cartographer').find('hieuler_cartographer') + + #配置节点启动信息 + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + # 地图的分辨率 + resolution = LaunchConfiguration('resolution', default='0.05') + # 地图的发布周期 + publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') + # 配置文件夹路径 + configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') ) + # 配置文件 + configuration_basename = LaunchConfiguration('configuration_basename', default='cartographer_2d.lua') + + #声明节点 + cartographer_node = Node( + package='cartographer_ros', + executable='cartographer_node', + name='cartographer_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=['-configuration_directory', configuration_directory, + '-configuration_basename', configuration_basename]) + + cartographer_occupancy_grid_node = Node( + package='cartographer_ros', + executable='cartographer_occupancy_grid_node', + name='cartographer_occupancy_grid_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]) + + # base_link to base_laser tf node + base_link_to_laser_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='base_link_to_base_laser_ld19', + # 雷达中心及方向相对于车辆中心及方向的偏移量 + arguments=['0.05','0','0','1.57','0','0','base_link','laser_link'] + ) + + #启动文件 + ld = LaunchDescription() + ld.add_action(cartographer_node) + ld.add_action(cartographer_occupancy_grid_node) + ld.add_action(base_link_to_laser_tf_node) + + return ld diff --git a/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/package.xml b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..0f2b8e96858505f8956bdbce1d363196cfd7b48d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/perception/hieuler_cartographer/package.xml @@ -0,0 +1,18 @@ + + + + hieuler_cartographer + 0.0.0 + TODO: Package description + antonio + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/CMakeLists.txt b/hi-pilot/ws_eulercar/src/sensing/bluesea2/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..cb01cf163aac6c2bf12d4a4d30f7eca8c60ec702 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(bluesea2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Get ROS2 version information (initial letter) from environment variables +#string(SUBSTRING $ENV{ROS_DISTRO} 0 1 tmp) +#MESSAGE("ROS_DISTRO2:${tmp}") + +#string(COMPARE LESS_EQUAL ${tmp} "e" flag) +#MESSAGE("flag:${flag}") + + +#if( ${flag} STREQUAL 1) +#add_definitions(-DROS_DISTRO_E) +#else() +add_definitions(-DROS_DISTRO_F) +#endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) + + +include_directories(${sensor_msgs_INCLUDE_DIRS}) + +add_executable(bluesea_node src/bluesea_node.cpp src/parser.cpp src/uart_reader.cpp src/udp_reader.cpp src/tcp_reader.cpp src/algorithmAPI.cpp src/uart.c) +ament_target_dependencies(bluesea_node + "rclcpp" + "sensor_msgs" + "visualization_msgs" + "geometry_msgs" + "std_srvs" +) +add_executable(bluesea_node_client src/bluesea_client.cpp) +ament_target_dependencies(bluesea_node_client rclcpp std_srvs) + +target_include_directories(bluesea_node PUBLIC + $ + $) + +target_compile_features(bluesea_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(bluesea_node_client PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS bluesea_node bluesea_node_client + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch params + DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/LICENSE b/hi-pilot/ws_eulercar/src/sensing/bluesea2/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..f63f5a9cf3498818a73068495709cceed67efd6a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/LICENSE @@ -0,0 +1,194 @@ +木兰宽松许可证，第2版 + +木兰宽松许可证，第2版 + +2020年1月 http://license.coscl.org.cn/MulanPSL2 + +您对“软件”的复制、使用、修改及分发受木兰宽松许可证，第2版（“本许可证”）的如下条款的约束： + +0. 定义 + +“软件” 是指由“贡献”构成的许可在“本许可证”下的程序和相关文档的集合。 + +“贡献” 是指由任一“贡献者”许可在“本许可证”下的受版权法保护的作品。 + +“贡献者” 是指将受版权法保护的作品许可在“本许可证”下的自然人或“法人实体”。 + +“法人实体” 是指提交贡献的机构及其“关联实体”。 + +“关联实体” 是指，对“本许可证”下的行为方而言，控制、受控制或与其共同受控制的机构，此处的控制是 +指有受控方或共同受控方至少50%直接或间接的投票权、资金或其他有价证券。 + +1. 授予版权许可 + +每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的版权许可，您可 +以复制、使用、修改、分发其“贡献”，不论修改与否。 + +2. 授予专利许可 + +每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的（根据本条规定 +撤销除外）专利许可，供您制造、委托制造、使用、许诺销售、销售、进口其“贡献”或以其他方式转移其“贡 +献”。前述专利许可仅限于“贡献者”现在或将来拥有或控制的其“贡献”本身或其“贡献”与许可“贡献”时的“软 +件”结合而将必然会侵犯的专利权利要求，不包括对“贡献”的修改或包含“贡献”的其他结合。如果您或您的“ +关联实体”直接或间接地，就“软件”或其中的“贡献”对任何人发起专利侵权诉讼（包括反诉或交叉诉讼）或 +其他专利维权行动，指控其侵犯专利权，则“本许可证”授予您对“软件”的专利许可自您提起诉讼或发起维权 +行动之日终止。 + +3. 无商标许可 + +“本许可证”不提供对“贡献者”的商品名称、商标、服务标志或产品名称的商标许可，但您为满足第4条规定 +的声明义务而必须使用除外。 + +4. 分发限制 + +您可以在任何媒介中将“软件”以源程序形式或可执行形式重新分发，不论修改与否，但您必须向接收者提供“ +本许可证”的副本，并保留“软件”中的版权、商标、专利及免责声明。 + +5. 免责声明与责任限制 + +“软件”及其中的“贡献”在提供时不带任何明示或默示的担保。在任何情况下，“贡献者”或版权所有者不对 +任何人因使用“软件”或其中的“贡献”而引发的任何直接或间接损失承担责任，不论因何种原因导致或者基于 +何种法律理论，即使其曾被建议有此种损失的可能性。 + +6. 语言 + +“本许可证”以中英文双语表述，中英文版本具有同等法律效力。如果中英文版本存在任何冲突不一致，以中文 +版为准。 + +条款结束 + +如何将木兰宽松许可证，第2版，应用到您的软件 + +如果您希望将木兰宽松许可证，第2版，应用到您的新软件，为了方便接收者查阅，建议您完成如下三步： + +1， 请您补充如下声明中的空白，包括软件名、软件的首次发表年份以及您作为版权人的名字； + +2， 请您在软件包的一级目录下创建以“LICENSE”为名的文件，将整个许可证文本放入该文件中； + +3， 请将如下声明文本放入每个源文件的头部注释中。 + +Copyright (c) [Year] [name of copyright holder] +[Software Name] is licensed under Mulan PSL v2. +You can use this software according to the terms and conditions of the Mulan +PSL v2. +You may obtain a copy of Mulan PSL v2 at: + http://license.coscl.org.cn/MulanPSL2 +THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO +NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE. +See the Mulan PSL v2 for more details. + +Mulan Permissive Software License，Version 2 + +Mulan Permissive Software License，Version 2 (Mulan PSL v2) + +January 2020 http://license.coscl.org.cn/MulanPSL2 + +Your reproduction, use, modification and distribution of the Software shall +be subject to Mulan PSL v2 (this License) with the following terms and +conditions: + +0. Definition + +Software means the program and related documents which are licensed under +this License and comprise all Contribution(s). + +Contribution means the copyrightable work licensed by a particular +Contributor under this License. + +Contributor means the Individual or Legal Entity who licenses its +copyrightable work under this License. + +Legal Entity means the entity making a Contribution and all its +Affiliates. + +Affiliates means entities that control, are controlled by, or are under +common control with the acting entity under this License, ‘control’ means +direct or indirect ownership of at least fifty percent (50%) of the voting +power, capital or other securities of controlled or commonly controlled +entity. + +1. Grant of Copyright License + +Subject to the terms and conditions of this License, each Contributor hereby +grants to you a perpetual, worldwide, royalty-free, non-exclusive, +irrevocable copyright license to reproduce, use, modify, or distribute its +Contribution, with modification or not. + +2. Grant of Patent License + +Subject to the terms and conditions of this License, each Contributor hereby +grants to you a perpetual, worldwide, royalty-free, non-exclusive, +irrevocable (except for revocation under this Section) patent license to +make, have made, use, offer for sale, sell, import or otherwise transfer its +Contribution, where such patent license is only limited to the patent claims +owned or controlled by such Contributor now or in future which will be +necessarily infringed by its Contribution alone, or by combination of the +Contribution with the Software to which the Contribution was contributed. +The patent license shall not apply to any modification of the Contribution, +and any other combination which includes the Contribution. If you or your +Affiliates directly or indirectly institute patent litigation (including a +cross claim or counterclaim in a litigation) or other patent enforcement +activities against any individual or entity by alleging that the Software or +any Contribution in it infringes patents, then any patent license granted to +you under this License for the Software shall terminate as of the date such +litigation or activity is filed or taken. + +3. No Trademark License + +No trademark license is granted to use the trade names, trademarks, service +marks, or product names of Contributor, except as required to fulfill notice +requirements in section 4. + +4. Distribution Restriction + +You may distribute the Software in any medium with or without modification, +whether in source or executable forms, provided that you provide recipients +with a copy of this License and retain copyright, patent, trademark and +disclaimer statements in the Software. + +5. Disclaimer of Warranty and Limitation of Liability + +THE SOFTWARE AND CONTRIBUTION IN IT ARE PROVIDED WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED. IN NO EVENT SHALL ANY CONTRIBUTOR OR +COPYRIGHT HOLDER BE LIABLE TO YOU FOR ANY DAMAGES, INCLUDING, BUT NOT +LIMITED TO ANY DIRECT, OR INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING +FROM YOUR USE OR INABILITY TO USE THE SOFTWARE OR THE CONTRIBUTION IN IT, NO +MATTER HOW IT’S CAUSED OR BASED ON WHICH LEGAL THEORY, EVEN IF ADVISED OF +THE POSSIBILITY OF SUCH DAMAGES. + +6. Language + +THIS LICENSE IS WRITTEN IN BOTH CHINESE AND ENGLISH, AND THE CHINESE VERSION +AND ENGLISH VERSION SHALL HAVE THE SAME LEGAL EFFECT. IN THE CASE OF +DIVERGENCE BETWEEN THE CHINESE AND ENGLISH VERSIONS, THE CHINESE VERSION +SHALL PREVAIL. + +END OF THE TERMS AND CONDITIONS + +How to Apply the Mulan Permissive Software License，Version 2 +(Mulan PSL v2) to Your Software + +To apply the Mulan PSL v2 to your work, for easy identification by +recipients, you are suggested to complete following three steps: + +i. Fill in the blanks in following statement, including insert your software +name, the year of the first publication of your software, and your name +identified as the copyright owner; + +ii. Create a file named "LICENSE" which contains the whole context of this +License in the first directory of your software package; + +iii. Attach the statement to the appropriate annotated syntax at the +beginning of each source file. + +Copyright (c) [Year] [name of copyright holder] +[Software Name] is licensed under Mulan PSL v2. +You can use this software according to the terms and conditions of the Mulan +PSL v2. +You may obtain a copy of Mulan PSL v2 at: + http://license.coscl.org.cn/MulanPSL2 +THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO +NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE. +See the Mulan PSL v2 for more details. diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/README.md b/hi-pilot/ws_eulercar/src/sensing/bluesea2/README.md new file mode 100644 index 0000000000000000000000000000000000000000..56d7f25524e58ff5c7ea1be7529a12ac2384b445 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/README.md @@ -0,0 +1,47 @@ +# bluesea2 +ROS2 driver for Lanhai USB/Network 2D LiDAR + +How to build Lanhai ros driver +===================================================================== + 1) Clone this project to your workspace src folder + 2) cd .. + 3) Running `colcon build` to build (The following commands are executed in this directory) + +How to run Lanhai ros node (Serial Port Version) +===================================================================== +1) Copy UDEV rule file : sudo cp src/LHLiDAR.rules /etc/udev/rules.d/ +2) or Run : sudo chmod 666 /dev/ttyUSB0 # make usb serial port readable + + +## if your lidar model is LDS-50C-2 : +* ros2 launch bluesea2 LDS-50C-2.py + +## if your lidar model is LDS-50C-C30E : +* ros2 launch bluesea2 LDS-50C-C30E.py + +## if your lidar model is LDS-50C-C20E : +* ros2 launch bluesea2 LDS-50C-C20E.py + +## if your lidar model is LSS-40D-C20E : +* ros2 launch bluesea2 LDS-40D-C20E.py + + +3) optional : ros2 topic hz /scan + +4) optional : rviz2 # + + +How to control Lanhai ros node start and stop +===================================================================== +* client: + + ros2 run bluesea2 bluesea_node_client start + + ros2 run bluesea2 bluesea_node_client stop + + +* server: + + ros2 launch bluesea2 xxxx.launch + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/alarm.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/alarm.h new file mode 100644 index 0000000000000000000000000000000000000000..9d7afc2dfe8beac25e0c61bb823d0a1f4475016d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/alarm.h @@ -0,0 +1,38 @@ +#ifndef _ALARM_ +#define _ALARM_ + +#define LMSG_ERROR 1 +#define LMSG_INFO 2 +#define LMSG_DEBUG 4 +#define LMSG_ALARM 0x100 + + +#define LERR_LOW_POWER 1 +#define LERR_MOTOR 2 +#define LERR_RANGER_HI 4 +#define LERR_NETWORK 8 +#define LERR_RANGER_IDLE 0x10 +#define LERR_ZERO_MISS 0x20 +#include +struct LidarMsgHdr +{ + char sign[4]; // must be "LMSG" + uint32_t proto_version; // 0x101 + char dev_sn[20]; + uint32_t dev_id; // + uint32_t timestamp;// + uint32_t type; + uint32_t data; + uint16_t id; + uint16_t extra; +}; + +struct LidarAlarm +{ + LidarMsgHdr hdr; + uint32_t zone_actived; + uint8_t all_states[32]; + uint32_t reserved[11]; +}; + +#endif diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/algorithmAPI.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/algorithmAPI.h new file mode 100644 index 0000000000000000000000000000000000000000..bf354842ce3092248b302be8971d2814200b33b5 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/algorithmAPI.h @@ -0,0 +1,12 @@ +#ifndef _ALGORITHMAPI_ +#define _ALGORITHMAPI_ +#include"parser.h" +#include +#include +bool checkWindowValid2(std::vectorscan, size_t idx, size_t window, double max_distance,double angle_increment); + +bool filter(std::vector& output_scan,int type,double max_range,double min_range,double max_range_difference,int filter_window,double angle_increment); + +//判定一圈的点数中距离为0的点位的比例是否符合要求 +bool checkZeroDistance(std::vectorcomsume,float error_scale); +#endif diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/bluesea_node.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/bluesea_node.h new file mode 100644 index 0000000000000000000000000000000000000000..9c61f3b729dc010b4faf965a5738b32e4e3088ed --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/bluesea_node.h @@ -0,0 +1,104 @@ +#include"parser.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + + +struct PubHub +{ + pthread_mutex_t mtx; + int nfan; + RawData *fans[MAX_FANS]; + int error_num; + int offsetangle; + int offsetidx; + std::vectorconsume;//统计完剩余的点数 + uint32_t ts_beg[2]; + uint32_t ts_end[2]; +}; + +struct ConnectArg +{ + std::string laser_topics; + std::string cloud_topics; + std::string arg1; + int arg2; +}; +struct Range +{ + double min; + double max; +}; +//定制隐藏功能 +struct Custom +{ + int error_circle; + double error_scale; + bool is_group_listener; + std::string group_ip; +}; +//过滤算法 +struct Fitter +{ + bool isopen; + int type; + double max_range; + double min_range; + double max_range_difference; + int filter_window; + +}; +struct ArgData +{ + std:: string frame_id; + int dev_id; + std::vectorconnectargs; + int raw_bytes; + bool from_zero; + bool output_scan; + bool output_cloud; + bool inverted; + bool reversed; + bool hard_resample; + bool soft_resample; + bool with_angle_filter; + double min_angle; + double max_angle; + bool output_360; + double min_dist; + double max_dist; + std::vectormasks; + std::string type; + int localport; + int num; + int uuid; + int rpm; + int model; + double resample; + int with_smooth; + int with_deshadow; + int alarm_msg; + int direction; + int unit_is_mm; + int with_confidence; + int ats; + Fitter fitter; + Custom custom; + +}; +//中断进程 +void closeSignal(int sig); +//指定编号雷达发送指令 +bool SendCmd(int len, char *cmd, int index); +//获取一帧第一个点所在的扇区，以及该点所在扇区的位置 +bool autoGetFirstAngle2(HPublish pub, bool from_zero); +//对读取的数据进行解析 参数:实际获取的数据 配置文件对应的参数 +int GetAllFans(HPublish pub,ArgData argdata,uint8_t &counterclockwise); +//对一帧的数据进行处于并上传 点云 +void PublishLaserScan(rclcpp::Publisher::SharedPtr laser_pub, HPublish pub, ArgData argdata,uint8_t counterclockwise); +//对一个扇区的数据处理并上传 +void PublishLaserScanFan(rclcpp::Publisher::SharedPtr laser_pub, RawData *fan,std::string &frame_id,double min_dist, double max_dist,uint8_t inverted,uint8_t reversed); +//对一帧的数据进行处于并上传 三维 +//void PublishCloud(rclcpp::Publisher::SharedPtr &cloud_pub, HPublish pub, ArgData argdata); \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/data_process.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/data_process.h new file mode 100644 index 0000000000000000000000000000000000000000..86b55e67fa852e1b58fce8b0c770acb72ac83e9e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/data_process.h @@ -0,0 +1,20 @@ +#ifndef __DATA_PROC__ +#define __DATA_PROC__ +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include +#include"parser.h" +typedef struct +{ + unsigned short N; + uint32_t ts[2]; + + struct DataPoint points[0]; + +}DataPoints,*PDataPoints; + +bool data_process(sensor_msgs::msg::LaserScan&); +bool whole_data_process(RawData raw, bool from_zero,int collect_angle,std::vector& whole_datas,PDataPoints *data,int &size,char *result); +int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result); +int find(std::vectora, int n, int x); +#endif \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/parser.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/parser.h new file mode 100644 index 0000000000000000000000000000000000000000..aebdc38d2cb3195006ce987356c3419c98c0102f --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/parser.h @@ -0,0 +1,315 @@ +#ifndef _PARSER_ +#define _PARSER_ +#include +#include +#include +#include +#include +#define DF_UNIT_IS_MM 0x0001 +#define DF_WITH_INTENSITY 0X0002 +#define DF_DESHADOWED 0x0004 +#define DF_SMOOTHED 0x0008 +#define DF_FAN_90 0x0020 +#define DF_WITH_RPM 0X0040 +#define DF_WITH_RESAMPLE 0X0010 +#define DF_WITH_RESAMPLE_SOFT 0X0080 +#define DF_MOTOR_REVERSE 0x0100 +#define DF_WITH_UUID 0X1000 + +#define EF_ENABLE_ALARM_MSG 0X10000 + +#define MAX_FANS 120 + +#define MAX_POINTS 510 + +// #define ANYONE 0x1234abcd +#define ANYONE -1 + +#define HDR_SIZE 6 +#define HDR2_SIZE 8 +#define HDR3_SIZE 16 +#define HDR7_SIZE 28 +#define HDR99_SIZE 32 +#define HDRAA_SIZE 48 +#define BUF_SIZE 8 * 1024 + +#define getbit(x, y) ((x) >> (y)&1) // 获取X的Y位置数据 +#define setbit(x, y) x |= (1 << y) // 将X的第Y位置1 +#define clrbit(x, y) x &= ~(1 << y) // 将X的第Y位清0 + +struct DataPoint +{ + //uint16_t idx; + // int angle; + double degree; + uint16_t distance; // mm + uint8_t confidence; +}; +struct CmdHeader +{ + unsigned short sign; + unsigned short cmd; + unsigned short sn; + unsigned short len; +}; + +struct RawData +{ + unsigned short code; + unsigned short N; + unsigned short angle; // 0.1 degree + unsigned short span; // 0.1 degree + unsigned short fbase; + unsigned short first; + unsigned short last; + unsigned short fend; + // short ros_angle; // 0.1 degree + DataPoint points[MAX_POINTS]; + uint32_t ts[2]; + uint8_t counterclockwise; + uint32_t flags; //消息类型 +}; +struct AllPointData +{ + std::vectorpoints;//已缓存的所有数据 + std::vectortimestamp;//时间戳[2] + +}; + +struct EEpromV101 +{ + char label[4]; // "EPRM" + uint16_t pp_ver; // paramter protocol version + uint16_t size; // total size of this structure + + // uint32_t version; // firmware version + + // device + uint8_t dev_sn[20]; + uint8_t dev_type[16]; + uint32_t dev_id; // identiy + + // network + uint8_t IPv4[4]; + uint8_t mask[4]; + uint8_t gateway[4]; + uint8_t srv_ip[4]; + uint16_t srv_port; + uint16_t local_port; + + // + uint16_t RPM; + uint16_t RPM_pulse; + uint8_t fir_filter; + uint8_t cir; + uint16_t with_resample; + + uint8_t auto_start; + uint8_t target_fixed; + uint8_t with_smooth; + uint8_t with_filter; + + // + uint8_t ranger_bias[8]; + uint32_t net_watchdog; + + uint32_t pnp_flags; + uint16_t deshadow; + uint8_t zone_acted; + uint8_t should_post; + + uint8_t functions_map[16]; + uint8_t reserved[36]; +}; +typedef void *HParser; +typedef void *HReader; +typedef void *HPublish; + +struct LidarNode +{ + HParser hParser; + HPublish hPublish; + char ip[16]; + int port; + in_addr_t s_addr; +}; +struct RawDataHdr +{ + unsigned short code; + unsigned short N; + unsigned short angle; +}; + +struct RawDataHdr2 +{ + unsigned short code; + unsigned short N; + unsigned short angle; + unsigned short span; +}; + +struct RawDataHdr3 +{ + unsigned short code; + unsigned short N; + unsigned short angle; + unsigned short span; + unsigned short fbase; + unsigned short first; + unsigned short last; + unsigned short fend; +}; + +struct RawDataHdr7 +{ + uint16_t code; + uint16_t N; + uint16_t whole_fan; + uint16_t ofset; + uint32_t beg_ang; + uint32_t end_ang; + uint32_t flags; + uint32_t timestamp; + uint32_t dev_id; +}; +struct RawDataHdrAA { + uint16_t code; // 0xFAAA + uint16_t N; + uint16_t whole_fan; + uint16_t ofset; + uint32_t beg_ang; + uint32_t end_ang; + uint32_t flags; + uint32_t second; + uint32_t nano_sec; + uint32_t dev_id; + uint32_t reserved[4]; +}; +struct RawDataHdr99 +{ + uint16_t code; + uint16_t N; + uint16_t from; + uint16_t total; + uint32_t flags; + uint32_t timestamp; + uint32_t dev_no; + uint32_t reserved[3]; +}; + +typedef struct +{ + uint16_t code; + uint16_t len; + uint32_t clk; + uint32_t dev_id; +} PacketUart; + +struct FanSegment_C7 +{ + RawDataHdr7 hdr; + + uint16_t dist[MAX_POINTS]; + uint16_t angle[MAX_POINTS]; + uint8_t energy[MAX_POINTS]; + + struct FanSegment_C7 *next; +}; +struct FanSegment_AA +{ + RawDataHdrAA hdr; //CN：HdrAA结构体 EN：Hdr7structure + + uint16_t dist[MAX_POINTS]; //CN:距离 EN:distance + uint16_t angle[MAX_POINTS]; //CN:角度 EN:angle + uint8_t energy[MAX_POINTS]; //CN:能量强度 EN:energy intensity + + struct FanSegment_AA* next; // CN:下个扇区指针 EN:next sector pointer +}; +struct CommandList +{ + char uuid[12]; + char model[12]; + char rpm[12]; + char res[12]; + char smooth[12]; + char fitter[12]; + char confidence[12]; + char unit_mm[12]; + + char alarm[12]; + char direction[12]; + char ats[12]; + + +}; +struct Parser +{ + bool stream_mode; + int rest_len; + unsigned char rest_buf[BUF_SIZE]; + bool with_chk; + int raw_mode; + uint32_t dev_id; + uint32_t flags; + FanSegment_AA *fan_segs_aa; + FanSegment_C7 *fan_segs_c7; + int error_circle; + float error_scale; + bool is_from_zero; + char logPath[256]; + CommandList cmd; + char ip[16]; + int port; +}; + +struct UartState +{ + //byte1 + bool unit_mm;//0 cm 1 mm + bool with_conf;//0 close 1 open + bool with_smooth; + bool with_fitter; + bool span_9; + bool span_18; + bool span_other; + bool resampele;//重采样 + //byte2 + + bool moter_turn;//0正向 1反向 + bool span_8; + bool span_16; + //byte3 + bool byte3_error0; + bool byte3_error1; + bool byte3_error2; + bool byte3_error3; + //byte4 + bool byte4_error0; + bool byte4_error1; + bool byte4_error2; + bool byte4_error3; + bool byte4_error4; + bool byte4_error5; + bool byte4_error6; + bool byte4_error7; +}; + +HParser ParserOpen(int raw_bytes, bool with_chksum,int dev_id, + int error_circle, double error_scale, bool from_zero,CommandList cmd, char *ip, int port); + +int strip(const char *s, char *buf); +int ParserClose(HParser); +int ParserRunStream(HParser, int len, unsigned char *buf, RawData *fans[]); +int ParserRun(LidarNode, int len, unsigned char *buf, RawData *fans[]); + +void SetTimeStamp(RawData *); +void saveLog(const char *logPath, int type, const char *ip, const int port, const unsigned char *buf, unsigned int len); +int mkpathAll(std::string s, mode_t mode); +unsigned int stm32crc(unsigned int *ptr, unsigned int len); +int alarmProc(unsigned char *buf, int len); +int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result); + +std::string stringfilter(char *str, int num); +int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result); +int getFirstidx(RawData raw, int angle); +#endif diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/reader.h b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/reader.h new file mode 100644 index 0000000000000000000000000000000000000000..e1ed552ed079a3e761e153b8cf162f44c4a1f3ed --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/include/reader.h @@ -0,0 +1,119 @@ +#ifndef _READER_ +#define _READER_ + +#include "parser.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define MAX_LIDARS 8 +#define GS_PACK 0x4753 +#define S_PACK 0x0053 +#define C_PACK 0x0043 + +#define ROS_ERROR printf +#define ROS_INFO printf +struct UartInfo +{ + char type[8]; // uart or vpc + int fd_uart; + char port[128]; + int baudrate; + int *rate_list; + + HParser hParser; + HPublish hPublish; + pthread_t thr; +}; + +struct UDPInfo +{ + char type[8]; //"udp" + int nnode; + LidarNode lidars[MAX_LIDARS]; + + int fd_udp; + int listen_port; + bool is_group_listener; + pthread_t thr; + +}; + +struct ScriptParam +{ + UDPInfo *info; + int id; +}; + +struct KeepAlive +{ + uint32_t world_clock; + uint32_t mcu_hz; + uint32_t arrive; + uint32_t delay; + uint32_t reserved[4]; +}; + +struct LidarInfo { + HParser parser; + HPublish pub; + char lidar_ip[32]; + int lidar_port; +}; +void PublishData(HPublish, int, RawData**); + +HReader StartUartReader(const char* type ,const char* port, int baudrate, HParser, HPublish); +bool SendUartCmd(HReader, int len, char*); +bool SendVpcCmd(HReader hr, int len, char* cmd); + + +HReader StartUDPReader(const char* type,unsigned short listen_port, bool is_group_listener, const char* group_ip, + int lidar_count, const LidarInfo* lidars); + + +bool SendUdpCmd(HReader hr, int id, int len, char* cmd); +bool SendUdpCmd2(HReader hr, char* ip, int len, char* cmd); +HReader StartTCPReader(const char* lidar_ip, unsigned short lidar_port, HParser hParser, HPublish hPub); + +bool SendTcpCmd(HReader hr, int len, char* cmd); + +void StopUartReader(HReader hr); +void StopUDPReader(HReader hr); +void StopTCPReader(HReader hr); + + +//UART +int open_serial_port(const char *port, int baudrate); +bool uart_talk(int fd, int n, const char* cmd, int nhdr, const char* hdr_str, int nfetch, char* fetch,int waittime=10,int cachelength=4096); + + + +//VPC +bool vpc_talk(int hcom, int mode, short sn, int len, const char* cmd, int nfetch, void* result); + + +//udp +bool setup_lidar_udp(HParser hP, int handle); +void send_cmd_udp(int fd_udp, const char* dev_ip, int dev_port, int cmd, int sn, int len, const void* snd_buf); +bool udp_talk_S_PACK(int fd_udp, const char* ip, int port, int n, const char* cmd, void* result); +bool udp_talk_C_PACK(int fd_udp, const char* lidar_ip, int lidar_port,int n, const char* cmd,int nhdr, const char* hdr_str,int nfetch, char* fetch); +bool udp_talk_GS_PACK(int fd_udp, const char *ip, int port, int n, const char *cmd, void *result); + +//common + +#endif diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15BDM.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15BDM.launch new file mode 100644 index 0000000000000000000000000000000000000000..7fa05ad67453691d12b3347b35ac4c818324f6dd --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15BDM.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-15BDM.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15D-B25R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15D-B25R.launch new file mode 100644 index 0000000000000000000000000000000000000000..3bc97845d3719ec78448fe4ab13288a3e5909278 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-15D-B25R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-15D-B25R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25BDM.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25BDM.launch new file mode 100644 index 0000000000000000000000000000000000000000..56ba80b4b957994928166d1b0d3813313d43cbf4 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25BDM.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-25BDM.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25D-B25R .launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25D-B25R .launch new file mode 100644 index 0000000000000000000000000000000000000000..ec821236298abab70f963fbdb3e345b3257d7cfe --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-25D-B25R .launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-25D-B25R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-2.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-2.launch new file mode 100644 index 0000000000000000000000000000000000000000..59d0ddaeb8c507bf87661dca345f5eda8f681988 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-2.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-2.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-3.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-3.launch new file mode 100644 index 0000000000000000000000000000000000000000..ec124a4e83881bea181ddf9ba45eb1e00978646a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-3.launch @@ -0,0 +1,47 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-3.yaml'), + description='FPath to the ROS2 parameters file to use.') + + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C20E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C20E.launch new file mode 100644 index 0000000000000000000000000000000000000000..1658ecb3c98a8dedfd62ba0792aad668da21ed0d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C20E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-C20E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C30E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C30E.launch new file mode 100644 index 0000000000000000000000000000000000000000..e5864e2e8e20f36c375db6260f26bc83ea049e9f --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-C30E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-C30E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..cdc08c0dc0749d5e1ef340209f191bfe3208ee9b --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..929076537b6617a7b441478caab376a67852a25a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-50C-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-50C-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E100-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E100-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..2752a4916089996d7bdcb6499c404b3c544dcbc7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E100-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E100-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E110-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E110-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..95a53aaa29da4658985becf8277eecf28d8c8ed4 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E110-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E110-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E120-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E120-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..141fee08fecccceb07a44b887f9245974eb9bc1d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E120-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E120-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-A.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-A.launch new file mode 100644 index 0000000000000000000000000000000000000000..0590c646836aeff3e3e16e484b8743bca0da3db7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-A.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E200-A.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..30a90f88dcf3a6c8c813261877bfa3d8e3a91eb7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E200-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..4d6b77f7e27f3a5a5b123966dcce8d7f74b7ec5d --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E200-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E200-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..08630699218371b1623d5e60a3e3ec4a3ef89836 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-E.launch @@ -0,0 +1,46 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E300-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("share_dir:",share_dir); + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..843d5de2418631463aa4da060f5e6452a6ee4301 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E300-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E300-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..74f96fb04cec89eecf925467154689b6f67a9455 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E310-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..da71e91eb6731bc75ab33ac98ad3db7a73c0a9d6 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E310-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E310-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E320-S.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E320-S.launch new file mode 100644 index 0000000000000000000000000000000000000000..1606830e82d49dbd2a666a836b8adbeb47315de7 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E320-S.launch @@ -0,0 +1,46 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E320-S.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S-R.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S-R.launch new file mode 100644 index 0000000000000000000000000000000000000000..520db328feab2b7733efafb814bb4f5f5b9f4b2c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S-R.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E330-S-R.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S.launch new file mode 100644 index 0000000000000000000000000000000000000000..cb77ee28a695b3c6255c3e0c172b70080cb2699e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E330-S.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E330-S.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..73bfa1f989397e1e5106c58d2c001ef4483d6b23 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E340-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-S.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-S.launch new file mode 100644 index 0000000000000000000000000000000000000000..9d874b3782f1f92d22e5d49bb6e2b3399e045d5c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E340-S.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E340-S.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E400-E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E400-E.launch new file mode 100644 index 0000000000000000000000000000000000000000..b1abd6d0394a93d5b1f4e09deefa7650806cd5e0 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LDS-E400-E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LDS-E400-E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LES-10A-D40U44.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LES-10A-D40U44.launch new file mode 100644 index 0000000000000000000000000000000000000000..72c79c48d9be5ead63f0dae9ac4d30f5e42d5825 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LES-10A-D40U44.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LES-10A-D40U44.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C20E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C20E.launch new file mode 100644 index 0000000000000000000000000000000000000000..e953fc8a5549e10b0351fc40dee852ee47a253eb --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C20E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LSS-40D-C20E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E.launch new file mode 100644 index 0000000000000000000000000000000000000000..fba2b33f89d624be2e7ad051a9d317de71d71d13 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LSS-40D-C30E.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E_VPC.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E_VPC.launch new file mode 100644 index 0000000000000000000000000000000000000000..2d11c2f1fe4fbc64c08472034b3c7f9a985faf59 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40D-C30E_VPC.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LSS-40D-C30E_VPC.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40S-B20E44.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40S-B20E44.launch new file mode 100644 index 0000000000000000000000000000000000000000..2651081209d74293461c58863741874e42cea0a6 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/LSS-40S-B20E44.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'LSS-40S-B20E44.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/VPC.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/VPC.launch new file mode 100644 index 0000000000000000000000000000000000000000..fcb9ff5f7874800e19bf091b8c22f8dbaf7bf1af --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/VPC.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'VPC.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/bluesea.py b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/bluesea.py new file mode 100644 index 0000000000000000000000000000000000000000..72a89481001182f3cd19b6017b8b1fbea9055a12 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/bluesea.py @@ -0,0 +1,29 @@ +from launch.exit_handler import ignore_exit_handler, restart_exit_handler +from ros2run.api import get_executable_path + + +def launch(launch_descriptor, argv): + ld = launch_descriptor + package = 'bluesea2' + ld.add_process( + cmd=[get_executable_path(package_name=package, executable_name='bluesea_node')], + name='bluesea_node', + exit_handler=restart_exit_handler, + ) + package = 'tf2_ros' + ld.add_process( + # The XYZ/Quat numbers for base_link -> laser_frame are taken from the + # turtlebot URDF in + # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro + cmd=[ + get_executable_path( + package_name=package, executable_name='static_transform_publisher'), + '0', '0', '0.02', + '0', '0', '0', '1', + 'base_link', + 'laser_frame' + ], + name='static_tf_pub_laser', + exit_handler=restart_exit_handler, + ) + return ld diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/dual_lidar.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/dual_lidar.launch new file mode 100644 index 0000000000000000000000000000000000000000..585bfc001de6ed30df8e3e770bb91114630f0daf --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/dual_lidar.launch @@ -0,0 +1,45 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('bluesea2') + parameter_file = LaunchConfiguration('params_file') + node_name = 'bluesea_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'dual_lidar.yaml'), + description='FPath to the ROS2 parameters file to use.') + + #对多版本的兼容性处理 首字母e以及之前为18.04或更早,之后为20.04或更晚 + ROS_DISTRO='' + ROS_DISTRO = os.getenv('ROS_DISTRO') + print("Current ROS2 Version: ",ROS_DISTRO) + if ROS_DISTRO[0] <= 'e': + try: + driver_node = LifecycleNode( node_name='bluesea_node', node_namespace='/', package='bluesea2', node_executable='bluesea_node', output='screen', parameters=[parameter_file]) + except: + pass + else : + try: + driver_node = LifecycleNode( name='bluesea_node', namespace='/', package='bluesea2', executable='bluesea_node', output='screen', emulate_tty=True, parameters=[parameter_file]) + except: + pass + + + return LaunchDescription([ + params_declare, + driver_node + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/test.launch b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/test.launch new file mode 100644 index 0000000000000000000000000000000000000000..26dd20ec0db851b87b956f7493dfdca94a8cf358 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/launch/test.launch @@ -0,0 +1,25 @@ +#!/usr/bin/python3 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + + +def generate_launch_description(): + pkg_gazebo_ros = get_package_share_directory('bluesea2') + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros,"launch"),"LDS-E300-E.launch") + ) + return LaunchDescription([ + gazebo_launch + + ]) \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/package.xml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..bcb62d68375db34ee9d765b75bcb7be5db813488 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/package.xml @@ -0,0 +1,35 @@ + + + + bluesea2 + 1.1.1 + Lanhai Lidar ROS2 + rayn john + Apache License 2.0 + + ament_cmake + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15BDM-2.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15BDM-2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..c3584bdc6a0322d8a21f18103bc592dd1d045ea5 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15BDM-2.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 230400 #rate_list[230400,256000]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15D-B25R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15D-B25R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..549cca46c5736843ae4f4c1d93dcebe050d43900 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-15D-B25R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 768000 #rate_list[ 768000, 921600, 1000000 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25BDM-2.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25BDM-2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..aa8f387617b085c81daa8bbfef65579c24a65c95 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25BDM-2.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 230400 #rate_list[ 230400, 256000 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25D-B25R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25D-B25R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..549cca46c5736843ae4f4c1d93dcebe050d43900 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-25D-B25R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 768000 #rate_list[ 768000, 921600, 1000000 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-2.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f514a3b52121cc8f22682c712b740d7d7edf4a28 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-2.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 500000 #rate_list[ 384000, 500000, 768000, 921600 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-3.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-3.yaml new file mode 100644 index 0000000000000000000000000000000000000000..63088c49d3a644adc9531c095cce7fafcf701979 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-3.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 500000 #rate_list[ 500000, 768000, 921600 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C20E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C20E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C20E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f8fc9f26dcef00b19a80125b4d8435c0e7ee5b28 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E-R.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-C30E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f8fc9f26dcef00b19a80125b4d8435c0e7ee5b28 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e45cd48bd9516d5316bd808745ced2de9f71b38a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-50C-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 921600 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E100-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E100-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e45cd48bd9516d5316bd808745ced2de9f71b38a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E100-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 921600 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E110-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E110-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f420e775bc6381420b6734bfd198a19dee20b0a3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E110-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "base_link" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyAMA4" + baud_rate: 230400 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 #E120不支持修改 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E120-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E120-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..878d0ad33553b54dcc58587b0de8cdd97b7742b8 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E120-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: true + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyAMA4" + baud_rate: 230400 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 #E120不支持修改 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-A.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-A.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e45cd48bd9516d5316bd808745ced2de9f71b38a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-A.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 921600 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6bdc22999fa1542986eaa93f63498b0132446e72 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.0.99" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e45cd48bd9516d5316bd808745ced2de9f71b38a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E200-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 921600 + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..b0e7907ac501cb261b9f422676dd09c6c8e45cfd --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2 + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2de3d731ac42fe42c31567a49883324f9caa6b64 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E300-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 768000 #rate_list[ 768000, 1000000 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..81d36636e95d11bf30fdd0426bb3cbfaa6dbe5e4 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E310-R.yaml @@ -0,0 +1,45 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 +#CONNECT# + type: "uart" + port: "/dev/ttyUSB0" + baud_rate: 100000 #rate_list[ 768000, 1000000 ]# + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + model: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E320-S.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E320-S.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E320-S.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E321-S.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E321-S.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E321-S.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f8fc9f26dcef00b19a80125b4d8435c0e7ee5b28 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S-R.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S.yaml new file mode 100644 index 0000000000000000000000000000000000000000..717d069e35675b9b0a18c6141e6c2ede6139a3b3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E330-S.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: true + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E-R.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E-R.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f8fc9f26dcef00b19a80125b4d8435c0e7ee5b28 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E-R.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: true + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-S.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-S.yaml new file mode 100644 index 0000000000000000000000000000000000000000..eb7a8f317b506563a70d31f4bf290ab8141352db --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E340-S.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: true + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.01 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.0.88" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E400-E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E400-E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..34439fd91a6dce3acbc712779ea8a92cd5296ac3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LDS-E400-E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.0.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-10A-C30E44.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-10A-C30E44.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-10A-C30E44.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E_VPC.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E_VPC.yaml new file mode 100644 index 0000000000000000000000000000000000000000..27adc90b552e1ec57d50c9ae8af55d28e5c44a56 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C20E_VPC.yaml @@ -0,0 +1,49 @@ +bluesea_node: + ros__parameters: + #ROS2 + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "vpc" + port: "/dev/ttyACM0" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: 1 + + + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8d4a1ab7f8f9162b5f67dc9b30a89bf32664ce6b --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E.yaml @@ -0,0 +1,53 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 + + + + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E_VPC.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E_VPC.yaml new file mode 100644 index 0000000000000000000000000000000000000000..27adc90b552e1ec57d50c9ae8af55d28e5c44a56 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40D-C30E_VPC.yaml @@ -0,0 +1,49 @@ +bluesea_node: + ros__parameters: + #ROS2 + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "vpc" + port: "/dev/ttyACM0" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: 1 + + + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40S-B20E44.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40S-B20E44.yaml new file mode 100644 index 0000000000000000000000000000000000000000..10519bf2f9e662a724babff0f7331b1f28e6b51a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/LSS-40S-B20E44.yaml @@ -0,0 +1,48 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + lidar_ip: "192.168.158.98" + lidar_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/VPC.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/VPC.yaml new file mode 100644 index 0000000000000000000000000000000000000000..27adc90b552e1ec57d50c9ae8af55d28e5c44a56 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/VPC.yaml @@ -0,0 +1,49 @@ +bluesea_node: + ros__parameters: + #ROS2 + frame_id: "map" + topic: "scan" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "vpc" + port: "/dev/ttyACM0" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: 1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: 1 + + + + diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/dual_lidar.yaml b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/dual_lidar.yaml new file mode 100644 index 0000000000000000000000000000000000000000..cf06afaaa4e2eb96535ab42774d24df6934d14b6 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/params/dual_lidar.yaml @@ -0,0 +1,52 @@ +bluesea_node: + ros__parameters: + #ROS2# + frame_id: "map" + topic: "scan" + topic1: "scan1" + #DATA# + dev_id: -1 + raw_bytes: 3 + output_360: true + output_scan: true + output_cloud: false + with_angle_filter: false + min_angle: -3.14159 + max_angle: 3.14159 + mask1: "0,3.14" + mask2: "-3,-2" + hard_resample: false + soft_resample: false + with_checksum: true + inverted: false + reversed: false + min_dist: 0.1 + max_dist: 50.0 + error_circle: 3 + error_scale: 0.9 + #CONNECT# + type: "udp" + number: 2 + lidar_ip: "192.168.0.99" + lidar_port: 6543 + lidar1_ip: "192.168.0.91" + lidar1_port: 6543 + local_port: 6668 + group_ip: "224.1.2.3" + #FITTER# + filter_open: false #滤波生效开关 + max_range: 20.0 #滤波生效的最大距离 + min_range: 0.5 #滤波生效的最小距离 + max_range_difference: 0.1 #离异点判断的物理范围 + filter_window: 1 #判断离异点下标的范围 + #GET# + uuid: -1 + #SET# + resample_res: -1.0 + with_confidence: -1 + with_smooth: -1 + with_deshadow: -1 + alarm_msg: -1 + rpm: -1 + direction: -1 + ats: -1 diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/LHLiDAR.rules b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/LHLiDAR.rules new file mode 100644 index 0000000000000000000000000000000000000000..51dd9681f3486758387619356a135593e97ad01e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/LHLiDAR.rules @@ -0,0 +1,2 @@ +KERNEL=="ttyUSB*", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0666", SYMLINK+="LHLiDAR" +KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", SYMLINK+="LHLiDAR" diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/algorithmAPI.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/algorithmAPI.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d343c8b0eb2dda71b721252e174ab4e343df0284 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/algorithmAPI.cpp @@ -0,0 +1,106 @@ +#include "algorithmAPI.h" +#include +#include + +bool checkWindowValid2(std::vector scan, size_t idx, size_t window, double max_distance,double angle_increment) +{ + + int num_neighbors = 0; + const float r1 = scan.at(idx).distance; // 当前点云的距离数据 + float r2 = 0.; // 范围内点云的数据 + // Look around the current point until either the window is exceeded + // or the number of neighbors was found. + for (int y = -(int)window; y < (int)window + 1 && num_neighbors < (int)window; y++) + { + int j = idx + y; + if (j < 0 || j >= static_cast(scan.size())||idx == j) + { + continue; + } + r2 = scan.at(j).distance; + if(r2==0) + continue; + // delete the re in the result + const float d = sqrt(pow(r1, 2) + pow(r2, 2) -(2 * r1 * r2 * cosf(y * angle_increment))); + if (d <= max_distance) + { + num_neighbors++; + } + } + //printf("%s %d %d %d\n", __FUNCTION__, __LINE__,num_neighbors,window); + // consider the window to be the number of neighbors we need + if (num_neighbors < window) + // if(num_neighbors == 0)//only the window is one + { + return false; // invalid + } + else + { + return true; // effective + } +} + +bool filter(std::vector &output_scan, int filter_type, double max_range, double min_range, double max_range_difference, int filter_window,double angle_increment) +{ + std::vector valid_ranges; + /*Check if range size is big enough to use7 the filter window */ + if (output_scan.size() <= filter_window + 1) + { + printf("Scan ranges size is too small: size = %ld", output_scan.size()); + return false; + } + size_t i = 0; + size_t i_max = output_scan.size(); + valid_ranges.clear(); + while (i < i_max) + { + bool out_of_range = ((output_scan.at(i).distance > max_range) || (output_scan.at(i).distance < min_range)); + // ROS_INFO("%lf", min_range); + valid_ranges.push_back(out_of_range); + ++i; + } + i = 0; + i_max = output_scan.size() - filter_window + 1; + while (i < i_max) + { + bool window_valid = checkWindowValid2(output_scan, i, filter_window, max_range_difference,angle_increment); + + if (window_valid) + { + size_t j = i, j_max = i + filter_window; + do + { + valid_ranges[j++] = true; + } while (j < j_max); + } + ++i; + } + i = 0; + i_max = valid_ranges.size(); + int errnum=0; + while (i < i_max) + { + if (!valid_ranges[i]) + { + output_scan[i].distance = std::numeric_limits::infinity(); + errnum++; + } + ++i; + } + //printf("%ld not valid num:%d\n",i,errnum); + return true; +} +bool checkZeroDistance(std::vectordata,float error_scale) +{ + bool result=true; + int lengthZeroNum = 0; + for(int i=0;i +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Current ROS2 Version is %s",getROS2Version()); + if (argc != 2) + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: ros2 run bluesea2 bluesea_node_client stop/start"); + return 1; + } + std::shared_ptr node = rclcpp::Node::make_shared("bluesea_node_client"); + char cmd[8] = {0}; + strcpy(cmd, argv[1]); + rclcpp::Client::SharedPtr client; + if (strcmp(cmd, "start") == 0) + { + client = node->create_client("start"); // CHANGE + } + else if (strcmp(cmd, "stop") == 0) + { + client = node->create_client("stop"); + } + + auto request = std::make_shared(); // CHANGE + while (!client->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + auto result = client->async_send_request(request); + // Wait for the result. + //ros2 rolling 20.04 + #ifdef ROS_DISTRO_F + if(rclcpp::spin_until_future_complete(node, result)==rclcpp::FutureReturnCode::SUCCESS) + #elif defined ROS_DISTRO_E + //ros2 eloquent 18.04 and lower + if(rclcpp::spin_until_future_complete(node, result)==rclcpp::executor::FutureReturnCode::SUCCESS) + #endif + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "client OK"); + } + else + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call ctrl_service "); // CHANGE + } + + rclcpp::shutdown(); + return 0; +} diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/bluesea_node.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/bluesea_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..041ddce5fd237b8bbba9c7e4d701a2e5547ea90f --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/bluesea_node.cpp @@ -0,0 +1,1046 @@ +#include +#include +#include +#include +#include +#include + +#include +// #include "timer.h" +#include "../include/reader.h" +#include "../include/algorithmAPI.h" +#include"../include/bluesea_node.h" +#include + +#define ROS2Verision "2.0" + +HReader g_reader = NULL; +std::string g_type = "uart"; + + +void closeSignal(int) +{ + rclcpp::shutdown(); + exit(1); +} + +bool SendCmd(int len, char *cmd) +{ + if (g_type == "uart") + { + return SendUartCmd(g_reader, len, cmd); + } + else if (g_type == "vpc") + { + return SendVpcCmd(g_reader, len, cmd); + } + else if (g_type == "udp") + { + int tmp = 0; + memcpy(&tmp, g_reader, sizeof(int)); + for (int i = 0; i < tmp; i++) + { + SendUdpCmd(g_reader, i, len, cmd); + } + return true; + } + else if (g_type == "tcp") + { + return SendTcpCmd(g_reader, len, cmd); + } + return false; +} + +void PublishData(HPublish pub, int n, RawData **fans) +{ + int skip = 0; + RawData *drop[MAX_FANS]; + PubHub *hub = (PubHub *)pub; + + pthread_mutex_lock(&hub->mtx); + + if (hub->nfan + n > MAX_FANS) + { + int nr = hub->nfan + n - MAX_FANS; + + for (int i = 0; i < nr; i++) + drop[skip++] = hub->fans[i]; + + for (int i = nr; i < hub->nfan; i++) + { + hub->fans[i - nr] = hub->fans[i]; + } + + hub->nfan -= nr; + } + + for (int i = 0; i < n; i++) + { + hub->fans[hub->nfan++] = fans[i]; + } + pthread_mutex_unlock(&hub->mtx); + for (int i = 0; i < skip; i++) + { + delete drop[i]; + } +} + +#if 0 +int angle_cmp(const void* p1, const void* p2) +{ + const RawData** pp1 = (const RawData**)p1; + const RawData** pp2 = (const RawData**)p2; + return (*pp1)->ros_angle - (*pp2)->ros_angle; +} +#endif + +void resample(RawData *dat, int NN) +{ + int *index = new int[NN]; + double *errs = new double[NN]; + + for (int i = 0; i < NN; i++) + index[i] = -1; + + for (int i = 0; i < dat->N; i++) + { + if (dat->points[i].distance == 0) + continue; + + int idx = round(double(i * NN) / dat->N); + if (idx < NN) + { + double err = fabs(double(dat->span * i) / dat->N - double(dat->span * idx) / NN); + if (index[idx] == -1 || err < errs[idx]) + { + index[idx] = i; + errs[idx] = err; + } + } + } + + for (int i = 1; i < NN; i++) + { + if (index[i] != -1) + { + dat->points[i] = dat->points[index[i]]; + } + else + { + dat->points[i].distance = 0; + dat->points[i].confidence = 0; + } + } + + dat->N = NN; + + delete index; + delete errs; +} + +bool GetFan(HPublish pub, bool with_resample, double resample_res, RawData **fans) +{ + bool got = false; + PubHub *hub = (PubHub *)pub; + if (hub->nfan > 0) + { + pthread_mutex_lock(&hub->mtx); + fans[0] = hub->fans[0]; + for (int i = 1; i < hub->nfan; i++) + hub->fans[i - 1] = hub->fans[i]; + hub->nfan--; + got = true; + pthread_mutex_unlock(&hub->mtx); + } + else + return false; + + if (with_resample) // && resample_res > 0.05) + { + int NN = fans[0]->span / (10 * resample_res); + + if (NN < fans[0]->N) + { + resample(fans[0], NN); + } + else if (NN > fans[0]->N) + { + printf("fan %d less than %d\n", fans[0]->N, NN); + } + } + return got; +} + +int GetAllFans(HPublish pub, ArgData argdata, uint8_t &counterclockwise) +{ + PubHub *hub = (PubHub *)pub; + RawData *fans[MAX_FANS]; + // int checkAngle = (from_zero == 0 ? 0 : 180); + // 解析出来一帧的数据 + pthread_mutex_lock(&hub->mtx); + int cnt = 0, nfan = 0; + for (int i = 1; i < hub->nfan; i++) + { + if (hub->fans[i]->angle == hub->offsetangle) + { + + hub->ts_end[0] = hub->fans[i]->ts[0]; + hub->ts_end[1] = hub->fans[i]->ts[1]; + cnt = i; + break; + } + } + if (cnt > 0) + { + // 起始点位在扇区的中间部分，删除扇区时就需要多保存一个扇区 + if (hub->offsetidx > 0) + { + nfan = cnt + 1; + for (int i = 0; i <= cnt; i++) + { + fans[i] = hub->fans[i]; + } + for (int i = cnt; i < hub->nfan; i++) + hub->fans[i - cnt] = hub->fans[i]; + + hub->nfan -= cnt; + } + else + { + nfan = cnt; + for (int i = 0; i < cnt; i++) + { + fans[i] = hub->fans[i]; + } + for (int i = cnt; i < hub->nfan; i++) + hub->fans[i - cnt] = hub->fans[i]; + hub->nfan -= cnt; + } + } + pthread_mutex_unlock(&hub->mtx); + // 一帧数据的丢包检测以及软采样角分辨率的适配 + if (cnt > 0) + { + counterclockwise = fans[0]->counterclockwise; + + bool circle = true; + int total = fans[0]->span; + hub->ts_beg[0] = fans[0]->ts[0]; + hub->ts_beg[1] = fans[0]->ts[1]; + + for (int i = 0; i < nfan - 1; i++) + { + if ((fans[i]->angle + fans[i]->span) % 3600 != fans[i + 1]->angle) + { + circle = false; + } + total += fans[i + 1]->span; + } + //printf("%d %d %d \n", circle,total,hub->offsetidx); + if (!circle || (total != 3600 && hub->offsetidx == 0) || ((total != 3600 + fans[0]->span) && hub->offsetidx > 0)) + { + // clean imcomplent datas + for (int i = 0; i < cnt; i++) + delete fans[i]; + cnt = 0; + } + } + if (cnt > 0) + { + if (argdata.soft_resample && argdata.resample > 0) + { + for (int i = 0; i < cnt; i++) + { + int NN = fans[i]->span / (10 * argdata.resample); + if (NN < fans[i]->N) + { + // printf("%s %d %d,NN:%d,fans[i]->N:%d %lf\n",__FUNCTION__,__LINE__,cnt,NN,fans[i]->span,resample_res); + resample(fans[i], NN); + } + else if (NN > fans[i]->N) + { + printf("fan [%d] %d less than %d\n", i, fans[i]->N, NN); + } + } + } + // 将扇区数据转为点 列表 + + for (int j = hub->offsetidx; j < fans[0]->N; j++) + { + hub->consume.push_back(fans[0]->points[j]); + } + for (int i = 1; i < cnt; i++) + { + for (int j = 0; j < fans[i]->N; j++) + { + hub->consume.push_back(fans[i]->points[j]); + } + } + for (int j = 0; j < hub->offsetidx; j++) + { + hub->consume.push_back(fans[cnt]->points[j]); + } + for (int i = 0; i < cnt; i++) + delete fans[i]; + // ROS_INFO("%s %d %f %f\n", __FUNCTION__, __LINE__, hub->consume[0].degree,hub->consume[hub->consume.size()-1].degree); + + if (cnt > 0) + { + bool res = checkZeroDistance(hub->consume, argdata.custom.error_scale); + if (res) + hub->error_num = 0; + else + { + hub->error_num++; + if (hub->error_num >= argdata.custom.error_circle) + { + printf("There are many points with a distance of 0 in the current lidar operation"); + hub->error_num = 0; + } + } + } + + int N = hub->consume.size(); + double angle_increment = 0; + + if (argdata.from_zero) + { + if (argdata.inverted) + angle_increment = M_PI * 2 / N; + else + angle_increment = -M_PI * 2 / N; + } + else + { + if (argdata.inverted) + angle_increment = M_PI * 2 / N; + else + angle_increment = -M_PI * 2 / N; + } + + if (counterclockwise) + angle_increment = -angle_increment; + Fitter *fitter = &argdata.fitter; + //ROS_DEBUG("%d %d %f %f %f %d %lf\n",fitter->isopen,fitter->type, fitter->max_range, fitter->min_range, fitter->max_range_difference, fitter->filter_window,angle_increment); + if (fitter->isopen) + filter(hub->consume, fitter->type, fitter->max_range*1000, fitter->min_range*1000, fitter->max_range_difference*1000, fitter->filter_window, angle_increment); + } + return cnt; +} + +void SetTimeStamp(RawData *dat) +{ + timeval t; + gettimeofday(&t, NULL); + // ros::Time t = ros::Time::now(); + dat->ts[0] = t.tv_sec; + dat->ts[1] = t.tv_usec * 1000; +} + +double ROSAng(double ang) +{ + ang = -ang; + return ang < -180 ? ang + 360 : ang; + // return ang < 180 ? ang : ang - 360; +} + +int GetCount(std::vector data, double min_deg, double max_deg, double &min_pos, double &max_pos) +{ + int N = 0; + + for (unsigned int i = 0; i < data.size(); i++) + { + double deg = ROSAng(data[i].degree); + + if (deg < min_deg || deg > max_deg) + continue; + if (N == 0) + { + min_pos = deg; + max_pos = deg; + } + else + { + if (min_pos > deg) + min_pos = deg; + if (max_pos < deg) + max_pos = deg; + } + N++; + } + + // printf("angle filter [%f, %f] %d to %d, [%f, %f]\n", min_deg, max_deg, cnt, N, min_pos, max_pos); + return N; +} + +void PublishLaserScanFan(rclcpp::Publisher::SharedPtr laser_pub, RawData *fan, std::string &frame_id, double min_dist, double max_dist, uint8_t inverted, uint8_t reversed) +{ + sensor_msgs::msg::LaserScan msg; + msg.header.stamp.sec = fan->ts[0]; + msg.header.stamp.nanosec = fan->ts[1]; + msg.header.frame_id = frame_id; + + int N = fan->N; + double scan_time = 1 / 100.; + msg.scan_time = scan_time; + msg.time_increment = scan_time / fan->N; + + msg.range_min = min_dist; + msg.range_max = max_dist; // 8.0; + double min_pos, max_pos; + msg.intensities.resize(N); // fan->N); + msg.ranges.resize(N); // fan->N); + if (inverted) + { + min_pos = ROSAng(-fan->angle / 10) * 10 * M_PI / 1800; + max_pos = -min_pos; // + fan->span * M_PI / 1800; + } + else + { + min_pos = ROSAng(fan->angle / 10) * 10 * M_PI / 1800; + max_pos = min_pos; //-fan->span * M_PI / 1800; + } + msg.intensities.resize(N); // fan->N); + msg.ranges.resize(N); // fan->N); + + if (inverted) + { + msg.angle_min = min_pos; + msg.angle_max = max_pos; + msg.angle_increment = (fan->span * M_PI / 1800) / fan->N; + } + else + { + msg.angle_min = max_pos; + msg.angle_max = min_pos; + msg.angle_increment = -(fan->span * M_PI / 1800) / fan->N; + } + N = 0; + if (reversed) + { + for (int i = fan->N - 1; i >= 0; i--) + { + //double deg = ROSAng(fan->points[i].degree); + + double d = fan->points[i].distance / 1000.0; + + if (fan->points[i].distance == 0 || d > max_dist || d < min_dist) + msg.ranges[N] = std::numeric_limits::infinity(); + else + msg.ranges[N] = d; + + msg.intensities[N] = fan->points[i].confidence; + N++; + } + } + else + { + for (int i = 0; i < fan->N; i++) + { + //double deg = ROSAng(fan->points[i].degree); + // customize angle filter + double d = fan->points[i].distance / 1000.0; + + if (fan->points[i].distance == 0 || d > max_dist || d < min_dist) + msg.ranges[N] = std::numeric_limits::infinity(); + else + msg.ranges[N] = d; + + msg.intensities[N] = fan->points[i].confidence; + N++; + } + } + laser_pub->publish(msg); +} +#if 0 +void PublishCloud(rclcpp::Publisher::SharedPtr cloud_pub, int nfan, RawData** fans, std::string& frame_id, + double max_dist, + bool with_filter, double min_ang, double max_ang) +{ + sensor_msgs::PointCloud cloud; + //cloud.header.stamp = ros::Time::now(); + + cloud.header.stamp.sec = fans[0]->ts[0]; + cloud.header.stamp.nsec = fans[0]->ts[1]; + + cloud.header.frame_id = frame_id; + + int N = 0; + for (int i=0; iN; + + cloud.points.resize(N); + cloud.channels.resize(1); + cloud.channels[0].name = "intensities"; + cloud.channels[0].values.resize(N); + + + double min_deg = min_ang * 180 / M_PI; + double max_deg = max_ang * 180 / M_PI; + if (with_filter) + { + N = GetCount(nfan, fans, min_deg, max_deg); + } + + int idx = 0; + for (int j=0; jN; i++) + { + if (with_filter) { + if (ROSAng(fans[j]->points[i].degree) < min_deg) continue; + if (ROSAng(fans[j]->points[i].degree) > max_deg) continue; + } + + float r = fans[j]->points[i].distance/1000.0 ; + // float a = j*M_PI/5 + i*M_PI/5/dat360[j].N; + float a = -fans[j]->points[i].degree * M_PI/180; + + cloud.points[idx].x = cos(a) * r; + cloud.points[idx].y = sin(a) * r; + cloud.points[idx].z = 0; + cloud.channels[0].values[idx] = fans[j]->points[i].confidence; + idx++; + } + } + cloud_pub.publish(cloud); +} + +#endif + +int time_cmp(const uint32_t *t1, const uint32_t *t2) +{ + if (t1[0] > t2[0]) + return 1; + else if (t1[0] < t2[0]) + return -1; + else if (t1[1] > t2[1]) + return 1; + else if (t1[1] < t2[1]) + return -1; + + return 0; +} + +void PublishLaserScan(rclcpp::Publisher::SharedPtr laser_pub,HPublish pub, ArgData argdata, uint8_t counterclockwise) +{ + PubHub *hub = (PubHub *)pub; + sensor_msgs::msg::LaserScan msg; + int N = hub->consume.size(); + // make min_ang max_ang convert to mask + msg.header.stamp.sec = hub->ts_beg[0]; + msg.header.stamp.nanosec = hub->ts_beg[1]; + + double ti = double(hub->ts_beg[0]) + double(hub->ts_beg[1]) / 1000000000.0; + double tx = double(hub->ts_end[0]) + double(hub->ts_end[1]) / 1000000000.0; + + msg.scan_time = tx - ti; // nfan/(nfan-1); + msg.time_increment = msg.scan_time / N; + + msg.header.frame_id = argdata.frame_id; + + double min_deg = argdata.min_angle * 180 / M_PI; + double max_deg = argdata.max_angle * 180 / M_PI; + + msg.range_min = argdata.min_dist; + msg.range_max = argdata.max_dist; // 8.0; + if (argdata.with_angle_filter) + { + double min_pos, max_pos; + N = GetCount(hub->consume, min_deg, max_deg, min_pos, max_pos); + if (argdata.inverted) + { + msg.angle_min = min_pos * M_PI / 180; + msg.angle_max = max_pos * M_PI / 180; + // printf("data1:deg:%lf %lf angle:%lf %lf %d\n",min_deg,max_deg,msg.angle_min,msg.angle_max,N); + } + else + { + msg.angle_min = max_pos * M_PI / 180; + msg.angle_max = min_pos * M_PI / 180; + } + msg.angle_increment = (msg.angle_max - msg.angle_min) / (N - 1); + } + else if (argdata.from_zero) + { + if (argdata.inverted) + { + msg.angle_min = 0; + msg.angle_max = 2 * M_PI * (N - 1) / N; + msg.angle_increment = M_PI * 2 / N; + } + else + { + msg.angle_min = 2 * M_PI * (N - 1) / N; + msg.angle_max = 0; + msg.angle_increment = -M_PI * 2 / N; + } + } + else + { + if (argdata.inverted) + { + msg.angle_min = -M_PI; + msg.angle_max = M_PI - (2 * M_PI) / N; + msg.angle_increment = M_PI * 2 / N; + } + else + { + msg.angle_min = M_PI; + msg.angle_max = -M_PI + (2 * M_PI) / N; + msg.angle_increment = -M_PI * 2 / N; + } + } + + if (counterclockwise != 0) + { + double d = msg.angle_min; + msg.angle_min = msg.angle_max; + msg.angle_max = d; + msg.angle_increment = -msg.angle_increment; + msg.angle_min -= msg.angle_increment; + msg.angle_max -= msg.angle_increment; + } + msg.intensities.resize(N); + msg.ranges.resize(N); + int idx = 0; + + if (argdata.reversed) + { + for (int i = N - 1; i >= 0; i--) + { + double deg = ROSAng(hub->consume[i].degree); + double d = hub->consume[i].distance / 1000.0; + bool custom = false; + for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) + { + if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) + custom = true; + } + + if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) + msg.ranges[idx] = std::numeric_limits::infinity(); + else + msg.ranges[idx] = d; + + msg.intensities[idx] = hub->consume[i].confidence; + idx++; + } + } + else + { + + // ROS_INFO("%s %d %f %f\n", __FUNCTION__, __LINE__, hub->consume[0].degree,hub->consume[hub->consume.size()-1].degree); + for (int i = 0; i <= N - 1; i++) + { + // printf(" %d %d %d %lf \n", datapoint[i].distance,datapoint[i].confidence,N,datapoint[i].degree); + double deg = ROSAng(hub->consume[i].degree); + double d = hub->consume[i].distance / 1000.0; + bool custom = false; + for (unsigned int k = 0; k < argdata.masks.size() && !custom; k++) + { + if (argdata.with_angle_filter && argdata.masks[k].min <= deg && deg <= argdata.masks[k].max) + custom = true; + } + + if (hub->consume[i].distance == 0 || d > argdata.max_dist || d < argdata.min_dist || custom) + msg.ranges[idx] = std::numeric_limits::infinity(); + else + msg.ranges[idx] = d; + + msg.intensities[idx] = hub->consume[i].confidence; + + idx++; + } + } + hub->consume.clear(); + laser_pub->publish(msg); +} + +#if 0 +void setup_params(bluesea2::DynParamsConfig &config, uint32_t level) +{ + ROS_INFO("Change RPM to [%d]", config.rpm); + + char cmd[32]; + sprintf(cmd, "LSRPM:%dH", config.rpm); + + SendCmd(strlen(cmd), cmd); +} +#endif + +bool should_start = true; +// service call back function +bool stop_motor(const std::shared_ptr, std::shared_ptr) +{ + + should_start = false; + // ROS_INFO("Stop motor"); + char cmd[] = "LSTOPH"; + return SendCmd(6, cmd); +} + +// service call back function +bool start_motor(const std::shared_ptr, std::shared_ptr) +{ + should_start = true; + char cmd[] = "LSTARH"; + + // ROS_INFO("Stop motor"); + + return SendCmd(6, cmd); +} +void split(const std::string &s, char delim, int *elems) +{ + int idx = 0; + std::stringstream ss(s); + std::string number; + + while (std::getline(ss, number, delim)) + { + elems[idx++] = atoi(number.c_str()); + elems[idx] = 0; + } +} + +#define READ_PARAM(TYPE, NAME, VAR, INIT) \ + VAR=INIT; \ + node->declare_parameter(NAME, VAR); \ + node->get_parameter(NAME, VAR); + +bool get_range_param(std::shared_ptr node, const char *name, Range &range) +{ + std::string mask; + READ_PARAM(std::string, name, mask, ""); + if (mask != "") + { + int index = mask.find(","); + float min = atof(mask.substr(0, index).c_str()); + float max = atof(mask.substr(index + 1).c_str()); + if (min < max) + { + range.min = min * 180 / M_PI; + range.max = max * 180 / M_PI; + return true; + } + } + return false; +} + +void getCMDList(CommandList &cmdlist, ArgData argdata) +{ + + if (argdata.uuid >= 0) + sprintf(cmdlist.uuid, "LUUIDH"); + if (argdata.model >= 0) + sprintf(cmdlist.model, "LTYPEH"); + if (argdata.rpm >= 0) + sprintf(cmdlist.rpm, "LSRPM:%dH", argdata.rpm); + if (argdata.resample >= 0) + { + + if (argdata.resample > 0 && argdata.resample < 1) + sprintf(cmdlist.res, "LSRES:%dH", (int)(argdata.resample * 1000)); + else + sprintf(cmdlist.res, "LSRES:%dH", (int)argdata.resample); + } + if (argdata.with_smooth >= 0) + { + if (argdata.type == "uart") + sprintf(cmdlist.smooth, "LSSS%dH", argdata.with_smooth); + else + sprintf(cmdlist.smooth, "LSSMT:%dH", argdata.with_smooth); + } + if (argdata.with_deshadow >= 0) + { + if (argdata.type == "uart") + sprintf(cmdlist.fitter, "LFFF%dH", argdata.with_deshadow); + else + sprintf(cmdlist.fitter, "LSDSW:%dH", argdata.with_deshadow); + } + if (argdata.alarm_msg >= 0) + sprintf(cmdlist.alarm, "LSPST:%dH", argdata.alarm_msg ? 3 : 1); + if (argdata.direction >= 0) + sprintf(cmdlist.direction, "LSCCW:%dH", argdata.direction); + if (argdata.unit_is_mm >= 0) + sprintf(cmdlist.unit_mm, "%s", argdata.unit_is_mm ? "LMDMMH" : "LMDCMH"); + if (argdata.with_confidence >= 0) + sprintf(cmdlist.confidence, "%s", argdata.with_confidence ? "LOCONH" : "LNCONH"); + + if (argdata.ats >= 0) + { + if (argdata.type == "vpc" || argdata.type == "uart") + sprintf(cmdlist.ats, "LSATS:002H"); + else if (argdata.type == "udp") + sprintf(cmdlist.ats, "LSATS:001H"); + } +} + +bool autoGetFirstAngle2(HPublish pub, bool from_zero) +{ + PubHub *hub = (PubHub *)pub; + int checkAngle = (from_zero == true ? 0 : 180); + for (int i = 0; i < hub->nfan; i++) + { + int idx = getFirstidx(*(hub->fans[i]), checkAngle); + if (idx >= 0) + { + //ROS_INFO("%s %d %d %d\n", __FUNCTION__, __LINE__, hub->fans[i]->angle, checkAngle); + hub->offsetangle = hub->fans[i]->angle; + hub->offsetidx = idx; + return true; + } + } + return false; +} + +bool ProfileInit(std::shared_ptrnode, ArgData &argdata) +{ + READ_PARAM(int, "number", argdata.num, 1); + READ_PARAM(std::string, "type", argdata.type, "uart"); + READ_PARAM(std::string, "frame_id", argdata.frame_id, "LH_laser"); + READ_PARAM(int, "dev_id", argdata.dev_id, ANYONE); + + printf("argdata.num %d\n",argdata.num); + // dual lidar arg + for (int i = 0; i node = rclcpp::Node::make_shared("bluesea_node"); + + rclcpp::QoS qos(0); + qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + // std::shared_ptrptr = rclcpp::Node::make_shared("bluesea_node"); + signal(SIGINT, closeSignal); + ArgData argdata; + ProfileInit(node, argdata); + + CommandList cmdlist; + memset(&cmdlist, 0, sizeof(CommandList)); + getCMDList(cmdlist, argdata); + printf("ROS2 Verision:%s\n", ROS2Verision); + + + rclcpp::Publisher::SharedPtr laser_pubs[MAX_LIDARS]; + rclcpp::Service::SharedPtr service = node->create_service("start", &start_motor); + rclcpp::Service::SharedPtr service2 = node->create_service("stop", &stop_motor); + rclcpp::WallRate loop_rate(100); + + HParser parsers[MAX_LIDARS]; + PubHub *hubs[MAX_LIDARS] = {NULL}; + for (int i = 0; i < argdata.num; i++) + { + parsers[i] = ParserOpen(argdata.raw_bytes, true, argdata.dev_id, argdata.custom.error_circle, argdata.custom.error_scale, argdata.from_zero, cmdlist, (char *)argdata.connectargs[i].arg1.c_str(), argdata.connectargs[i].arg2); + hubs[i] = new PubHub; + hubs[i]->nfan = 0; + hubs[i]->offsetangle = -1; + pthread_mutex_init(&hubs[i]->mtx, NULL); + if (argdata.output_scan) + laser_pubs[i] = node->create_publisher(argdata.connectargs[i].laser_topics, qos); + + } + + if (argdata.type == "uart" || argdata.type == "vpc") + { + g_reader = StartUartReader(argdata.type.c_str(), argdata.connectargs[0].arg1.c_str(), argdata.connectargs[0].arg2, parsers[0], hubs[0]); + } + else if (argdata.type == "udp") + { + LidarInfo lidars[MAX_LIDARS]; + for (int i = 0; i < argdata.num; i++) + { + lidars[i].parser = parsers[i]; + lidars[i].pub = hubs[i]; + strcpy(lidars[i].lidar_ip, argdata.connectargs[i].arg1.c_str()); + lidars[i].lidar_port = argdata.connectargs[i].arg2; + } + g_reader = StartUDPReader(argdata.type.c_str(), argdata.localport, argdata.custom.is_group_listener, argdata.custom.group_ip.c_str(), argdata.num, lidars); + } + else if (argdata.type == "tcp") + { + g_reader = StartTCPReader(argdata.connectargs[0].arg1.c_str(), argdata.connectargs[0].arg2, parsers[0], hubs[0]); + } + + + while (rclcpp::ok()) + { + rclcpp::spin_some(node); + bool idle = true; + for (int i = 0; i < argdata.num; i++) + { + if (hubs[i]->nfan == 0) + continue; + bool ret = false; + if (hubs[i]->offsetangle == -1) + { + ret = autoGetFirstAngle2(hubs[i], argdata.from_zero); + if (ret > 0) + { + ROS_INFO("lidar start work,offset angle %d offset idx %d\n", hubs[i]->offsetangle/10, hubs[i]->offsetidx); + } + continue; + } + if (!argdata.output_360) + { + RawData *fans[MAX_FANS] = {NULL}; + if (GetFan(hubs[i], argdata.soft_resample, argdata.resample, fans)) + { + if (argdata.output_scan) + { + PublishLaserScanFan(laser_pubs[i], fans[0], argdata.frame_id, + argdata.min_dist, argdata.max_dist, argdata.inverted, argdata.reversed); + } + delete fans[0]; + idle = false; + } + } + else + { + // 按照点数判定一帧的数据，然后 零数判定， 离异点过滤，最大最小值过滤(空点去除)， 指定角度屏蔽(mask置零) + uint8_t counterclockwise = false; + int n = GetAllFans(hubs[i], argdata, counterclockwise); + if (n > 0) + { + idle = false; + if (argdata.output_scan) + { + PublishLaserScan(laser_pubs[i], hubs[i], argdata, counterclockwise); + } + + if (argdata.output_cloud) + { + //PublishCloud(cloud_pubs[i], hubs[i], argdata); + } + } + } + } + if (idle) + { + loop_rate.sleep(); + } + } + return 0; +} diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/data_process.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/data_process.cpp new file mode 100644 index 0000000000000000000000000000000000000000..129942cbafbe3ee8e28a961697911a49e8d127bd --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/data_process.cpp @@ -0,0 +1,243 @@ + +#include "data_process.h" + +void get_data(const sensor_msgs::msg::LaserScan& scan, int idx, double& dist, double& energy) +{ + if (idx < 0) + idx += scan.ranges.size(); + else if (idx >= scan.ranges.size()) + idx -= scan.ranges.size(); + dist = scan.ranges[idx]; + energy = scan.intensities[idx]; +} + +bool is_high(const sensor_msgs::msg::LaserScan& scan, int idx) +{ + double dist, energy; + get_data(scan, idx, dist, energy); + + return dist < 1000000 && energy > 200; +} + +bool data_process(sensor_msgs::msg::LaserScan& scan) +{ + int idx = 0; + while (idx < scan.ranges.size()) + { + if (scan.ranges[idx] < 10000000) + { + idx++; + continue; + } + + bool bl1 = is_high(scan, idx-1); + bool bl2 = is_high(scan, idx-2); + bool br1 = is_high(scan, idx+1); + bool br2 = is_high(scan, idx+2); + bool br3 = is_high(scan, idx+3); + + if ( (bl1 && br1 && br2) || (bl1 && bl2 && br1) ) + { + double d1, d2, e1, e2; + get_data(scan, idx-1, d1, e1); + get_data(scan, idx+1, d2, e2); + scan.ranges[idx] = (d1+d2)/2; + scan.intensities[idx] = (e1+e2)/2; + idx+=2; + } + else if (bl1 && bl2 && !br1 && br2 && br3) + { + double d1, d2, e1, e2; + get_data(scan, idx-1, d1, e1); + get_data(scan, idx+2, d2, e2); + scan.ranges[idx] = (d1+d2)/2; + scan.intensities[idx] = (e1+e2)/2; + scan.ranges[idx+1] = (d1+d2)/2; + scan.intensities[idx+1] = (e1+e2)/2; + idx+=2; + } + else { + idx++; + } + + } + return true; +} + +int find(std::vectora, int n, int x) +{ + int i; + int min = abs(a.at(0).angle - x); + int r = 0; + + for (i = 0; i < n; ++i) + { + if (abs(a.at(i).angle - x) < min) + { + min = abs(a.at(i).angle - x); + r = i; + } + } + + return a[r].angle; +} +int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result) +{ + int angles = 0; + int size = raws.size(); + //printf("angle %d size:%d\n", raw.angle,size); + if(size>=1) + { + RawData tmp = raws.at(size-1); + RawData tmp2 = raws.at(0); + if(raw.angle==tmp2.angle) + { + for (int i=0;i& whole_datas,PDataPoints *data,int &size,char *result) +{ + whole_datas.push_back(raw); + if (raw.angle + raw.span == collect_angle*10) + { + + } + else + { + return false; + } + int count = 0, n = 0, angles = 0; + for (std::vector::iterator it = whole_datas.begin(); it != whole_datas.end(); ++it) + { + RawData tmp = *it; + angles += tmp.span; + count += tmp.N; + n++; + } + if (angles != 3600) + { + sprintf(result,"angle sum:%d drop fans:%d points:%d",angles,n,count); + whole_datas.clear(); + return false; + } + size = sizeof(DataPoints)+sizeof(DataPoint)*count; + *data=(PDataPoints)malloc(size); + (*data)->N =count; + (*data)->ts[0]=whole_datas.at(0).ts[0]; + (*data)->ts[1]=whole_datas.at(0).ts[1]; + + int index=0;bool first=true; + if(from_zero) + { + if(collect_angle>180) + { + index = (360-collect_angle)/((float)whole_datas.at(0).span/whole_datas.at(0).N); + first=true; + } + else if(collect_angle<180) + { + index = (collect_angle-0)/((float)whole_datas.at(whole_datas.size()-1).span/whole_datas.at(whole_datas.size()-1).N); + first=false; + } + } + else + { + if(collect_angle>180) + { + index = (collect_angle-180)/((float)whole_datas.at(whole_datas.size()-1).span/whole_datas.at(whole_datas.size()-1).N); + first=false; + } + else if(collect_angle<180) + { + index = (180-collect_angle)/((float)whole_datas.at(0).span/whole_datas.at(0).N); + first=true; + } + } + + int pointindex=0; + if(first) + { + for(int i=index;ipoints[pointindex]=whole_datas.at(0).points[i]; + pointindex++; + } + for(unsigned int i=1;ipoints[pointindex]=whole_datas.at(i).points[j]; + pointindex++; + } + } + for(int i=0;ipoints[pointindex]=whole_datas.at(0).points[i]; + pointindex++; + } + + } + else + { + for(int i=index;ipoints[pointindex]=whole_datas.at(whole_datas.size()-1).points[i]; + pointindex++; + } + for(unsigned int i=0;ipoints[pointindex]=whole_datas.at(i).points[j]; + pointindex++; + } + } + for(int i=0;ipoints[pointindex]=whole_datas.at(whole_datas.size()-1).points[i]; + pointindex++; + } + } +// for(int i=0;ipoints[i].angle<<(int)(*data)->points[i].confidence<<(*data)->points[i].distance< +#include +#include +#include +#include +#include +#include +#include "parser.h" +#include "alarm.h" +char g_uuid[32] = ""; +char g_model[16] = ""; +#if 0 +short LidarAng2ROS(short ang) +{ + //ang = -ang; + //return ang < -1800 ? ang + 3600 : ang; + return ang >= 1800 ? ang - 3600 : ang; +} +#endif +// CRC32 +unsigned int stm32crc(unsigned int *ptr, unsigned int len) +{ + unsigned int xbit, data; + unsigned int crc32 = 0xFFFFFFFF; + const unsigned int polynomial = 0x04c11db7; + + for (unsigned int i = 0; i < len; i++) + { + xbit = 1 << 31; + data = ptr[i]; + for (unsigned int bits = 0; bits < 32; bits++) + { + if (crc32 & 0x80000000) + { + crc32 <<= 1; + crc32 ^= polynomial; + } + else + crc32 <<= 1; + + if (data & xbit) + crc32 ^= polynomial; + + xbit >>= 1; + } + } + return crc32; +} + +static uint32_t update_flags(unsigned char *buf) +{ + uint32_t v; + memcpy(&v, buf, 4); + return v; +} + +static unsigned char is_data(unsigned char *buf) +{ + if (buf[1] != 0xFA) + return 0; + + if (buf[0] == 0xCE || buf[0] == 0xCF || buf[0] == 0xDF || buf[0] == 0xC7|| buf[0] == 0xAA) + return buf[0]; + + return 0; +} + +static RawData *GetData0xCE_2(const RawDataHdr &hdr, unsigned char *buf, uint32_t flags, bool with_chk) +{ + RawData *dat = new RawData; + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + memcpy(dat, &hdr, HDR_SIZE); + dat->span = 360; + + int is_mm = flags & DF_UNIT_IS_MM; + int with_conf = flags & DF_WITH_INTENSITY; + + // calc checksum + unsigned short sum = hdr.angle + hdr.N, chk; + unsigned char *pdat = buf + HDR_SIZE; + for (int i = 0; i < hdr.N; i++) + { + unsigned short v = *pdat++; + unsigned short v2 = *pdat++; + unsigned short val = (v2 << 8) | v; + + if (with_conf) + { + dat->points[i].confidence = val >> 13; + dat->points[i].distance = val & 0x1fff; + if (is_mm == 0) + dat->points[i].distance *= 10; + } + else + { + dat->points[i].distance = is_mm ? val : val * 10; + dat->points[i].confidence = 0; + } + + // dat->points[i].angle = hdr.angle*10 + 3600 * i / hdr.N; + dat->points[i].degree = hdr.angle / 10.0 + (dat->span * i) / (10.0 * hdr.N); + + sum += val; + } + + memcpy(&chk, buf + HDR_SIZE + hdr.N * 2, 2); + + if (with_chk && chk != sum) + { + delete dat; + printf("chksum error"); + return NULL; + } + + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + + SetTimeStamp(dat); + + return dat; +} + +static RawData *GetData0xCE_3(const RawDataHdr &hdr, unsigned char *buf, uint32_t flags, bool with_chk) +{ + RawData *dat = new RawData; + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + memcpy(dat, &hdr, HDR_SIZE); + int span = (flags & DF_FAN_90) ? 90 : 360; + + if (hdr.angle == 3420 && (flags & DF_FAN_90)) + span = 180; + dat->span = span; + + unsigned char *pdat = buf + HDR_SIZE; + // calc checksum + unsigned short sum = hdr.angle + hdr.N, chk; + for (int i = 0; i < hdr.N; i++) + { + dat->points[i].confidence = *pdat++; + sum += dat->points[i].confidence; + + unsigned short v = *pdat++; + unsigned short v2 = *pdat++; + unsigned short vv = (v2 << 8) | v; + sum += vv; + dat->points[i].distance = vv; + // dat->points[i].angle = hdr.angle*10 + span * i * 10 / hdr.N; + dat->points[i].degree = hdr.angle / 10.0 + (span * i) / (10.0 * hdr.N); + } + + memcpy(&chk, pdat, 2); + + if (with_chk && chk != sum) + { + delete dat; + printf("chksum ce error"); + return NULL; + } + + // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); + // printf("get3 %d(%d)\n", hdr.angle, hdr.N); + + SetTimeStamp(dat); + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + return dat; +} + +static FanSegment_C7 *GetFanSegment(const RawDataHdr7 &hdr, uint8_t *pdat, bool /*with_chk*/) +{ + FanSegment_C7 *fan_seg = new FanSegment_C7; + if (!fan_seg) + { + printf("out of memory\n"); + return NULL; + } + fan_seg->hdr = hdr; + fan_seg->next = NULL; + + uint16_t sum = 0; + // if (with_chk) + { + uint16_t *pchk = (uint16_t *)pdat; + for (int i = 1; i < HDR7_SIZE / 2; i++) + sum += pchk[i]; + } + + uint8_t *pDist = pdat + HDR7_SIZE; + uint8_t *pAngle = pdat + HDR7_SIZE + 2 * hdr.N; + uint8_t *energy = pdat + HDR7_SIZE + 4 * hdr.N; + + for (int i = 0; i < hdr.N; i++, pDist += 2, pAngle += 2) + { + fan_seg->dist[i] = ((uint16_t)(pDist[1]) << 8) | pDist[0]; + fan_seg->angle[i] = ((uint16_t)(pAngle[1]) << 8) | pAngle[0]; + fan_seg->energy[i] = energy[i]; + + sum += fan_seg->dist[i]; + sum += fan_seg->angle[i]; + sum += energy[i]; + } + + uint8_t *pchk = pdat + HDR7_SIZE + 5 * hdr.N; + uint16_t chksum = ((uint16_t)(pchk[1]) << 8) | pchk[0]; + if (chksum != sum) + { + printf("checksum error\n"); + delete fan_seg; + return NULL; + } + return fan_seg; +} +static FanSegment_AA *GetFanSegment(const RawDataHdrAA &hdr, uint8_t *pdat, bool /*with_chk*/) +{ + FanSegment_AA *fan_seg = new FanSegment_AA; + if (!fan_seg) + { + printf("out of memory\n"); + return NULL; + } + fan_seg->hdr = hdr; + fan_seg->next = NULL; + + uint16_t sum = 0; + // if (with_chk) + { + uint16_t *pchk = (uint16_t *)pdat; + for (int i = 1; i < HDRAA_SIZE / 2; i++) + sum += pchk[i]; + } + + uint8_t *pDist = pdat + HDRAA_SIZE; + uint8_t *pAngle = pdat + HDRAA_SIZE + 2 * hdr.N; + uint8_t *energy = pdat + HDRAA_SIZE + 4 * hdr.N; + + for (int i = 0; i < hdr.N; i++, pDist += 2, pAngle += 2) + { + fan_seg->dist[i] = ((uint16_t)(pDist[1]) << 8) | pDist[0]; + fan_seg->angle[i] = ((uint16_t)(pAngle[1]) << 8) | pAngle[0]; + fan_seg->energy[i] = energy[i]; + + sum += fan_seg->dist[i]; + sum += fan_seg->angle[i]; + sum += energy[i]; + } + + uint8_t *pchk = pdat + HDRAA_SIZE + 5 * hdr.N; + uint16_t chksum = ((uint16_t)(pchk[1]) << 8) | pchk[0]; + if (chksum != sum) + { + printf("checksum error\n"); + delete fan_seg; + return NULL; + } + return fan_seg; +} +void DecTimestamp(uint32_t ts, uint32_t *ts2) +{ + timeval tv; + gettimeofday(&tv, NULL); + uint32_t sec = tv.tv_sec % 3600; + if (sec < 5 && ts / 1000 > 3595) + { + ts2[0] = (tv.tv_sec / 3600 - 1) * 3600 + ts / 1000; + } + else + { + ts2[0] = (tv.tv_sec / 3600) * 3600 + ts / 1000; + } + + ts2[1] = (ts % 1000) * 1000000; +} + +static RawData *PackFanData(FanSegment_C7 *seg) +{ + RawData *dat = new RawData; + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + dat->code = 0xfac7; + dat->N = seg->hdr.whole_fan; + dat->angle = seg->hdr.beg_ang / 100; // 0.1 degree + dat->span = (seg->hdr.end_ang - seg->hdr.beg_ang) / 100; // 0.1 degree + dat->fbase = 0; + dat->first = 0; + dat->last = 0; + dat->fend = 0; + dat->counterclockwise = 0; + + DecTimestamp(seg->hdr.timestamp, dat->ts); + int count = 0; + while (seg) + { + for (int i = 0; i < seg->hdr.N; i++, count++) + { + dat->points[count].confidence = seg->energy[i]; + dat->points[count].distance = seg->dist[i]; + dat->points[count].degree = (seg->angle[i] + seg->hdr.beg_ang) / 1000.0; + } + + seg = seg->next; + } + + return dat; +} +static RawData *PackFanData(FanSegment_AA *seg) +{ + RawData *dat = new RawData; + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + dat->code = 0xfaaa; + dat->N = seg->hdr.whole_fan; + dat->angle = seg->hdr.beg_ang / 100; // 0.1 degree + dat->span = (seg->hdr.end_ang - seg->hdr.beg_ang) / 100; // 0.1 degree + dat->fbase = 0; + dat->first = 0; + dat->last = 0; + dat->fend = 0; + dat->counterclockwise = 0; + + dat->ts[0]=seg->hdr.second; + dat->ts[1]=seg->hdr.nano_sec/1000; + int count = 0; + while (seg) + { + for (int i = 0; i < seg->hdr.N; i++, count++) + { + dat->points[count].confidence = seg->energy[i]; + dat->points[count].distance = seg->dist[i]; + dat->points[count].degree = (seg->angle[i] + seg->hdr.beg_ang) / 1000.0; + } + + seg = seg->next; + } + + return dat; +} +static int GetFanPointCount(FanSegment_C7 *seg) +{ + int n = 0; + + while (seg) + { + n += seg->hdr.N; + seg = seg->next; + } + + return n; +} +static int GetFanPointCount(FanSegment_AA *seg) +{ + int n = 0; + + while (seg) + { + n += seg->hdr.N; + seg = seg->next; + } + + return n; +} +static RawData *GetData0xC7(Parser *parser, const RawDataHdr7 &hdr, uint8_t *pdat) +{ + if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) + { + static time_t last = 0; + time_t t = time(NULL); + if (t > last) + { + printf("device id [%d] != my id [%d]\n", hdr.dev_id, parser->dev_id); + last = t; + } + // not my data + return NULL; + } + + FanSegment_C7 *fan_seg = GetFanSegment(hdr, pdat, parser->with_chk); + if (!fan_seg) + { + return NULL; + } + + // printf("fan %d %d\n", fan_seg->hdr.beg_ang, fan_seg->hdr.ofset); + + if (parser->fan_segs_c7 != NULL) + { + FanSegment_C7 *seg =parser->fan_segs_c7; + + if (seg->hdr.timestamp != fan_seg->hdr.timestamp) + { + printf("drop old fan segments\n"); + while (seg) + { + parser->fan_segs_c7 = seg->next; + delete seg; + seg =parser->fan_segs_c7; + } + parser->fan_segs_c7 =fan_seg; + } + else + { + while (seg) + { + if (seg->hdr.ofset == fan_seg->hdr.ofset) + { + printf("drop duplicated segment\n"); + delete fan_seg; + fan_seg = NULL; + break; + } + if (seg->next == NULL) + { + seg->next = fan_seg; + break; + } + seg = seg->next; + } + } + } + + if (parser->fan_segs_c7 == NULL && fan_seg != NULL) + { + parser->fan_segs_c7 = fan_seg; + } + + // if (parser->fan_segs == NULL) { return NULL; } + + unsigned int N = GetFanPointCount(parser->fan_segs_c7); + + if (N >= parser->fan_segs_c7->hdr.whole_fan) + { + RawData *dat = NULL; + + if (N == parser->fan_segs_c7->hdr.whole_fan) + { + if (N > sizeof(dat->points) / sizeof(dat->points[0])) + { + printf("too many %d points in 1 fan\n", N); + } + else + { + dat = PackFanData(parser->fan_segs_c7); + } + } + + // remove segments + FanSegment_C7 *seg = parser->fan_segs_c7; + while (seg) + { + parser->fan_segs_c7 = seg->next; + delete seg; + seg = parser->fan_segs_c7; + } + + if (dat) + { + // SetTimeStamp(dat, ); + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + } + + return dat; + } + + return NULL; +} + +static RawData *GetData0xAA(Parser *parser, const RawDataHdrAA &hdr, uint8_t *pdat) +{ + if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) + { + static time_t last = 0; + time_t t = time(NULL); + if (t > last) + { + printf("device id [%d] != my id [%d]\n", hdr.dev_id, parser->dev_id); + last = t; + } + // not my data + return NULL; + } + FanSegment_AA *fan_seg = GetFanSegment(hdr, pdat, parser->with_chk); + if (!fan_seg) + { + return NULL; + } + + // printf("fan %d %d\n", fan_seg->hdr.beg_ang, fan_seg->hdr.ofset); + + if (parser->fan_segs_aa != NULL) + { + FanSegment_AA *seg = parser->fan_segs_aa; + + if ((seg->hdr.second != fan_seg->hdr.second)||(seg->hdr.nano_sec != fan_seg->hdr.nano_sec)) + { + printf("drop old fan segments\n"); + while (seg) + { + parser->fan_segs_aa = seg->next; + delete seg; + seg = parser->fan_segs_aa; + } + parser->fan_segs_aa = fan_seg; + } + else + { + while (seg) + { + if (seg->hdr.ofset == fan_seg->hdr.ofset) + { + printf("drop duplicated segment\n"); + delete fan_seg; + fan_seg = NULL; + break; + } + if (seg->next == NULL) + { + seg->next = fan_seg; + break; + } + seg = seg->next; + } + } + } + + if (parser->fan_segs_aa == NULL && fan_seg != NULL) + { + parser->fan_segs_aa = fan_seg; + } + + // if (parser->fan_segs == NULL) { return NULL; } + + unsigned int N = GetFanPointCount(parser->fan_segs_aa); + + if (N >= parser->fan_segs_aa->hdr.whole_fan) + { + RawData *dat = NULL; + + if (N == parser->fan_segs_aa->hdr.whole_fan) + { + if (N > sizeof(dat->points) / sizeof(dat->points[0])) + { + printf("too many %d points in 1 fan\n", N); + } + else + { + dat = PackFanData(parser->fan_segs_aa); + } + } + + // remove segments + FanSegment_AA *seg = parser->fan_segs_aa; + while (seg) + { + parser->fan_segs_aa = seg->next; + delete seg; + seg = parser->fan_segs_aa; + } + + if (dat) + { + // SetTimeStamp(dat, ); + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + } + + return dat; + } + + return NULL; +} + +static RawData *GetData0x99(const RawDataHdr99 &hdr, unsigned char *pdat, bool) +{ + RawData *dat = new RawData; + + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + + memset(dat, 0, sizeof(RawData)); + dat->code = hdr.code; + dat->N = hdr.N; + dat->angle = hdr.from * 3600 / hdr.total; // 0.1 degree + dat->span = hdr.N * 3600 / hdr.total; // 0.1 degree + // dat->fbase = ; + // dat->first; + // dat->last; + // dat->fend; + DecTimestamp(hdr.timestamp, dat->ts); + + dat->counterclockwise = (hdr.flags & DF_MOTOR_REVERSE) ? 1 : 0; + + pdat += HDR99_SIZE; + + uint8_t *dist = pdat; + uint8_t *energy = pdat + 2 * hdr.N; + + for (int i = 0; i < hdr.N; i++) + { + uint16_t lo = *dist++; + uint16_t hi = *dist++; + dat->points[i].distance = (hi << 8) | lo; + dat->points[i].degree = (i + hdr.from) * 360.0 / hdr.total; + dat->points[i].confidence = energy[i]; + } + + return dat; +} + +static RawData *GetData0xCF(const RawDataHdr2 &hdr, unsigned char *pdat, bool with_chk) +{ + RawData *dat = new RawData; + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + memcpy(dat, &hdr, HDR2_SIZE); + + unsigned short sum = hdr.angle + hdr.N + hdr.span, chk; + + pdat += HDR2_SIZE; + + for (int i = 0; i < hdr.N; i++) + { + dat->points[i].confidence = *pdat++; + sum += dat->points[i].confidence; + + unsigned short v = *pdat++; + unsigned short v2 = *pdat++; + + unsigned short vv = (v2 << 8) | v; + + sum += vv; + dat->points[i].distance = vv; + // dat->points[i].angle = hdr.angle*10 + hdr.span * i *10 / hdr.N; + dat->points[i].degree = hdr.angle / 10.0 + (hdr.span * i) / (10.0 * hdr.N); + } + + memcpy(&chk, pdat, 2); + + if (with_chk && chk != sum) + { + delete dat; + printf("chksum cf error"); + return NULL; + } + + // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); + // printf("get CF %d(%d) %d\n", hdr.angle, hdr.N, hdr.span); + + SetTimeStamp(dat); + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + return dat; +} + +static RawData *GetData0xDF(const RawDataHdr3 &hdr, unsigned char *pdat, bool with_chk) +{ + RawData *dat = new RawData; + + if (!dat) + { + printf("out of memory\n"); + return NULL; + } + memset(dat, 0, sizeof(RawData)); + + memcpy(dat, &hdr, HDR3_SIZE); + + unsigned short sum = hdr.angle + hdr.N + hdr.span, chk; + + sum += hdr.fbase; + sum += hdr.first; + sum += hdr.last; + sum += hdr.fend; + + double dan = (hdr.last - hdr.first) / double(hdr.N - 1); + + pdat += HDR3_SIZE; + + for (int i = 0; i < hdr.N; i++) + { + dat->points[i].confidence = *pdat++; + sum += dat->points[i].confidence; + + unsigned short v = *pdat++; + unsigned short v2 = *pdat++; + unsigned short vv = (v2 << 8) | v; + sum += vv; + dat->points[i].distance = vv; + // dat->points[i].angle = hdr.first + dan * i; + dat->points[i].degree = (hdr.first + dan * i) / 100.0; + } + + memcpy(&chk, pdat, 2); + + if (with_chk && chk != sum) + { + delete dat; + printf("chksum df error"); + return NULL; + } + + // memcpy(dat.data, buf+idx+HDR_SIZE, 2*hdr.N); + // printf("get DF %d=%d %d %d\n", hdr.angle, hdr.first, hdr.N, hdr.span); + + SetTimeStamp(dat); + // dat->ros_angle = LidarAng2ROS(dat->angle + dat->span); + return dat; +} + +static int MsgProc(Parser *parser, int len, unsigned char *buf) +{ + if (len >= 8 && buf[0] == 'S' && buf[1] == 'T' && buf[6] == 'E' && buf[7] == 'D') + { + parser->flags = update_flags(buf + 2); + return 1; + } + else if (alarmProc(buf, len)) + { + return 2; + } + else + { + printf("unknown %d bytes : %02x ", len, buf[0]); + for (int i = 1; i < len; i++) + printf("%02x ", buf[i]); + printf("\n"); + } + + return -1; +} + +static int ParseStream(Parser *parser, int len, unsigned char *buf, int *nfan, RawData *fans[]) +{ + int max_fan = *nfan; + int idx = 0; + *nfan = 0; + int unk = 0; + unsigned char unknown[1024]; + while (idx < len - 128 && *nfan < max_fan) + { + unsigned char type = is_data(buf + idx); + if (type == 0) + { + unknown[unk++] = buf[idx++]; + if (unk == sizeof(unknown)) + { + printf("drop %d bytes : %02x %02x %02x %02x ...\n", unk, + unknown[0], unknown[1], + unknown[2], unknown[3]); + unk = 0; + } + continue; + } + + if (unk > 0) + { + int ret = MsgProc(parser, unk, unknown); + if (ret == 1) + { + int8_t flag = parser->flags >> 24; + // printf("%d %s\n", flag,g_model); + if (strcmp(g_model, "LDS-50C-R") == 0 || strcmp(g_model, "LDS-E200-R") == 0) + { + if (flag & 0x1) + printf("bottom plate low voltage\n"); + if (flag & 0x2) + printf("bottom plate high voltage\n"); + if (flag & 0x4) + printf("abnormal motor head temperature\n"); + if (flag & 0x8) + printf("motor head low voltage\n"); + if (flag & 0x10) + printf("motor head high voltage\n"); + } + } + else if( ret==-1) + { + parser->rest_len=0; + memset(parser->rest_buf,0,sizeof(parser->rest_buf)); + } + unk = 0; + } + + RawDataHdr hdr; + memcpy(&hdr, buf + idx, HDR_SIZE); + + if (hdr.N > MAX_POINTS) + { + printf("points number %d seem not correct\n", hdr.N); + idx += 2; + continue; + } + + if (type == 0xCE) + { + if (parser->raw_mode == 2) + { + if (idx + HDR_SIZE + hdr.N * 2 + 2 > len) + { + // need more bytes + break; + } + + RawData *fan = GetData0xCE_2(hdr, buf + idx, parser->flags, parser->with_chk); + if (fan) + { + fans[*nfan] = fan; + *nfan += 1; + } + + idx += HDR_SIZE + hdr.N * 2 + 2; + } + else + { + if (idx + HDR_SIZE + hdr.N * 3 + 2 > len) + { + // need more bytes + break; + } + RawData *fan = GetData0xCE_3(hdr, buf + idx, parser->flags, parser->with_chk); + if (fan) + { + fans[*nfan] = fan; + *nfan += 1; + } + idx += HDR_SIZE + hdr.N * 3 + 2; + } + } + else if (type == 0x99) + { + if (idx + hdr.N * 3 + HDR99_SIZE + 2 > len) + { + // need more bytes + break; + } + + RawDataHdr99 hdr99; + memcpy(&hdr99, buf + idx, HDR99_SIZE); + + RawData *fan = GetData0x99(hdr99, buf + idx, parser->with_chk); + if (fan) + { + fans[*nfan] = fan; + *nfan += 1; + } + idx += hdr.N * 3 + HDR99_SIZE + 2; + } + + else if (type == 0xCF) + { + if (idx + hdr.N * 3 + HDR2_SIZE + 2 > len) + { + // need more bytes + break; + } + RawDataHdr2 hdr2; + memcpy(&hdr2, buf + idx, HDR2_SIZE); + + RawData *fan = GetData0xCF(hdr2, buf + idx, parser->with_chk); + if (fan) + { + fans[*nfan] = fan; + *nfan += 1; + } + idx += hdr.N * 3 + HDR2_SIZE + 2; + } + else if (type == 0xC7) + { + if (idx + hdr.N * 5 + HDR7_SIZE + 2 > len) + { + // need more bytes + break; + } + RawDataHdr7 hdr7; + memcpy(&hdr7, buf + idx, HDR7_SIZE); + + RawData *fan = GetData0xC7(parser, hdr7, buf + idx); + if (fan) + { + // printf("set [%d] %d\n", *nfan, fan->angle); + fans[*nfan] = fan; + *nfan += 1; + } + idx += hdr.N * 5 + HDR7_SIZE + 2; + } + else if (type == 0xAA) + { + printf("%s %d\n",__FUNCTION__,__LINE__); + if (idx + hdr.N * 5 + HDRAA_SIZE + 2 > len) + { + // need more bytes + break; + } + RawDataHdrAA hdrAA; + memcpy(&hdrAA, buf + idx, HDRAA_SIZE); + + RawData *fan = GetData0xAA(parser, hdrAA, buf + idx); + if (fan) + { + printf("%s %d\n",__FUNCTION__,__LINE__); + fans[*nfan] = fan; + *nfan += 1; + } + idx += hdr.N * 5 + HDRAA_SIZE + 2; + } + else if (type == 0xDF) + { + if (idx + hdr.N * 3 + HDR3_SIZE + 2 > len) + { + // need more bytes + break; + } + RawDataHdr3 hdr3; + memcpy(&hdr3, buf + idx, HDR3_SIZE); + + RawData *fan = GetData0xDF(hdr3, buf + idx, parser->with_chk); + if (fan) + { + // printf("set [%d] %d\n", *nfan, fan->angle); + fans[*nfan] = fan; + *nfan += 1; + } + idx += hdr.N * 3 + HDR3_SIZE + 2; + } + else + { + // should not + } + } + return idx; +} + +HParser ParserOpen(int raw_bytes, bool with_chksum, int dev_id, + int error_circle, double error_scale, bool from_zero, + CommandList cmd, char *ip, int port) +{ + Parser *parser = new Parser; + + parser->rest_len = 0; + parser->raw_mode = raw_bytes; + parser->with_chk = with_chksum; + parser->fan_segs_c7 = NULL; + parser->fan_segs_aa = NULL; + parser->dev_id = dev_id; + parser->error_circle = error_circle; + parser->error_scale = error_scale; + parser->cmd = cmd; + parser->is_from_zero = from_zero; + strcpy(parser->ip, ip); + parser->port = port; + return parser; +} + +int ParserClose(HParser hP) +{ + Parser *parser = (Parser *)hP; + delete parser; + return 0; +} + +int ParserRunStream(HParser hP, int len, unsigned char *bytes, RawData *fans[]) +{ + Parser *parser = (Parser *)hP; + + int nfan = MAX_FANS; + + unsigned char *buf = new unsigned char[len + parser->rest_len]; + memset(buf, 0, sizeof(len + parser->rest_len)); + if (!buf) + { + printf("out of memory\n"); + return -1; + } + + if (parser->rest_len > 0) + { + memcpy(buf, parser->rest_buf, parser->rest_len); + } + memcpy(buf + parser->rest_len, bytes, len); + len += parser->rest_len; + + int used = ParseStream(parser, len, buf, &nfan, fans); + +#if 0 + for (int i=0; iros_angle = LidarAng2ROS(fans[i]->angle + fans[i]->span); + } +#endif + + parser->rest_len = len - used; + if (parser->rest_len > 0) + { + memcpy(parser->rest_buf, buf + used, parser->rest_len); + } + + delete[] buf; + + return nfan; +} +int alarmProc(unsigned char *buf, int len) +{ + if (memcmp(buf, "LMSG", 4) == 0) + { + if (len >= (int)sizeof(LidarAlarm)) + { + LidarAlarm *msg = (LidarAlarm *)buf; + if (msg->hdr.type >= 0x100) + { + if (getbit(msg->hdr.data, 12) == 1) + { + printf("ALARM LEVEL:OBSERVE MSG TYPE:%d ZONE ACTIVE:%x\n", msg->hdr.type, msg->zone_actived); + } + if (getbit(msg->hdr.data, 13) == 1) + { + printf("ALARM LEVEL:WARM MSG TYPE:%d ZONE ACTIVE:%x\n", msg->hdr.type, msg->zone_actived); + } + if (getbit(msg->hdr.data, 14) == 1) + { + printf("ALARM LEVEL:ALARM MSG TYPE:%d ZONE ACTIVE:%x\n", msg->hdr.type, msg->zone_actived); + } + if (getbit(msg->hdr.data, 15) == 1) + { + printf("ALARM COVER MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 16) == 1) + { + printf("ALARM NO DATA MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 17) == 1) + { + // printf("ALARM ZONE NO ACTIVE MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 18) == 1) + { + printf("ALARM SYSTEM ERROR MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 19) == 1) + { + printf("ALARM RUN EXCEPTION MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 20) == 1) + { + printf("ALARM NETWORK ERROR MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 21) == 1) + { + printf("ALARM UPDATING MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 22) == 1) + { + printf("ALARM ZERO POS ERROR MSG TYPE:%d\n", msg->hdr.type); + } + // 说明有LMSG_ERROR报错信息 + if (msg->hdr.type % 2 == 1) + { + + if (getbit(msg->hdr.data, 0) == 1) + { + printf("LIDAR LOW POWER MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 1) == 1) + { + printf("LIDAR MOTOR STALL MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 2) == 1) + { + printf("LIDAR RANGING MODULE TEMPERATURE HIGH MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 3) == 1) + { + printf("LIDAR NETWORK ERROR MSG TYPE:%d\n", msg->hdr.type); + } + if (getbit(msg->hdr.data, 4) == 1) + { + printf("LIDAR RANGER MODULE NO OUTPUT MSG TYPE:%d\n", msg->hdr.type); + } + } + } + } + + return 1; + } + return 0; +} +int ParserRun(LidarNode hP, int len, unsigned char *buf, RawData *fans[]) +{ + Parser *parser = (Parser *)hP.hParser; + + uint8_t type = buf[0]; + if (alarmProc(buf, len)) + return 0; + + if (buf[1] != 0xfa) + { + // printf("skip packet %x %x len\n", buf[0], buf[1]); + } + else if (buf[0] == 0x88) + { + PacketUart hdr; + memcpy(&hdr, buf, sizeof(PacketUart)); + if ((unsigned int)len >= hdr.len + sizeof(PacketUart)) + { + if (parser->dev_id != (u_int32_t)ANYONE && hdr.dev_id != parser->dev_id) + { + // not my data + return 0; + } + return ParserRunStream(hP.hParser, hdr.len, buf + sizeof(PacketUart), fans); + } + } + else if (type == 0xCE) + { + RawDataHdr hdr; + memcpy(&hdr, buf, HDR_SIZE); + + if (HDR_SIZE + hdr.N * 2 + 2 == len) + { + RawData *fan = GetData0xCE_2(hdr, buf, parser->flags, parser->with_chk); + if (fan) + { + fans[0] = fan; + return 1; + } + } + else if (HDR_SIZE + hdr.N * 3 + 2 == len) + { + RawData *fan = GetData0xCE_3(hdr, buf, parser->flags, parser->with_chk); + if (fan) + { + fans[0] = fan; + return 1; + } + } + else + printf("CE len %d N %d\n", len, hdr.N); + } + else if (type == 0xCF) + { + RawDataHdr2 hdr; + memcpy(&hdr, buf, HDR2_SIZE); + + if (hdr.N * 3 + HDR2_SIZE + 2 > len) + { + // need more bytes + printf("CF len %d N %d\n", len, hdr.N); + return 0; + } + + RawData *fan = GetData0xCF(hdr, buf, parser->with_chk); + if (fan) + { + // printf("CF %d + %d %d\n", fan->angle, fan->span, fan->N); + fans[0] = fan; + return 1; + } + } + else if (type == 0xDF) + { + RawDataHdr3 hdr; + memcpy(&hdr, buf, HDR3_SIZE); + + if (hdr.N * 3 + HDR3_SIZE + 2 > len) + { + // need more bytes + printf("DF len %d N %d\n", len, hdr.N); + return 0; + } + + RawData *fan = GetData0xDF(hdr, buf, parser->with_chk); + if (fan) + { + // printf("set [%d] %d\n", *nfan, fan->angle); + fans[0] = fan; + return 1; + } + } + else if (type == 0xC7) + { + RawDataHdr7 hdr; + memcpy(&hdr, buf, HDR7_SIZE); + + if (hdr.N * 5 + HDR7_SIZE + 2 > len) + { + // need more bytes + // printf("C7 len %d N %d\n", len, hdr.N); + return 0; + } + + RawData *fan = GetData0xC7(parser, hdr, buf); + if (fan) + { + // printf("set [%d] %d\n", *nfan, fan->angle); + fans[0] = fan; + return 1; + } + return 0; + } + else if (type == 0xAA) + { + + RawDataHdrAA hdr; + memcpy(&hdr, buf, HDRAA_SIZE); + + if (hdr.N * 5 + HDRAA_SIZE + 2 > len) + { + // need more bytes + // printf("C7 len %d N %d\n", len, hdr.N); + return 0; + } + + RawData *fan = GetData0xAA(parser, hdr, buf); + if (fan) + { + //printf("set [%d] %d\n",fan->N, fan->angle); + fans[0] = fan; + return 1; + } + return 0; + } + else if (type == 0x99) + { + RawDataHdr99 hdr; + memcpy(&hdr, buf, HDR99_SIZE); + + if (hdr.N * 3 + HDR99_SIZE + 2 > len) + { + // need more bytes + // printf("99 len %d N %d\n", len, hdr.N); + return 0; + } + + RawData *fan = GetData0x99(hdr, buf, parser->with_chk); + if (fan) + { + // printf("set [%d] %d\n", *nfan, fan->angle); + fans[0] = fan; + return 1; + } + return 0; + } + + if (buf[0] == 0x4c && buf[1] == 0x48) + { + // + return 0; + } + if (buf[0] == 0x4f && buf[1] == 0x4f && buf[2] == 0x42 && buf[3] == 0x53) + { + return 0; + } + printf("skip packet %08x len %d\n", *(uint32_t *)buf, len); + // printf("qwer:%02X %02X %02X %02X\n", buf[0],buf[1],buf[2],buf[3]); + return 0; +} + +int strip(const char *s, char *buf) +{ + int len = 0; + for (int i = 0; s[i] != 0; i++) + { + if (s[i] >= 'a' && s[i] <= 'z') + buf[len++] = s[i]; + else if (s[i] >= 'A' && s[i] <= 'Z') + buf[len++] = s[i]; + else if (s[i] >= '0' && s[i] <= '9') + buf[len++] = s[i]; + else if (s[i] == '-') + buf[len++] = s[i]; + else if (len > 0) + break; + } + buf[len] = 0; + return len; +} + + +void saveLog(const char *logPath, int type, const char *ip, const int port, const unsigned char *buf, unsigned int len) +{ + if (strlen(logPath) > 0) + { + FILE *fp = fopen(logPath, "aw"); + if (fp) + { + if (type == 0) + fprintf(fp, "%s %d SEND MSG:\t", ip, port); + if (type == 1) + fprintf(fp, "%s %d REV MSG:\t", ip, port); + + for (unsigned int i = 0; i < len; i++) + { + fprintf(fp, "%02x\t", buf[i]); + } + fprintf(fp, "\n"); + fclose(fp); + } + } +} + +int mkpathAll(std::string s, mode_t mode = 0755) +{ + size_t pre = 0, pos; + std::string dir; + int mdret; + + if (s[s.size() - 1] != '/') + { + // force trailing / so we can handle everything in loop + s += '/'; + } + + while ((pos = s.find_first_of('/', pre)) != std::string::npos) + { + dir = s.substr(0, pos++); + pre = pos; + if (dir.size() == 0) + continue; // if leading / first time is 0 length + if ((mdret = ::mkdir(dir.c_str(), mode)) && errno != EEXIST) + { + return mdret; + } + } + return mdret; +} +std::string stringfilter(char *str, int num) +{ + int index = 0; + for(int i=0;i=45&&str[i]<=58)||(str[i]>=65&&str[i]<=90)||(str[i]>=97&&str[i]<=122)||str[i]==32||str[i]=='_') + { + index++; + } + else + { + std::string arr = str; + arr=arr.substr(0,index); + return arr; + } + } + return ""; +} + +int find(std::vectora, int n, int x) +{ + int i; + int min = abs(a.at(0).angle - x); + int r = 0; + + for (i = 0; i < n; ++i) + { + if (abs(a.at(i).angle - x) < min) + { + min = abs(a.at(i).angle - x); + r = i; + } + } + + return a[r].angle; +} +int autoGetFirstAngle(RawData raw, bool from_zero, std::vector &raws,std::string &result) +{ + int angles = 0; + int size = raws.size(); + //printf("angle %d size:%d\n", raw.angle,size); + if(size>=1) + { + RawData tmp = raws.at(size-1); + RawData tmp2 = raws.at(0); + if(raw.angle==tmp2.angle) + { + for (int i=0;iangle*10)) + { + //下标减一 + idx=1.0*(angle*10-raw.angle)/raw.span*raw.N; + //printf("%s %d %d %d\n",__FUNCTION__,__LINE__,idx,raw.angle); + } + return idx; +} \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/tcp_reader.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/tcp_reader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8f76926e96fe0d0416e1087379651e309a7d8705 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/tcp_reader.cpp @@ -0,0 +1,183 @@ +#include "reader.h" + +struct TCPInfo +{ + HParser hParser; + HPublish hPublish; + int fd_tcp; + int lidar_port; + char lidar_ip[32]; + //int listen_port; + //char group_ip[32]; + pthread_t thr; +}; + +int tcp_reader(int sock, HParser hParser, HPublish hPublish) +{ + RawData* fans[MAX_FANS]; + + while (1) + { + fd_set fds; + FD_ZERO(&fds); + + FD_SET(sock, &fds); + + struct timeval to = { 5, 5 }; + int ret = select(sock+1, &fds, NULL, NULL, &to); + + if (ret == 0) { + + } + + if (ret < 0) { + printf("select error\n"); + continue; + } + + // read TCP data + if (FD_ISSET(sock, &fds)) + { + unsigned char buf[1024]; + int nr = recv(sock, buf, sizeof(buf), 0); + if (nr <= 0) { + printf("read tcp error %d\n", nr); + break; + } + else + { + int nfan = ParserRunStream(hParser, nr, buf, &(fans[0])); + if (nfan > 0) { + PublishData(hPublish, nfan, fans); + } + } + } + } + + return 0; +} + +void* TcpThreadProc(void* p) +{ + TCPInfo* info = (TCPInfo*)p; + + while (1) + { + // open TCP + int sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); + if (sock < 0) { + printf("socket TCP failed\n"); + return 0; + } + + struct sockaddr_in addr; + memset(&addr, 0, sizeof(addr)); /* Zero out structure */ + addr.sin_family = AF_INET; /* Internet address family */ + + addr.sin_addr.s_addr = inet_addr(info->lidar_ip); + addr.sin_port = htons(info->lidar_port); + + int ret = connect(sock, (struct sockaddr *) &addr, sizeof(addr)); + + if (ret != 0) + { + printf("connect (%s:%d) failed", info->lidar_ip, info->lidar_port); + close(sock); + //sleep(15); + continue; + } + info->fd_tcp = sock; + printf("connect (%s:%d) ok", info->lidar_ip, info->lidar_port); + + tcp_reader(sock, info->hParser, info->hPublish); + + printf("disconnect (%s:%d)\n", info->lidar_ip, info->lidar_port); + + info->fd_tcp = 0; + close(sock); + } + return NULL; +} + +#if 0 +void* TcpSrvProc(void* p) +{ + TCPInfo* info = (TCPInfo*)p; + + // open TCP port + int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + sockaddr_in addr; + addr.sin_family = AF_INET; + addr.sin_port = htons(info->listen_port); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + + int rt = ::bind(info->fd_tcp, (struct sockaddr *)&addr, sizeof(addr)); + + if (rt != 0) + { + printf("bind port %d failed\n", info->listen_port); + return NULL; + } + + printf("start tcp server %s:%d\n", lidar_ip, lidar_port); + + + while (1) { + + + } + + return NULL; +} + + +HReader StartTCPServer(unsigned short listen_port, HParser hParser, HPublish hPub) +{ + signal(SIGPIPE, SIG_IGN); + + TCPInfo* info = new TCPInfo; + info->hParser = hParser; + info->hPublish = hPub; + info->listen_port = listen_port; + + pthread_create(&info->thr, NULL, TcpSrvProc, info); + + return info; +} +#endif + +HReader StartTCPReader(const char* lidar_ip, unsigned short lidar_port, HParser hParser, HPublish hPub) +{ + signal(SIGPIPE, SIG_IGN); + + TCPInfo* info = new TCPInfo; + info->hParser = hParser; + info->hPublish = hPub; + info->lidar_port = lidar_port; + strcpy(info->lidar_ip, lidar_ip); + //strcpy(info->group_ip, group_ip); + //printf("start udp %s:%d udp %d\n", lidar_ip, lidar_port, info->fd_tcp); + + pthread_create(&info->thr, NULL, TcpThreadProc, info); + + return info; +} + +bool SendTcpCmd(HReader hr, int len, char* cmd) +{ + TCPInfo* info = (TCPInfo*)hr; + if (!info || info->fd_tcp <= 0) + return false; + + return send(info->fd_tcp, cmd, len, 0) == len; +} + + +void StopTCPReader(HReader hr) +{ + TCPInfo* info = (TCPInfo*)hr; + //info->should_exit = true; + //sleep(1); + pthread_join(info->thr, NULL); + delete info; +} diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart.c b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart.c new file mode 100644 index 0000000000000000000000000000000000000000..599744ed9cb66c4056b312e4d96e8ea57233c9e2 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart.c @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include +// #include + +int ioctl(int fd, unsigned long request, ...); + +int change_baud(int fd, int baud) +{ + struct termios2 t; + if (ioctl(fd, TCGETS2, &t)) + { + return -1; + } + t.c_cflag &= ~CBAUD; + t.c_cflag |= BOTHER; + t.c_ispeed = baud; + t.c_ospeed = baud; + + if (ioctl(fd, TCSETS2, &t)) + { + return -2; + } + +#if 1 + if (ioctl(fd, TCGETS2, &t) == 0) + { + if (abs(t.c_ospeed - baud) <= 20000) + { + printf("reported %d\n", t.c_ospeed); + } + else + { + printf("reported err set:%d result:%d\n", baud, t.c_ospeed); + return -3; + } + } +#endif + + return 0; +} diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart_reader.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart_reader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..44fd1de1bcab4625c02652fc2c0106d9b761d709 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/uart_reader.cpp @@ -0,0 +1,647 @@ +#include "reader.h" + + +extern "C" int change_baud(int fd, int baud); + +bool uart_talk(int fd, int n, const char *cmd, int nhdr, const char *hdr_str, int nfetch, char *fetch, int waittime,int CacheLength) +{ + //printf("send command : %s\n", cmd); + write(fd, cmd, n); + + char *buf=new char[CacheLength]; + int nr = read(fd, buf, sizeof(buf)); + int idx = waittime; + + while (nr < CacheLength) + { + int n = read(fd, buf + nr, CacheLength - nr); + // printf(" fd %d %d \n",n,nr); + if (n > 0) + { + nr += n; + idx = waittime; + } + else if (n == 0) + { + idx--; + usleep(1000); + if (idx == 0) + { + // printf("read 0 byte max index break\n"); + break; + } + } + } + // if(idx>0) + // printf("read max byte break\n"); + bool res=false; + for (unsigned int i = 0; i < CacheLength - nhdr - nfetch; i++) + { + if (memcmp(buf + i, hdr_str, nhdr) == 0) + { + if (nfetch > 0) + { + if (strcmp(cmd, "LXVERH") == 0 || strcmp(cmd, "LVERSH") == 0 || strcmp(cmd, "LUUIDH") == 0 || strcmp(cmd, "LTYPEH") == 0 || strcmp(cmd, "LQAZNH") == 0 || strcmp(cmd, "LQPSTH") == 0 || strcmp(cmd, "LQNPNH") == 0 || strcmp(cmd, "LQOUTH") == 0 || strcmp(cmd, "LQCIRH") == 0 || strcmp(cmd, "LQFIRH") == 0 || strcmp(cmd, "LQSRPMH") == 0 || strcmp(cmd, "LQSMTH") == 0 || strcmp(cmd, "LQDSWH") == 0 || strcmp(cmd, "LQZTPH") == 0) + { + int length =CacheLength - nhdr - i; + if(length>nfetch) + length=nfetch; + + memcpy(fetch, buf + i + nhdr, length); + fetch[length] = 0; + } + else if (strstr(cmd, "LSRPM") != NULL) + { + if (buf[i + nhdr + 1] == 'O' && buf[i + nhdr + 2] == 'K') + { + strncpy(fetch, "OK", 2); + fetch[2] = 0; + } + else if (buf[i + nhdr + 1] == 'e' && buf[i + nhdr + 2] == 'r') + { + strncpy(fetch, "NG", 2); + fetch[2] = 0; + } + } + else + { + strncpy(fetch, "OK", 2); + fetch[2] = 0; + } + } + delete []buf; + return true; + } + else if (memcmp(buf + i, cmd, n) == 0) + { + if (nfetch > 0) + { + memcpy(fetch, buf + i + n + 1, 2); + if (buf[i + n + 1] == 'E' && buf[i + n + 2] == 'R') + { + fetch[0] = 'N'; + fetch[1] = 'G'; + } + fetch[2] = 0; + } + delete []buf; + return true; + } + else if (memcmp(buf + i, "unsupport", 9) == 0) + { + //ROS_INFO("%s %d \n", __FUNCTION__, __LINE__); + if (nfetch > 0) + { + strcpy(fetch, "unsupport"); + fetch[10] = 0; + } + delete []buf; + return true; + } + } + delete []buf; + ROS_ERROR("read %d bytes, not found %s\n", nr, hdr_str); + return false; +} + +bool vpc_talk(int hcom, int mode, short sn, int len, const char *cmd, int nfetch, void *result) +{ + printf("USB send command : %s\n", cmd); + char buffer[2048]; + CmdHeader *hdr = (CmdHeader *)buffer; + hdr->sign = 0x484c; + hdr->cmd = mode; + hdr->sn = sn; + len = ((len + 3) >> 2) * 4; + + hdr->len = len; + + memcpy(buffer + sizeof(CmdHeader), cmd, len); + + unsigned int *pcrc = (unsigned int *)(buffer + sizeof(CmdHeader) + len); + pcrc[0] = stm32crc((unsigned int *)(buffer + 0), len / 4 + 2); + // pcrc[0] = BaseAPI::stm32crc((unsigned int*)(buffer + 0), len / 4 + 2); + + int len2 = len + sizeof(CmdHeader) + 4; + int nr = write(hcom, buffer, len2); + char buf[2048]; + int index = 10; + // 4C 48 BC FF xx xx xx xx result + // 读取之后的10*2048个长度，如果不存在即判定失败 + while (index--) + { + nr = read(hcom, buf, sizeof(buf)); + while (nr < (int)sizeof(buf)) + { + int n = read(hcom, buf + nr, sizeof(buf) - nr); + if (n > 0) + nr += n; + } + + for (int i = 0; i < (int)sizeof(buf) - nfetch; i++) + { + if (mode == C_PACK) + { + char *fetch = (char *)result; + if (buf[i] == 0x4C && buf[i + 1] == 0x48 && buf[i + 2] == (signed char)0xBC && buf[i + 3] == (signed char)0xFF) + { + /*int packSN = ((unsigned int)buf[i + 5] << 8) | (unsigned int)buf[i + 4]; + if (packSN != sn) + continue;*/ + + for (int j = 0; j < nfetch; j++) + { + if ((buf[i + j + 8] >= 33 && buf[i + j + 8] <= 127)) + { + fetch[j] = buf[i + j + 8]; + } + else + { + fetch[j] = ' '; + } + } + fetch[nfetch] = 0; + return true; + } + } + else if (mode == S_PACK) + { + if ((buf[i + 2] == (signed char)0xAC && buf[i + 3] == (signed char)0xB8) || (buf[i + 2] == (signed char)0xAC && buf[i + 3] == (signed char)0xff)) + { + // printf("%02x %02x\n", buf[i + 2], buf[i + 3]); + // 随机码判定 + short packSN = ((unsigned char)buf[i + 5] << 8) | (unsigned char)buf[i + 4]; + if (packSN != sn) + continue; + + memcpy(result, buf + i + 8, nfetch); + return true; + } + } + } + } + printf("read %d bytes, not found %s\n", nr, cmd); + return false; +} + +int open_serial_port(const char *port, int baudrate) +{ + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) + { + ROS_ERROR("open %s error", port); + return -1; + } + + int ret; + struct termios attrs; + + tcflush(fd, TCIOFLUSH); + + /* get current attrs */ + ret = tcgetattr(fd, &attrs); + if (ret < 0) + { + ROS_ERROR("get attrs failed"); + return -1; + } + + /* set speed */ + int speed = B230400; + // if (baudrate == 115200) speed = B115200; + + ret = cfsetispeed(&attrs, speed); //[baudrate]); + ret |= cfsetospeed(&attrs, speed); //[baudrate]); + + /* enable recieve and set as local line */ + attrs.c_cflag |= (CLOCAL | CREAD); + + /* set data bits */ + attrs.c_cflag &= ~CSIZE; + attrs.c_cflag |= CS8; + + /* set parity */ + if (1) + { // parity == UART_POFF) { + attrs.c_cflag &= ~PARENB; // disable parity + attrs.c_iflag &= ~INPCK; + } + else + { + attrs.c_cflag |= (PARENB | PARODD); // enable parity + attrs.c_iflag |= INPCK; + // if(parity == UART_PEVEN) attrs.c_cflag &= ~PARODD; + } + + /* set stop bits */ + attrs.c_cflag &= ~CSTOPB; // 1 stop bit + // attrs.c_cflag |= CSTOPB; // 2 stop bits + + // Disable Hardware flowcontrol + attrs.c_cflag &= ~CRTSCTS; + + /* set to raw mode, disable echo, signals */ + attrs.c_lflag &= ~(ICANON | ECHO | ECHOE | IEXTEN | ISIG); + + /* set no output process, raw mode */ + attrs.c_oflag &= ~OPOST; + attrs.c_oflag &= ~(ONLCR | OCRNL); + + /* disable CR map */ + attrs.c_iflag &= ~(ICRNL | INLCR); + /* disable software flow control */ + attrs.c_iflag &= ~(IXON | IXOFF | IXANY); + + attrs.c_cc[VMIN] = 0; + attrs.c_cc[VTIME] = 0; + + /* flush driver buf */ + tcflush(fd, TCIFLUSH); + + /* update attrs now */ + if (tcsetattr(fd, TCSANOW, &attrs) < 0) + { + close(fd); + ROS_ERROR("tcsetattr err"); + return -1; + } + + if (change_baud(fd, baudrate)) + { + close(fd); + ROS_ERROR("fail to set baudrate %d", baudrate); + return -1; + } + + return fd; +} + +bool setup_lidar_uart(HParser hP, int handle) +{ + Parser *parser = (Parser *)hP; + unsigned int index = 5; + int cmdLength; + char buf[32]; + char result[3] = {0}; + result[2] = '\0'; + + for (unsigned int i = 0; i < index; i++) + { + if ((uart_talk(handle, 6, "LVERSH", 14, "MOTOR VERSION:", 16, buf))) + { + std::string sn = stringfilter(buf, 16); + ROS_INFO("version:%s\n", sn.c_str()); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.uuid); + if (cmdLength <= 0) + break; + if ((uart_talk(handle, 6, "LUUIDH", 11, "PRODUCT SN:", 20, buf))) + { + std::string sn = stringfilter(buf, 20); + ROS_INFO("uuid:%s\n", sn.c_str()); + break; + } + } + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.model); + if (cmdLength <= 0) + break; + if (uart_talk(handle, cmdLength, parser->cmd.model, 8, "TYPE ID:", 16, buf)) + { + std::string sn = stringfilter(buf, 16); + ROS_INFO("product model:%s\n", sn.c_str()); + break; + } + } + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.unit_mm); + if (cmdLength <= 0) + break; + if (uart_talk(handle, cmdLength, parser->cmd.unit_mm, 10, "SET LiDAR ", 12, buf)) + { + ROS_INFO("set LiDAR unit %s\n",buf); + break; + } + } + // enable/disable output intensity + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.confidence); + if (cmdLength <= 0) + break; + if (uart_talk(handle, cmdLength, parser->cmd.confidence, 6, "LiDAR ", 12, buf)) + { + ROS_INFO("set LiDAR confidence %s\n", buf); + break; + } + } + // enable/disable shadow filter + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.fitter); + if (cmdLength <= 0) + break; + if (uart_talk(handle, cmdLength, parser->cmd.fitter, 2, "ok", 2, buf,100)) + { + ROS_INFO("set LiDAR shadow filter ok\n"); + break; + } + } + // enable/disable smooth + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.smooth); + if (cmdLength <= 0) + break; + + if (uart_talk(handle, cmdLength, parser->cmd.smooth, 2, "ok", 2, buf,100)) + { + ROS_INFO("set LiDAR smooth OK\n"); + break; + } + } + // setup rpm + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.rpm); + if (cmdLength <= 0) + break; + + if (uart_talk(handle, cmdLength, parser->cmd.rpm, 8, "Set RPM:", 12, buf,100)) + { + ROS_INFO("%s %s\n", parser->cmd.rpm, buf); + break; + } + } + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.res); + if (cmdLength <= 0) + break; + if (uart_talk(handle, cmdLength, parser->cmd.res, 15, "set resolution ", 12, buf)) + { + ROS_INFO("%s %s\n", parser->cmd.res,buf); + break; + } + } + return true; +} + +bool setup_lidar_vpc(HParser hP, int handle) +{ + Parser *parser = (Parser *)hP; + unsigned int index = 5; + int cmdLength; + char buf[32]; + char result[3] = {0}; + result[2] = '\0'; + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.ats); + if (cmdLength <= 0) + break; + if (vpc_talk(handle, 0x0053, rand(),cmdLength, parser->cmd.ats, 2, result)) + { + ROS_INFO("%s %s\n", parser->cmd.ats,result); + break; + } + } + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.uuid); + if (cmdLength <= 0) + break; + if (vpc_talk(handle, 0x0043, rand(),cmdLength, parser->cmd.uuid, 20, buf)) + { + std::string sn = stringfilter(buf, 20); + ROS_INFO("uuid:%s\n", sn.c_str()); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.model); + if (cmdLength <= 0) + break; + if (vpc_talk(handle, 0x0043, rand(),cmdLength, parser->cmd.model, 16, buf)) + { + std::string sn = stringfilter(buf, 16); + ROS_INFO("product model:%s\n", sn.c_str()); + break; + } + } + + // enable/disable shadow filter + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.fitter); + if (cmdLength <= 0) + break; + + if (vpc_talk(handle, 0x0053, rand(),cmdLength, parser->cmd.fitter, 2, result)) + { + ROS_INFO("set LiDAR shadow filter %s %s\n", parser->cmd.fitter, result); + break; + } + } + // enable/disable smooth + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.smooth); + if (cmdLength <= 0) + break; + + if (vpc_talk(handle, 0x0053,rand(), cmdLength, parser->cmd.smooth, 2, result)) + { + ROS_INFO("set LiDAR smooth %s %s\n", parser->cmd.smooth, result); + break; + } + } + // setup rpm + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.rpm); + if (cmdLength <= 0) + break; + + if (vpc_talk(handle, 0x0053, rand(),cmdLength, parser->cmd.rpm, 2, result)) + { + ROS_INFO("%s %s\n", parser->cmd.rpm, result); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.res); + if (cmdLength <= 0) + break; + + if (vpc_talk(handle, 0x0053, rand(),cmdLength, parser->cmd.res, 2, result)) + { + ROS_INFO("%s %s\n", parser->cmd.res, result); + break; + } + } + + // enable/disable alaram message uploading + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.alarm); + if (cmdLength <= 0) + break; + if (vpc_talk(handle, 0x0053, rand(),cmdLength, parser->cmd.alarm, 2, result)) + { + ROS_INFO("set alarm_msg %s\n", result); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.direction); + if (cmdLength <= 0) + break; + if (vpc_talk(handle, 0x0053, rand(), cmdLength, parser->cmd.direction, 2, result)) + { + ROS_INFO("set direction %s\n", result); + break; + } + } + return true; +} +int UartReader(UartInfo *info) +{ + int fd_uart = info->fd_uart; + + RawData *fans[MAX_FANS]; + while (1) + { + fd_set fds; + FD_ZERO(&fds); + + FD_SET(fd_uart, &fds); + + struct timeval to = {1, 500000}; + + int ret = select(fd_uart + 1, &fds, NULL, NULL, &to); + if (ret < 0) + { + printf("select error\n"); + break; + } + + // read UART data + if (FD_ISSET(fd_uart, &fds)) + { + unsigned char buf[512]; + unsigned int nr = read(fd_uart, buf, sizeof(buf)); + if (nr <= 0) + { + printf("read port error %d\n", nr); + break; + } + // saveLog(info->logPath, 1, "UART", 0, (unsigned char *)buf, nr); + int nfan = ParserRunStream(info->hParser, nr, buf, &(fans[0])); + // for (int i=0; iangle, fans[i]->points[0].distance); + if (nfan > 0) + { + PublishData(info->hPublish, nfan, fans); + } + if (nr < sizeof(buf) - 10) + usleep(10000 - nr * 10); + } + } + return 0; +} + +void *UartThreadProc(void *p) +{ + UartInfo *info = (UartInfo *)p; + if (access(info->port, R_OK)) + { + printf("port %s not ready\n", info->port); + // sleep(10); + return NULL; + } + int fd = open_serial_port(info->port, info->baudrate); + if (fd > 0) + { + info->fd_uart = fd; + if (strcmp(info->type, "uart") == 0) + setup_lidar_uart(info->hParser, info->fd_uart); + if (strcmp(info->type, "vpc") == 0) + setup_lidar_vpc(info->hParser, info->fd_uart); + + UartReader(info); + } + + return NULL; +} + +void *StartUartReader(const char *type, const char *port, int baudrate, HParser hParser, HPublish hPublish) +{ + UartInfo *info = new UartInfo; + strcpy(info->type, type); + strcpy(info->port, port); + info->baudrate = baudrate; + info->hParser = hParser; + info->hPublish = hPublish; + pthread_create(&info->thr, NULL, UartThreadProc, info); + + return info; +} + +bool SendUartCmd(HReader hr, int len, char *cmd) +{ + UartInfo *info = (UartInfo *)hr; + if (info && info->fd_uart > 0) + write(info->fd_uart, cmd, len); + return true; +} +bool SendVpcCmd(HReader hr, int len, char *cmd) +{ + UartInfo *info = (UartInfo *)hr; + if (info && info->fd_uart > 0) + { + int fd = info->fd_uart; + + unsigned char buffer[2048]; + CmdHeader *hdr = (CmdHeader *)buffer; + hdr->sign = 0x484c; + hdr->cmd = 0x0043; + hdr->sn = rand(); + + len = ((len + 3) >> 2) * 4; + + hdr->len = len; + + memcpy(buffer + sizeof(CmdHeader), cmd, len); + + // int n = sizeof(CmdHeader); + unsigned int *pcrc = (unsigned int *)(buffer + sizeof(CmdHeader) + len); + pcrc[0] = stm32crc((unsigned int *)(buffer + 0), len / 4 + 2); + int len2 = len + sizeof(CmdHeader) + 4; + write(fd, buffer, len2); + } + return true; +} +void StopUartReader(HReader hr) +{ + UartInfo *info = (UartInfo *)hr; + // info->should_exit = true; + // sleep(1); + pthread_join(info->thr, NULL); + delete info; +} \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/udp_reader.cpp b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/udp_reader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7e437da0be0e70b2aef8d0d9bf85e710e0a9f6e9 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/bluesea2/src/udp_reader.cpp @@ -0,0 +1,544 @@ +#include "alarm.h" +#include "reader.h" + +void send_cmd_udp(int fd_udp, const char* dev_ip, int dev_port, int cmd, int sn, int len, const void* snd_buf) +{ + char buffer[2048]; + CmdHeader* hdr = (CmdHeader*)buffer; + hdr->sign = 0x484c; + hdr->cmd = cmd; + hdr->sn = sn; + + len = ((len + 3) >> 2) * 4; + + hdr->len = len; + + memcpy(buffer + sizeof(CmdHeader), snd_buf, len); + + unsigned int* pcrc = (unsigned int*)(buffer + sizeof(CmdHeader) + len); + pcrc[0] = stm32crc((unsigned int*)(buffer + 0), len / 4 + 2); + + sockaddr_in to; + to.sin_family = AF_INET; + to.sin_addr.s_addr = inet_addr(dev_ip); + to.sin_port = htons(dev_port); + + int len2 = len + sizeof(CmdHeader) + 4; + + sendto(fd_udp, buffer, len2, 0, (struct sockaddr*)&to, sizeof(struct sockaddr)); + +} + +bool udp_talk_S_PACK(int fd_udp, const char* ip, int port, int n, const char* cmd, void* result) +{ + ROS_INFO("%s",cmd); + unsigned short sn = rand(); + send_cmd_udp(fd_udp, ip, port, 0x0053, sn, n, cmd); + + int nr = 0; + for (int i = 0; i < 1000; i++) + { + fd_set fds; + FD_ZERO(&fds); + + FD_SET(fd_udp, &fds); + + struct timeval to = { 3, 0 }; + int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); + if (ret <= 0) + { + return false; + } + // read UDP data + if (FD_ISSET(fd_udp, &fds)) + { + nr++; + sockaddr_in addr; + socklen_t sz = sizeof(addr); + char buf[1024] = { 0 }; + int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr*)&addr, &sz); + if (nr > 0) + { + CmdHeader* hdr = (CmdHeader*)buf; + if (hdr->sign != 0x484c || hdr->sn != sn) + continue; + memcpy(result, buf + 8, 2); + return true; + } + } + } + + printf("read %d packets, not response\n", nr); + return false; +} + +bool udp_talk_C_PACK(int fd_udp, const char* lidar_ip, int lidar_port, + int n, const char* cmd, + int nhdr, const char* hdr_str, + int nfetch, char* fetch) +{ + printf("send command : \'%s\' \n", cmd); + + unsigned short sn = rand(); + send_cmd_udp(fd_udp, lidar_ip, lidar_port, 0x0043, sn, n, cmd); + + time_t t0 = time(NULL); + int ntry = 0; + while (time(NULL) < t0 + 3 && ntry < 1000) + { + fd_set fds; + FD_ZERO(&fds); + FD_SET(fd_udp, &fds); + + struct timeval to = { 1, 0 }; + int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); + + if (ret < 0) + { + printf("select error\n"); + return false; + } + if (ret == 0) + { + continue; + } + + // read UDP data + if (FD_ISSET(fd_udp, &fds)) + { + ntry++; + sockaddr_in addr; + socklen_t sz = sizeof(addr); + + char buf[1024] = { 0 }; + int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr*)&addr, &sz); + if (nr > 0) + { + CmdHeader* hdr = (CmdHeader*)buf; + if (hdr->sign != 0x484c || hdr->sn != sn) + continue; + + char* payload = buf + sizeof(CmdHeader); + for (int i = 0; i < nr - nhdr - 1; i++) + { + if (memcmp(payload + i, hdr_str, nhdr) == 0) + { + if (nfetch > 0) + { + memset(fetch, 0, nfetch); + for (int j = 0; j < nfetch && i + nhdr + j < nr; j++) + fetch[j] = payload[i + nhdr + j]; + } + return true; + } + } + } + } + } + printf("read %d packets, not response\n", ntry); + return false; +} + + + +void *UdpThreadProc(void *p) +{ + UDPInfo *info = (UDPInfo *)p; + int fd_udp = info->fd_udp; + int error_num = 0; + char buf[1024]; + + timeval tv; + gettimeofday(&tv, NULL); + + time_t tto = tv.tv_sec + 1; + uint32_t delay = 0; + if (!info->is_group_listener) + { + for (int i = 0; i < info->nnode; i++) + { + setup_lidar_udp(info->lidars[i].hParser, info->fd_udp); + } + } + while (1) + { + fd_set fds; + FD_ZERO(&fds); + FD_SET(fd_udp, &fds); + struct timeval to = {1, 5}; + int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); + + if (ret == 0) + { + if (!info->is_group_listener) + { + for (int i = 0; i < info->nnode; i++) + { + char cmd[12] = "LGCPSH"; + send_cmd_udp(info->fd_udp, + info->lidars[i].ip, info->lidars[i].port, + 0x0043, rand(), 6, cmd); + } + } + } + + if (ret < 0) + { + //printf("%d select error\n",fd_udp); + continue; + } + + gettimeofday(&tv, NULL); + // read UDP data + if (FD_ISSET(fd_udp, &fds)) + { + sockaddr_in addr; + socklen_t sz = sizeof(addr); + + int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); + if (nr > 0) + { + int id = -1; + for (int i = 0; i < info->nnode; i++) + { + if (addr.sin_addr.s_addr == info->lidars[i].s_addr) + { + id = i; + //saveLog(logPath, 1, info->lidars[i].ip, info->lidars[i].port, (unsigned char *)buf, sizeof(buf)); + break; + } + } + if (id == -1) + { + // continue; + // printf("packet from unknown address %s\n", inet_ntoa(addr.sin_addr)); + } + else if (buf[0] == 0x4c && buf[1] == 0x48 && buf[2] == ~0x41 && buf[3] == ~0x4b) + { + if (nr == sizeof(KeepAlive) + 12) + { + uint32_t clock = (tv.tv_sec % 3600) * 1000 + tv.tv_usec / 1000; + KeepAlive *ka = (KeepAlive *)(buf + 8); + if (clock >= ka->world_clock) + delay = clock - ka->world_clock; + else + delay = clock + 36000000 - ka->world_clock; + } + } + else + { + //printf(" %d %s\n",__LINE__,__FUNCTION__); + RawData *fans[MAX_FANS]; + int nfan = ParserRun(info->lidars[id], nr, (uint8_t *)buf, &(fans[0])); + // test for span without 0/180 + //for (int i=0; iangle, fans[i]->span,fans[i]->ts[0],fans[i]->ts[1]); + if (nfan > 0) + PublishData(info->lidars[id].hPublish, nfan, fans); + } + } + } + + if (tv.tv_sec > tto && !info->is_group_listener) + { + for (int i = 0; i < info->nnode; i++) + { + KeepAlive alive; + gettimeofday(&tv, NULL); + alive.world_clock = (tv.tv_sec % 3600) * 1000 + tv.tv_usec / 1000; + alive.delay = delay; + + // acknowlege device + // int rt = send_cmd_udp(fd_udp, info->lidar_ip, info->lidar_port, 0x4753, rand(), 0, NULL); + send_cmd_udp(info->fd_udp, info->lidars[i].ip, info->lidars[i].port, + 0x4b41, rand(), sizeof(alive), &alive); + } + + tto = tv.tv_sec + 3; + } + } + + return NULL; +} + +int AddLidar(HReader hr, const char *lidar_ip, unsigned short lidar_port, HParser hParser, HPublish hPub) +{ + UDPInfo *info = (UDPInfo *)hr; + + if (info->nnode >= MAX_LIDARS) + { + printf("There has %d lidars\n", info->nnode); + return -1; + } + + info->lidars[info->nnode].hParser = hParser; + info->lidars[info->nnode].hPublish = hPub; + strcpy(info->lidars[info->nnode].ip, lidar_ip); + info->lidars[info->nnode].port = lidar_port; + info->lidars[info->nnode].s_addr = inet_addr(lidar_ip); + + printf("add lidar [%d] %s:%d\n", info->nnode, lidar_ip, lidar_port); + + return info->nnode++; +} + +HReader StartUDPReader(const char *type, unsigned short listen_port, bool is_group_listener, const char *group_ip, + int lidar_count, const LidarInfo *lidars) +{ + UDPInfo *info = new UDPInfo; + memset(info, 0, sizeof(UDPInfo)); + + for (int i = 0; i < lidar_count; i++) + { + AddLidar(info, lidars[i].lidar_ip, lidars[i].lidar_port, lidars[i].parser, lidars[i].pub); + } + + strcpy(info->type, type); + info->listen_port = listen_port; + info->is_group_listener = is_group_listener; + + // open UDP port + info->fd_udp = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + + int one = 1; + setsockopt(info->fd_udp, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + + sockaddr_in addr; + addr.sin_family = AF_INET; + addr.sin_port = htons(listen_port); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + + int rt = ::bind(info->fd_udp, (struct sockaddr *)&addr, sizeof(addr)); + if (rt != 0) + { + printf("bind port %d failed\n", listen_port); + } + + printf("start udp listen port %d udp %d\n", listen_port, info->fd_udp); + + if (is_group_listener) + { + ip_mreq group; + memset(&group, 0, sizeof(group)); + group.imr_multiaddr.s_addr = inet_addr(group_ip); + group.imr_interface.s_addr = INADDR_ANY; + + int rt = setsockopt(info->fd_udp, IPPROTO_IP, + IP_ADD_MEMBERSHIP, (char *)&group, + sizeof(group)); + + printf("Adding to multicast group %s %s\n", group_ip, rt < 0 ? "fail!" : "ok"); + } + pthread_create(&info->thr, NULL, UdpThreadProc, info); + return info; +} + +bool SendUdpCmd(HReader hr, int lidar_id, int len, char *cmd) +{ + UDPInfo *info = (UDPInfo *)hr; + if (!info || info->fd_udp <= 0 || info->is_group_listener) + return false; + + if (lidar_id < 0 || lidar_id >= info->nnode) + return false; + + send_cmd_udp(info->fd_udp, info->lidars[lidar_id].ip, info->lidars[lidar_id].port,0x0043, rand(), len, cmd); + return true; +} +bool SendUdpCmd2(HReader hr, char *ip, int len, char *cmd) +{ + UDPInfo *info = (UDPInfo *)hr; + if (!info || info->fd_udp <= 0 || info->is_group_listener) + return false; + + for (int i = 0; i < info->nnode; i++) + { + if (strcmp(ip, info->lidars[i].ip) == 0) + { + send_cmd_udp(info->fd_udp, info->lidars[i].ip, info->lidars[i].port, + 0x0053, rand(), len, cmd); + return true; + } + } + return false; +} + +bool udp_talk_GS_PACK(int fd_udp, const char *ip, int port, int n, const char *cmd, void *result) +{ + unsigned short sn = rand(); + send_cmd_udp(fd_udp, ip, port, 0x4753, sn, n, cmd); + + int nr = 0; + for (int i = 0; i < 1000; i++) + { + fd_set fds; + FD_ZERO(&fds); + + FD_SET(fd_udp, &fds); + + struct timeval to = {1, 0}; + int ret = select(fd_udp + 1, &fds, NULL, NULL, &to); + + if (ret <= 0) + { + return false; + } + + // read UDP data + if (FD_ISSET(fd_udp, &fds)) + { + nr++; + sockaddr_in addr; + socklen_t sz = sizeof(addr); + + char buf[1024] = {0}; + int nr = recvfrom(fd_udp, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &sz); + if (nr > 0) + { + CmdHeader *hdr = (CmdHeader *)buf; + if (hdr->sign != 0x484c || hdr->sn != sn) + continue; + + memcpy(result, buf + sizeof(CmdHeader), sizeof(EEpromV101)); + // EEpromV101 t; + // memcpy(&t, result, sizeof(EEpromV101)); + return true; + } + } + } + + printf("read %d packets, not response\n", nr); + return false; +} + +void StopUDPReader(HReader hr) +{ + UDPInfo *info = (UDPInfo *)hr; + close(info->fd_udp); + // printf("stop udp reader\n"); + // info->should_exit = true; + // sleep(1); + pthread_join(info->thr, NULL); + + delete info; +} +bool setup_lidar_udp(HParser hP, int handle) +{ + Parser *parser = (Parser *)hP; + unsigned int index = 5; + int cmdLength; + char buf[32]; + char result[3] = {0}; + result[2] = '\0'; + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.ats); + if (cmdLength <= 0) + break; + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.ats, result)) + { + ROS_INFO("set ats %s\n", result); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.uuid); + if (cmdLength <= 0) + break; + if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.uuid, 11, "PRODUCT SN:", 20, buf)) + { + std::string sn = stringfilter(buf, 20); + ROS_INFO("uuid:%s\n", sn.c_str()); + break; + } + else if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.uuid, 10, "VENDOR ID:", 20, buf)) + { + std::string sn = stringfilter(buf, 20); + ROS_INFO("uuid:%s\n", sn.c_str()); + break; + } + } + + // enable/disable shadow filter + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.fitter); + if (cmdLength <= 0) + break; + + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.fitter, result)) + { + ROS_INFO("set LiDAR shadow filter %s %s\n", parser->cmd.fitter, result); + break; + } + } + // enable/disable smooth + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.smooth); + if (cmdLength <= 0) + break; + + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.smooth, result)) + { + ROS_INFO("set LiDAR smooth %s %s\n", parser->cmd.smooth, result); + break; + } + } + // setup rpm + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.rpm); + if (cmdLength <= 0) + break; + + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.rpm, result)) + { + ROS_INFO("%s %s\n", parser->cmd.rpm, result); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.res); + if (cmdLength <= 0) + break; + + if (udp_talk_C_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.res, 2, "OK", 0, NULL)) + { + ROS_INFO("%s OK\n", parser->cmd.res); + break; + } + } + + // enable/disable alaram message uploading + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.alarm); + if (cmdLength <= 0) + break; + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.alarm, result)) + { + ROS_INFO("set alarm_msg %s\n", result); + break; + } + } + + for (unsigned int i = 0; i < index; i++) + { + cmdLength = strlen(parser->cmd.direction); + if (cmdLength <= 0) + break; + if (udp_talk_S_PACK(handle, parser->ip, parser->port, cmdLength, parser->cmd.direction, result)) + { + ROS_INFO("set direction %s\n", result); + break; + } + } + + return true; +} diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/LICENSE b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..f6c26977bbb993b180afd759658dcf5ea6619cd0 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/LICENSE @@ -0,0 +1,194 @@ +木兰宽松许可证，第2版 + +木兰宽松许可证，第2版 + +2020年1月 http://license.coscl.org.cn/MulanPSL2 + +您对“软件”的复制、使用、修改及分发受木兰宽松许可证，第2版（“本许可证”）的如下条款的约束： + +0. 定义 + +“软件” 是指由“贡献”构成的许可在“本许可证”下的程序和相关文档的集合。 + +“贡献” 是指由任一“贡献者”许可在“本许可证”下的受版权法保护的作品。 + +“贡献者” 是指将受版权法保护的作品许可在“本许可证”下的自然人或“法人实体”。 + +“法人实体” 是指提交贡献的机构及其“关联实体”。 + +“关联实体” 是指，对“本许可证”下的行为方而言，控制、受控制或与其共同受控制的机构，此处的控制是 +指有受控方或共同受控方至少50%直接或间接的投票权、资金或其他有价证券。 + +1. 授予版权许可 + +每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的版权许可，您可 +以复制、使用、修改、分发其“贡献”，不论修改与否。 + +2. 授予专利许可 + +每个“贡献者”根据“本许可证”授予您永久性的、全球性的、免费的、非独占的、不可撤销的（根据本条规定 +撤销除外）专利许可，供您制造、委托制造、使用、许诺销售、销售、进口其“贡献”或以其他方式转移其“贡 +献”。前述专利许可仅限于“贡献者”现在或将来拥有或控制的其“贡献”本身或其“贡献”与许可“贡献”时的“软 +件”结合而将必然会侵犯的专利权利要求，不包括对“贡献”的修改或包含“贡献”的其他结合。如果您或您的“ +关联实体”直接或间接地，就“软件”或其中的“贡献”对任何人发起专利侵权诉讼（包括反诉或交叉诉讼）或 +其他专利维权行动，指控其侵犯专利权，则“本许可证”授予您对“软件”的专利许可自您提起诉讼或发起维权 +行动之日终止。 + +3. 无商标许可 + +“本许可证”不提供对“贡献者”的商品名称、商标、服务标志或产品名称的商标许可，但您为满足第4条规定 +的声明义务而必须使用除外。 + +4. 分发限制 + +您可以在任何媒介中将“软件”以源程序形式或可执行形式重新分发，不论修改与否，但您必须向接收者提供“ +本许可证”的副本，并保留“软件”中的版权、商标、专利及免责声明。 + +5. 免责声明与责任限制 + +“软件”及其中的“贡献”在提供时不带任何明示或默示的担保。在任何情况下，“贡献者”或版权所有者不对 +任何人因使用“软件”或其中的“贡献”而引发的任何直接或间接损失承担责任，不论因何种原因导致或者基于 +何种法律理论，即使其曾被建议有此种损失的可能性。 + +6. 语言 + +“本许可证”以中英文双语表述，中英文版本具有同等法律效力。如果中英文版本存在任何冲突不一致，以中文 +版为准。 + +条款结束 + +如何将木兰宽松许可证，第2版，应用到您的软件 + +如果您希望将木兰宽松许可证，第2版，应用到您的新软件，为了方便接收者查阅，建议您完成如下三步： + +1， 请您补充如下声明中的空白，包括软件名、软件的首次发表年份以及您作为版权人的名字； + +2， 请您在软件包的一级目录下创建以“LICENSE”为名的文件，将整个许可证文本放入该文件中； + +3， 请将如下声明文本放入每个源文件的头部注释中。 + +Copyright (c) [Year] [name of copyright holder] +[Software Name] is licensed under Mulan PSL v2. +You can use this software according to the terms and conditions of the Mulan +PSL v2. +You may obtain a copy of Mulan PSL v2 at: + http://license.coscl.org.cn/MulanPSL2 +THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO +NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE. +See the Mulan PSL v2 for more details. + +Mulan Permissive Software License，Version 2 + +Mulan Permissive Software License，Version 2 (Mulan PSL v2) + +January 2020 http://license.coscl.org.cn/MulanPSL2 + +Your reproduction, use, modification and distribution of the Software shall +be subject to Mulan PSL v2 (this License) with the following terms and +conditions: + +0. Definition + +Software means the program and related documents which are licensed under +this License and comprise all Contribution(s). + +Contribution means the copyrightable work licensed by a particular +Contributor under this License. + +Contributor means the Individual or Legal Entity who licenses its +copyrightable work under this License. + +Legal Entity means the entity making a Contribution and all its +Affiliates. + +Affiliates means entities that control, are controlled by, or are under +common control with the acting entity under this License, ‘control’ means +direct or indirect ownership of at least fifty percent (50%) of the voting +power, capital or other securities of controlled or commonly controlled +entity. + +1. Grant of Copyright License + +Subject to the terms and conditions of this License, each Contributor hereby +grants to you a perpetual, worldwide, royalty-free, non-exclusive, +irrevocable copyright license to reproduce, use, modify, or distribute its +Contribution, with modification or not. + +2. Grant of Patent License + +Subject to the terms and conditions of this License, each Contributor hereby +grants to you a perpetual, worldwide, royalty-free, non-exclusive, +irrevocable (except for revocation under this Section) patent license to +make, have made, use, offer for sale, sell, import or otherwise transfer its +Contribution, where such patent license is only limited to the patent claims +owned or controlled by such Contributor now or in future which will be +necessarily infringed by its Contribution alone, or by combination of the +Contribution with the Software to which the Contribution was contributed. +The patent license shall not apply to any modification of the Contribution, +and any other combination which includes the Contribution. If you or your +Affiliates directly or indirectly institute patent litigation (including a +cross claim or counterclaim in a litigation) or other patent enforcement +activities against any individual or entity by alleging that the Software or +any Contribution in it infringes patents, then any patent license granted to +you under this License for the Software shall terminate as of the date such +litigation or activity is filed or taken. + +3. No Trademark License + +No trademark license is granted to use the trade names, trademarks, service +marks, or product names of Contributor, except as required to fulfill notice +requirements in section 4. + +4. Distribution Restriction + +You may distribute the Software in any medium with or without modification, +whether in source or executable forms, provided that you provide recipients +with a copy of this License and retain copyright, patent, trademark and +disclaimer statements in the Software. + +5. Disclaimer of Warranty and Limitation of Liability + +THE SOFTWARE AND CONTRIBUTION IN IT ARE PROVIDED WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED. IN NO EVENT SHALL ANY CONTRIBUTOR OR +COPYRIGHT HOLDER BE LIABLE TO YOU FOR ANY DAMAGES, INCLUDING, BUT NOT +LIMITED TO ANY DIRECT, OR INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING +FROM YOUR USE OR INABILITY TO USE THE SOFTWARE OR THE CONTRIBUTION IN IT, NO +MATTER HOW IT’S CAUSED OR BASED ON WHICH LEGAL THEORY, EVEN IF ADVISED OF +THE POSSIBILITY OF SUCH DAMAGES. + +6. Language + +THIS LICENSE IS WRITTEN IN BOTH CHINESE AND ENGLISH, AND THE CHINESE VERSION +AND ENGLISH VERSION SHALL HAVE THE SAME LEGAL EFFECT. IN THE CASE OF +DIVERGENCE BETWEEN THE CHINESE AND ENGLISH VERSIONS, THE CHINESE VERSION +SHALL PREVAIL. + +END OF THE TERMS AND CONDITIONS + +How to Apply the Mulan Permissive Software License，Version 2 +(Mulan PSL v2) to Your Software + +To apply the Mulan PSL v2 to your work, for easy identification by +recipients, you are suggested to complete following three steps: + +i. Fill in the blanks in following statement, including insert your software +name, the year of the first publication of your software, and your name +identified as the copyright owner; + +ii. Create a file named "LICENSE" which contains the whole context of this +License in the first directory of your software package; + +iii. Attach the statement to the appropriate annotated syntax at the +beginning of each source file. + +Copyright (c) [Year] [name of copyright holder] +[Software Name] is licensed under Mulan PSL v2. +You can use this software according to the terms and conditions of the Mulan +PSL v2. +You may obtain a copy of Mulan PSL v2 at: + http://license.coscl.org.cn/MulanPSL2 +THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY +KIND, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO +NON-INFRINGEMENT, MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE. +See the Mulan PSL v2 for more details. diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/README.md b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/README.md new file mode 100644 index 0000000000000000000000000000000000000000..b8b0db8c6728e1aaf6b8ca8ab5bf1105317150fc --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/README.md @@ -0,0 +1,5 @@ +该目录下dtof_ros_demo_7_plane_ok是基于ss928 主控SoC，采用开源openEuler Embeded系统，运行在ros2 humble版本上，实现7个dToF的点云拼接，模拟的是一个无人机模型，开发者可以基于自己实际的dToF数量进行有选择的裁剪，具体步骤如下： +1. /hirobot_componet_dtof/hieuler/dtof_ros_demo_7_plane_ok/src/dtof_client_node/launch目录下的dtof_client_node.launch.py文件中launch_ros.actions.Node节点参考样例修改成实际的dToF接入参数，端口号默认即可，ip地址dToF出厂时会进行标注，Node节点数量本实例为7，实际需要开发者根据自己接入的dToF数量进行修改，ld.add_action也需要跟实际接入的dToF数量保持一致。 +2. /hirobot_componet_dtof/hieuler/dtof_ros_demo_7_plane_ok/src/depth_image/depth_image_to_point_cloud/launch目录下point_cloud_xyz.launch.py文件中launch_ros.descriptions.ComposableNode可参考样例修改成实际dToF的接入环境，需要与dtof_client_node.launch.py中的配置进行对应。 +4. /hirobot_componet_dtof/hieuler/dtof_ros_demo_7_plane_ok/src/hirobot_description/urdf目录下的robot.urdf为统一机器人描述格式文件，是一个XML语法框架下用来描述机器人的语言格式, urdf在ROS界很流行。一个机器人主要由连杆（link）和关节（joint），开发者需要根据实际dToF的接入数量来配置urdf文件中的link和joint参数。 +5. 点云拼接是主要调整的就是urdf文件中的xyz和rpy六个参数，xyz就是正交坐标xyz（满足右手定则），在rviz中显示为红:x 绿:y 蓝:z，对于rpy分别对应Roll（横滚-以X轴翻转）、Pitch（俯仰-以Y轴翻转）、Yaw（偏航-以Z轴翻转） \ No newline at end of file diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CHANGELOG.rst b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CHANGELOG.rst new file mode 100644 index 0000000000000000000000000000000000000000..c655da6f4629d26637af496ffadd879d319ac71e --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CHANGELOG.rst @@ -0,0 +1,173 @@ +2.0.0 (2018-12-09) +------------------ +* enable rclcpp_register_node_plugins (`#368 `_) +* Port depth image proc on ROS2 (`#362 `_) +* Initial ROS2 commit. +* Contributors: Chris Ye, Michael Carroll + +2.2.1 (2020-08-27) +------------------ +* remove email blasts from steve macenski (`#596 `_) +* [Foxy] Use ament_auto Macros (`#573 `_) + * Fixing version and maintainer problems in camera_calibration. + * Applying ament_auto macros to depth_image_proc. + * Cleaning up package.xml in image_pipeline. + * Applying ament_auto macros to image_proc. + * Applying ament_auto macros to image_publisher. + * Applying ament_auto macros to image_rotate. + * Applying ament_auto macros to image_view. + * Replacing some deprecated headers in image_view. + * Fixing some build warnings in image_view. + * Applying ament_auto macros to stereo_image_proc. + * Adding some linter tests to image_pipeline. + * Updating package URLs to point to ROS Index. +* Add rclcpp and rclcpp_components dependencies to package.xml. (`#569 `_) (`#570 `_) + I noticed that these are listed in CMakeLists.txt but not in package.xml + and this is causing a build failure for the binary releases on + build.ros2.org: + http://build.ros2.org/view/Dbin_ubhf_uBhf/job/Dbin_uB64__depth_image_proc__ubuntu_bionic_amd64__binary/ + Co-authored-by: Steven! Ragnarök +* Contributors: Joshua Whitley, Steve Macenski + +2.2.0 (2020-07-27) +------------------ +* Replacing deprecated header includes with new HPP versions. (`#566 `_) + * Replacing deprecated header includes with new HPP versions. + * CI: Switching to official Foxy Docker container. + * Fixing headers which don't work correctly. +* Contributors: Joshua Whitley + +* make parameters work in depth_image_proc (`#544 `_) +* update depth_image_proc components (`#543 `_) + * update depth_image_proc components + This makes them loadable with the rclcpp_components + interface. I've fully tested PointCloudXYZRGB and + ConvertMetric, my use case doesn't use the others. + I also lack a setup to test the launch files fully, + but ran them with the realsense commented out and + they appear to be OK. + * fix linting +* Contributors: Michael Ferguson + +1.12.23 (2018-05-10) +-------------------- + +1.12.22 (2017-12-08) +-------------------- + +1.12.21 (2017-11-05) +-------------------- +* Fix C++11 compilation + This fixes `#292 `_ and `#291 `_ +* Contributors: Vincent Rabaud + +1.12.20 (2017-04-30) +-------------------- +* Fix CMake warnings about Eigen. +* Convert depth image metric from [m] to [mm] +* address gcc6 build error + With gcc6, compiling fails with `stdlib.h: No such file or directory`, + as including '-isystem /usr/include' breaks with gcc6, cf., + https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129. + This commit addresses this issue for this package in the same way + it was addressed in various other ROS packages. A list of related + commits and pull requests is at: + https://github.com/ros/rosdistro/issues/12783 + Signed-off-by: Lukas Bulwahn +* Contributors: Kentaro Wada, Lukas Bulwahn, Vincent Rabaud + +1.12.19 (2016-07-24) +-------------------- + +1.12.18 (2016-07-12) +-------------------- + +1.12.17 (2016-07-11) +-------------------- + +1.12.16 (2016-03-19) +-------------------- +* check number of channels before the process +* search minimum value with OpenCV +* Use OpenCV to be faster +* Add a feature for a depth image to crop foremost image +* Contributors: Kenta Yonekura + +1.12.15 (2016-01-17) +-------------------- +* Add option for exact time sync for point_cloud_xyzrgb +* simplify OpenCV3 conversion +* Contributors: Kentaro Wada, Vincent Rabaud + +1.12.14 (2015-07-22) +-------------------- + +1.12.13 (2015-04-06) +-------------------- +* Add radial point cloud processors +* Contributors: Hunter Laux + +1.12.12 (2014-12-31) +-------------------- +* adds range_max +* exports depth_conversions + with convert for xyz PC only +* exports DepthTraits +* Contributors: enriquefernandez + +1.12.11 (2014-10-26) +-------------------- + +1.12.10 (2014-09-28) +-------------------- + +1.12.9 (2014-09-21) +------------------- +* get code to compile with OpenCV3 + fixes `#96 `_ +* Contributors: Vincent Rabaud + +1.12.8 (2014-08-19) +------------------- + +1.12.6 (2014-07-27) +------------------- +* Add point_cloud_xyzi nodelet + This is for cameras that output depth and intensity images. + It's based on the point_cloud_xyzrgb nodelet. +* Missing runtime dependency - eigen_conversions + `libdepth_image_proc` is missing this dependency at runtime + ``` + > ldd libdepth_image_proc.so | grep eigen + libeigen_conversions.so => not found + ``` + Which causes the following error on loading depth_image_proc: + ``` + [ INFO] [1402564815.530736554]: /camera/rgb/camera_info -> /camera/rgb/camera_info + [ERROR] [1402564815.727176562]: Failed to load nodelet [/camera/depth_metric_rect] of type + [depth_image_proc/convert_metric]: Failed to load library /opt/ros/indigo/lib//libdepth_image_proc.so. + Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that + names are consistent between this macro and your XML. Error string: Could not load library (Poco + exception = libeigen_conversions.so: cannot open shared object file: No such file or directory) + [FATAL] [1402564815.727410623]: Service call failed! + ``` +* Contributors: Daniel Stonier, Hunter Laux + +1.12.4 (2014-04-28) +------------------- +* depth_image_proc: fix missing symbols in nodelets +* Contributors: Michael Ferguson + +1.12.3 (2014-04-12) +------------------- + +1.12.2 (2014-04-08) +------------------- + +1.12.1 (2014-04-06) +------------------- +* replace tf usage by tf2 usage + +1.12.0 (2014-04-04) +------------------- +* remove PCL dependency diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CMakeLists.txt b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..97501d3229de9f831b26ec78f36591fff40a9894 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.5) +project(depth_image_proc) + +# 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_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + include_directories(include ${EIGEN3_INCLUDE_DIRS}) +endif() + +find_package(OpenCV REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/conversions.cpp + src/convert_metric.cpp + src/crop_foremost.cpp + src/disparity.cpp + src/point_cloud_xyz.cpp + src/point_cloud_xyzrgb.cpp + src/point_cloud_xyzi.cpp + src/point_cloud_xyz_radial.cpp + src/point_cloud_xyzi_radial.cpp + src/point_cloud_xyzrgb_radial.cpp + src/register.cpp +) + +rclcpp_components_register_nodes(${PROJECT_NAME} + "${PROJECT_NAME}::ConvertMetricNode" + "${PROJECT_NAME}::CropForemostNode" + "${PROJECT_NAME}::DisparityNode" + "${PROJECT_NAME}::PointCloudXyzNode" + "${PROJECT_NAME}::PointCloudXyzRadialNode" + "${PROJECT_NAME}::PointCloudXyziNode" + "${PROJECT_NAME}::PointCloudXyziRadialNode" + "${PROJECT_NAME}::PointCloudXyzrgbNode" + "${PROJECT_NAME}::PointCloudXyzrgbRadialNode" + "${PROJECT_NAME}::RegisterNode" +) + +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/conversions.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/conversions.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0d76391550a0abb2139709274ad8e2b73df49934 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/conversions.hpp @@ -0,0 +1,157 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__CONVERSIONS_HPP_ +#define DEPTH_IMAGE_PROC__CONVERSIONS_HPP_ + +#include +#include +#include +#include + +#include + +namespace depth_image_proc +{ + +// Handles float or uint16 depths +template +void convertDepth( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + const image_geometry::PinholeCameraModel & model, + double range_max = 0.0) +{ + // Use correct principal point from calibration + // Use correct principal point from calibration + auto center_x = static_cast(model.cx()); + auto center_y = static_cast(model.cy()); + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + double unit_scaling = DepthTraits::toMeters(T(1)); + auto constant_x = static_cast(unit_scaling / model.fx()); + auto constant_y = static_cast(unit_scaling / model.fy()); + float bad_point = std::numeric_limits::quiet_NaN(); + + sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + const T *depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + // printf("row_step is %d %d %d\n", row_step, cloud_msg->height, cloud_msg->width); + for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { + for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { + T depth = depth_row[u]; + + // Missing points denoted by NaNs + if (!DepthTraits::valid(depth)) { + if (range_max != 0.0) { + depth = DepthTraits::fromMeters(range_max); + } else { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } + } + + // Fill in XYZ + *iter_x = (static_cast(u) - center_x) * depth * constant_x; + *iter_y = (static_cast(v) - center_y) * depth * constant_y * (-1); + *iter_z = DepthTraits::toMeters(depth); + // *iter_x = DepthTraits::toMeters(depth); + // *iter_z = -(v - center_y) * depth * constant_y; + // *iter_y = (u - center_x) * depth * constant_x; + + + } + } +} + +// Handles float or uint16 depths +template +void convertDepthRadial( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + cv::Mat & transform) +{ + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + float bad_point = std::numeric_limits::quiet_NaN(); + + sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); + const T * depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { + for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { + T depth = depth_row[u]; + + // Missing points denoted by NaNs + if (!DepthTraits::valid(depth)) { + *iter_x = *iter_y = *iter_z = bad_point; + continue; + } + const cv::Vec3f & cvPoint = transform.at(u, v) * DepthTraits::toMeters(depth); + // Fill in XYZ + *iter_x = cvPoint(0); + *iter_y = cvPoint(1) * (-1); + *iter_z = cvPoint(2); + } + } +} + +// Handles float or uint16 depths +template +void convertIntensity( + const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg, + sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) +{ + sensor_msgs::PointCloud2Iterator iter_i(*cloud_msg, "intensity"); + const T * inten_row = reinterpret_cast(&intensity_msg->data[0]); + + const int i_row_step = intensity_msg->step / sizeof(T); + for (int v = 0; v < static_cast(cloud_msg->height); ++v, inten_row += i_row_step) { + for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_i) { + *iter_i = inten_row[u]; + } + } +} + +// Handles RGB8, BGR8, and MONO8 +void convertRgb( + const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, + sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + int red_offset, int green_offset, int blue_offset, int color_step); + +cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial); + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__CONVERSIONS_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/depth_traits.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/depth_traits.hpp new file mode 100644 index 0000000000000000000000000000000000000000..1dcf5b1d0d6138ac7b3cba8fcd9acf50c1a6a097 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/depth_traits.hpp @@ -0,0 +1,74 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__DEPTH_TRAITS_HPP_ +#define DEPTH_IMAGE_PROC__DEPTH_TRAITS_HPP_ + +#include +#include +#include +#include + +namespace depth_image_proc +{ + +// Encapsulate differences between processing float and uint16_t depths +template +struct DepthTraits {}; + +template<> +struct DepthTraits +{ + static inline bool valid(uint16_t depth) {return depth != 0;} + static inline float toMeters(uint16_t depth) {return depth * 0.001f;} // originally mm + static inline uint16_t fromMeters(float depth) {return (depth * 1000.0f) + 0.5f;} + // Do nothing - already zero-filled + static inline void initializeBuffer(std::vector & buffer) {(void) buffer;} +}; + +template<> +struct DepthTraits +{ + static inline bool valid(float depth) {return std::isfinite(depth);} + static inline float toMeters(float depth) {return depth;} + static inline float fromMeters(float depth) {return depth;} + + static inline void initializeBuffer(std::vector & buffer) + { + float * start = reinterpret_cast(&buffer[0]); + float * end = reinterpret_cast(&buffer[0] + buffer.size()); + std::fill(start, end, std::numeric_limits::quiet_NaN()); + } +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__DEPTH_TRAITS_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz.hpp new file mode 100644 index 0000000000000000000000000000000000000000..aa823ceff500c2bc1c881dc67b405e26c623f255 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class PointCloudXyzNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyzNode(const rclcpp::NodeOptions & options); + +private: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + + // Subscriptions + image_transport::CameraSubscriber sub_depth_; + int queue_size_; + std::string output_frame; + + // Publications + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + image_geometry::PinholeCameraModel model_; + + void connectCb(); + + void depthCb( + const Image::ConstSharedPtr & depth_msg, + const CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyzNode"); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz_radial.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz_radial.hpp new file mode 100644 index 0000000000000000000000000000000000000000..5292afda1eced6482b34104835ff2c38f3647f6c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyz_radial.hpp @@ -0,0 +1,86 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_RADIAL_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_RADIAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class PointCloudXyzRadialNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyzRadialNode(const rclcpp::NodeOptions & options); + +private: + // Subscriptions + image_transport::CameraSubscriber sub_depth_; + int queue_size_; + + // Publications + std::mutex connect_mutex_; + using PointCloud = sensor_msgs::msg::PointCloud2; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + std::vector D_; + std::array K_; + + uint32_t width_; + uint32_t height_; + + cv::Mat transform_; + + void connectCb(); + + void depthCb( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyzRadialNode"); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZ_RADIAL_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi.hpp new file mode 100644 index 0000000000000000000000000000000000000000..304956497f6192c6e432294995aedbb28823a1f3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class PointCloudXyziNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyziNode(const rclcpp::NodeOptions & options); + +private: + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + using PointCloud = sensor_msgs::msg::PointCloud2; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_, sub_intensity_; + message_filters::Subscriber sub_info_; + using SyncPolicy = + message_filters::sync_policies::ApproximateTime; + using Synchronizer = message_filters::Synchronizer; + std::shared_ptr sync_; + + // Publications + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + image_geometry::PinholeCameraModel model_; + + void connectCb(); + + void imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & intensity_msg, + const CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyziNode"); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi_radial.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi_radial.hpp new file mode 100644 index 0000000000000000000000000000000000000000..88fc1aae7549db077e1b01ac86078f807e6c3301 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzi_radial.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_RADIAL_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_RADIAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; +using SyncPolicy = + message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo>; + +class PointCloudXyziRadialNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyziRadialNode(const rclcpp::NodeOptions & options); + +private: + using PointCloud = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_, sub_intensity_; + message_filters::Subscriber sub_info_; + + int queue_size_; + + // Publications + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + using Synchronizer = message_filters::Synchronizer; + std::shared_ptr sync_; + + std::vector D_; + std::array K_; + + uint32_t width_; + uint32_t height_; + + cv::Mat transform_; + + void connectCb(); + + void imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & intensity_msg_in, + const CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyziRadialNode"); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZI_RADIAL_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb.hpp new file mode 100644 index 0000000000000000000000000000000000000000..58efdf31d3f0ce38319ee0c043c0b8c7227401fc --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class PointCloudXyzrgbNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyzrgbNode(const rclcpp::NodeOptions & options); + +private: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_, sub_rgb_; + message_filters::Subscriber sub_info_; + using SyncPolicy = + message_filters::sync_policies::ApproximateTime; + using ExactSyncPolicy = + message_filters::sync_policies::ExactTime; + using Synchronizer = message_filters::Synchronizer; + using ExactSynchronizer = message_filters::Synchronizer; + std::shared_ptr sync_; + std::shared_ptr exact_sync_; + + // Publications + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + image_geometry::PinholeCameraModel model_; + + void connectCb(); + + void imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & rgb_msg, + const CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("PointCloudXyzrgbNode"); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp new file mode 100644 index 0000000000000000000000000000000000000000..81864925f06c4b2fcb6ddf3b746e8ce91297e5bb --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_RADIAL_HPP_ +#define DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_RADIAL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class PointCloudXyzrgbRadialNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions & options); + +private: + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_, sub_rgb_; + message_filters::Subscriber sub_info_; + using SyncPolicy = + message_filters::sync_policies::ApproximateTime; + using ExactSyncPolicy = + message_filters::sync_policies::ExactTime; + using Synchronizer = message_filters::Synchronizer; + using ExactSynchronizer = message_filters::Synchronizer; + std::unique_ptr sync_; + std::unique_ptr exact_sync_; + + // Publications + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + + std::vector D_; + std::array K_; + + uint32_t width_; + uint32_t height_; + + cv::Mat transform_; + + image_geometry::PinholeCameraModel model_; + + void connectCb(); + + void imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & rgb_msg, + const CameraInfo::ConstSharedPtr & info_msg); + + rclcpp::Logger logger_ = this->get_logger(); +}; + +} // namespace depth_image_proc + +#endif // DEPTH_IMAGE_PROC__POINT_CLOUD_XYZRGB_RADIAL_HPP_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/visibility.h b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/visibility.h new file mode 100644 index 0000000000000000000000000000000000000000..43d4d047a39c8b050973e6e5b3ffe9eb6eafcf60 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/include/depth_image_proc/visibility.h @@ -0,0 +1,83 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#ifndef DEPTH_IMAGE_PROC__VISIBILITY_H_ +#define DEPTH_IMAGE_PROC__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + + #ifdef __GNUC__ + #define DEPTH_IMAGE_PROC_EXPORT __attribute__ ((dllexport)) + #define DEPTH_IMAGE_PROC_IMPORT __attribute__ ((dllimport)) + #else + #define DEPTH_IMAGE_PROC_EXPORT __declspec(dllexport) + #define DEPTH_IMAGE_PROC_IMPORT __declspec(dllimport) + #endif + + #ifdef DEPTH_IMAGE_PROC_DLL + #define DEPTH_IMAGE_PROC_PUBLIC DEPTH_IMAGE_PROC_EXPORT + #else + #define DEPTH_IMAGE_PROC_PUBLIC DEPTH_IMAGE_PROC_IMPORT + #endif + + #define DEPTH_IMAGE_PROC_PUBLIC_TYPE DEPTH_IMAGE_PROC_PUBLIC + + #define DEPTH_IMAGE_PROC_LOCAL + +#else + + #define DEPTH_IMAGE_PROC_EXPORT __attribute__ ((visibility("default"))) + #define DEPTH_IMAGE_PROC_IMPORT + + #if __GNUC__ >= 4 + #define DEPTH_IMAGE_PROC_PUBLIC __attribute__ ((visibility("default"))) + #define DEPTH_IMAGE_PROC_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define DEPTH_IMAGE_PROC_PUBLIC + #define DEPTH_IMAGE_PROC_LOCAL + #endif + + #define DEPTH_IMAGE_PROC_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // DEPTH_IMAGE_PROC__VISIBILITY_H_ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/convert_metric.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/convert_metric.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..37e6c09f2f1d54bed56a6c4622d9844298b88922 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/convert_metric.launch.py @@ -0,0 +1,75 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/convert_metric.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # launch plugin through rclcpp_components container + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::ConvertMetricNode', + name='convert_metric_node', + remappings=[('image_raw', '/camera/depth/image_rect_raw'), + ('camera_info', '/camera/depth/camera_info'), + ('image', '/camera/depth/converted_image')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/crop_foremost.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/crop_foremost.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..17485336fc68b6194da1dfd080ad703f12bc10c2 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/crop_foremost.launch.py @@ -0,0 +1,74 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/crop_formost.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::CropForemostNode', + name='crop_foremost_node', + remappings=[('image_raw', '/camera/depth/image_rect_raw'), + ('camera_info', '/camera/depth/camera_info'), + ('image', '/camera/depth/converted_image')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/disparity.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/disparity.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..acf2af812a1a5de2c8c19f125ef0161889846ce3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/disparity.launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/disparity.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # we use realsense camera for test, realsense not support left and right topic + # so we remap to depth image only for interface test. + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::DisparityNode', + name='disparity_node', + remappings=[('left/image_rect', '/camera/depth/image_rect_raw'), + ('right/camera_info', '/camera/depth/camera_info'), + ('left/disparity', '/camera/left/disparity')] + ), + ], + output='screen', + ), + + # TODO: rviz could not display disparity(stereo_msgs) + # run stereo_view for display after image_view be ported + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..ed72e6c2bca367dc010a925d19559c3d91a1af6c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz.launch.py @@ -0,0 +1,119 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + return LaunchDescription([ + + # launch plugin through rclcpp_components container + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node1', + remappings=[('image_rect', '/dtof/depth1/image'), + ('/camera_info', '/dtof/depth1/camera_info'), + ('points', '/dtof/depth1/point')], + parameters=[{'output_frame':'tof_link1'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node2', + remappings=[('image_rect', '/dtof/depth2/image'), + ('/camera_info', '/dtof/depth2/camera_info'), + ('points', '/dtof/depth2/point')], + parameters=[{'output_frame':'tof_link2'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node3', + remappings=[('image_rect', '/dtof/depth3/image'), + ('/camera_info', '/dtof/depth3/camera_info'), + ('points', '/dtof/depth3/point')], + parameters=[{'output_frame':'tof_link3'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node4', + remappings=[('image_rect', '/dtof/depth4/image'), + ('/camera_info', '/dtof/depth4/camera_info'), + ('points', '/dtof/depth4/point')], + parameters=[{'output_frame':'tof_link4'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node5', + remappings=[('image_rect', '/dtof/depth5/image'), + ('/camera_info', '/dtof/depth5/camera_info'), + ('points', '/dtof/depth5/point')], + parameters=[{'output_frame':'tof_link5'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node6', + remappings=[('image_rect', '/dtof/depth6/image'), + ('/camera_info', '/dtof/depth6/camera_info'), + ('points', '/dtof/depth6/point')], + parameters=[{'output_frame':'tof_link6'}] + ), + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzNode', + name='point_cloud_xyz_node7', + remappings=[('image_rect', '/dtof/depth7/image'), + ('/camera_info', '/dtof/depth7/camera_info'), + ('points', '/dtof/depth7/point')], + parameters=[{'output_frame':'tof_link7'}] + ), + + ], + output='screen', + ), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz_radial.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz_radial.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..d578a05ce4def355a7d834932967628373ae9402 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyz_radial.launch.py @@ -0,0 +1,75 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/point_cloud_xyz_radial.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # launch plugin through rclcpp_components container + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzRadialNode', + name='point_cloud_xyz_radial_node', + remappings=[('image_raw', '/camera/depth/image_rect_raw'), + ('camera_info', '/camera/depth/camera_info'), + ('image', '/camera/depth/converted_image')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..191b3f089129be78e860f701fffebbe6614c1746 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi.launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/point_cloud_xyzi.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # TODO: Realsense camera do not support intensity message + # use color image instead of intensity only for interface test + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyziNode', + name='point_cloud_xyzi', + remappings=[('depth/image_rect', '/camera/aligned_depth_to_color/image_raw'), + ('intensity/image_rect', '/camera/color/image_raw'), + ('intensity/camera_info', '/camera/color/camera_info'), + ('points', '/camera/depth/points')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi_radial.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi_radial.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..7213877480b9f585cdd9cf4cfd0a8495c3825536 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzi_radial.launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/point_cloud_xyzi.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # TODO: Realsense camera do not support intensity message, + # use depth instead of intensity only for interface test + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyziRadialNode', + name='point_cloud_xyzi_radial_node', + remappings=[('depth/image_raw', '/camera/depth/image_rect_raw'), + ('intensity/image_raw', '/camera/depth/image_rect_raw'), + ('intensity/camera_info', '/camera/depth/camera_info'), + ('points', '/camera/depth/points')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..10b333ac267f9e46680bfed6a496b2bcd55810d1 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb.launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/point_cloud_xyzrgb.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # launch plugin through rclcpp_components container + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzrgbNode', + name='point_cloud_xyzrgb_node', + remappings=[('rgb/camera_info', '/camera/color/camera_info'), + ('rgb/image_rect_color', '/camera/color/image_raw'), + ('depth_registered/image_rect', + '/camera/aligned_depth_to_color/image_raw'), + ('points', '/camera/depth_registered/points')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb_radial.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb_radial.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..263e2397c2ce337b861d96f74a390586ecb1f8b9 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/point_cloud_xyzrgb_radial.launch.py @@ -0,0 +1,90 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +# uncomment if you want to launch rviz too in this launch +# import os +# from ament_index_python.packages import get_package_share_directory +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/point_cloud_xyzrgb.rviz') + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name='rviz', default_value='False', + description='Launch RViz for viewing point cloud xyzrgb radial data' + ), + launch_ros.actions.Node( + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration('rviz') + ), + package='rviz2', + executable='rviz2', + output='screen', + arguments=['--display-config', default_rviz] + ), + # launch plugin through rclcpp_components container + # make sure remapped topics are available + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::PointCloudXyzrgbRadialNode', + name='point_cloud_xyzrgb_node', + remappings=[('rgb/camera_info', '/camera/color/camera_info'), + ('rgb/image_rect_color', '/camera/color/image_raw'), + ('depth_registered/image_rect', + '/camera/aligned_depth_to_color/image_raw'), + ('points', '/camera/depth_registered/points')] + ), + ], + output='screen', + ), + + # rviz + # launch_ros.actions.Node( + # package='rviz2', executable='rviz2', output='screen', + # arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/register.launch.py b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/register.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..0eab8878e6bbb1eb822df29b1728c399c07000e3 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/register.launch.py @@ -0,0 +1,79 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + +import launch_ros.actions +import launch_ros.descriptions + + +def generate_launch_description(): + default_rviz = os.path.join(get_package_share_directory('depth_image_proc'), + 'launch', 'rviz/register.rviz') + return LaunchDescription([ + # install realsense from https://github.com/intel/ros2_intel_realsense + launch_ros.actions.Node( + package='realsense_ros2_camera', node_executable='realsense_ros2_camera', + output='screen'), + + # launch plugin through rclcpp_components container + launch_ros.actions.ComposableNodeContainer( + name='container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + # Driver itself + launch_ros.descriptions.ComposableNode( + package='depth_image_proc', + plugin='depth_image_proc::RegisterNode', + name='register_node', + remappings=[('depth/image_rect', '/camera/depth/image_rect_raw'), + ('depth/camera_info', '/camera/depth/camera_info'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth_registered/image_rect', + '/camera/depth_registered/image_rect'), + ('depth_registered/camera_info', + '/camera/depth_registered/camera_info')] + ), + ], + output='screen', + ), + + # rviz + launch_ros.actions.Node( + package='rviz2', node_executable='rviz2', output='screen', + arguments=['--display-config', default_rviz]), + ]) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/convert_metric.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/convert_metric.rviz new file mode 100644 index 0000000000000000000000000000000000000000..d76e1637f9b5d78597b8e5c405ec986b0b76bfce --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/convert_metric.rviz @@ -0,0 +1,136 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /image_raw1 + - /image_raw1/Status1 + - /converted_image1 + - /converted_image1/Status1 + Splitter Ratio: 0.5 + Tree Height: 581 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: image_raw + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: converted_image + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/converted_image + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_color_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: XYOrbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000282000000c900fffffffb000000120069006d006100670065005f00720061007701000002c5000000c50000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0063006f006e007600650072007400650064005f0069006d0061006700650100000390000000cc0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d00610067006500000003ad000000af0000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003ae0000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1853 + X: 67 + Y: 27 + converted_image: + collapsed: false + image_raw: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/crop_formost.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/crop_formost.rviz new file mode 100644 index 0000000000000000000000000000000000000000..f1cf786e67992f715e4985707c242760dd04e4f0 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/crop_formost.rviz @@ -0,0 +1,134 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /image_raw1 + - /crop_name1 + Splitter Ratio: 0.5 + Tree Height: 581 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: image_raw + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: crop_name + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/crop_image + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_color_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: XYOrbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000282000000c900fffffffb000000120069006d006100670065005f00720061007701000002c5000000c50000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200630072006f0070005f006e0061006d00650100000390000000cc0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d00610067006500000003ad000000af0000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003ae0000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1853 + X: 67 + Y: 27 + crop_name: + collapsed: false + image_raw: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/disparity.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/disparity.rviz new file mode 100644 index 0000000000000000000000000000000000000000..6ffe90c5ca5464cf3b8917f075bfe22887103dd0 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/disparity.rviz @@ -0,0 +1,134 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /fake left/image_rect1 + - /fake left/disparity1 + Splitter Ratio: 0.5 + Tree Height: 581 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: fake left/image_rect + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: fake left/disparity + Normalize Range: true + Queue Size: 10 + Topic: /camera/left/disparity + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_color_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: XYOrbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000282000000c900fffffffb0000002800660061006b00650020006c006500660074002f0069006d006100670065005f007200650063007401000002c5000000c50000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002600660061006b00650020006c006500660074002f0064006900730070006100720069007400790100000390000000cc0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d00610067006500000003ad000000af0000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003ae0000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1853 + X: 67 + Y: 27 + fake left/disparity: + collapsed: false + fake left/image_rect: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz.rviz new file mode 100644 index 0000000000000000000000000000000000000000..ae68f102aebbe42de63d32e362c361ac8c12ed30 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /image_rect1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 608 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: image_rect + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_depth_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.125654935836792 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5597962141036987 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.718582630157471 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb000000140069006d006100670065005f007200650063007401000002e00000017c0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c30000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 + image_rect: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz_radial.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz_radial.rviz new file mode 100644 index 0000000000000000000000000000000000000000..ae68f102aebbe42de63d32e362c361ac8c12ed30 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyz_radial.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /image_rect1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 608 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: image_rect + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_depth_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.125654935836792 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5597962141036987 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.718582630157471 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb000000140069006d006100670065005f007200650063007401000002e00000017c0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c30000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 + image_rect: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzi.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzi.rviz new file mode 100644 index 0000000000000000000000000000000000000000..ee87ced3026b6e50ae35707b76ec4ca9394906ae --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzi.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /image_rect1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 608 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: image_rect + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 2 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_depth_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.125654935836792 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5597962141036987 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.718582630157471 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb000000140069006d006100670065005f007200650063007401000002e00000017c0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c30000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 + image_rect: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzrgb.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzrgb.rviz new file mode 100644 index 0000000000000000000000000000000000000000..daba8ef8e2396f4c3e11aff82fb4cca47531b22c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/point_cloud_xyzrgb.rviz @@ -0,0 +1,179 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /color_image1 + - /depth_image1 + - /registered_depth_image1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 392 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: color_image + Normalize Range: true + Queue Size: 10 + Topic: /camera/color/image_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth_image + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: registered_depth_image + Normalize Range: true + Queue Size: 10 + Topic: /camera/aligned_depth_to_color/image_raw + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth_registered/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_color_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.8979061841964722 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5597962141036987 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.718582630157471 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000213000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650100000256000000a20000002800fffffffb0000001600640065007000740068005f0069006d00610067006501000002fe000000a90000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d00610067006501000003ad000000af0000002800ffffff000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c30000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 + color_image: + collapsed: false + depth_image: + collapsed: false + registered_depth_image: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/register.rviz b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/register.rviz new file mode 100644 index 0000000000000000000000000000000000000000..6191de02637220276367f64ee2f88d32911afdd5 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/launch/rviz/register.rviz @@ -0,0 +1,148 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /depth/image_rect1 + - /depth_registered1 + - /color image1 + Splitter Ratio: 0.5 + Tree Height: 366 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth/image_rect + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth/image_rect_raw + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth_registered + Normalize Range: true + Queue Size: 10 + Topic: /camera/depth_registered/image_rect + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: color image + Normalize Range: true + Queue Size: 10 + Topic: /camera/color/image_raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_depth_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.125654935836792 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5597962141036987 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.723582744598389 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1145 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002740000041ffc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ab000000c900fffffffb0000002000640065007000740068002f0069006d006100670065005f007200650063007401000001ee000000c70000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb000000160063006f006c006f007200200069006d00610067006501000002bb000000cb0000002800fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb0000002000640065007000740068005f0072006500670069007300740065007200650064010000038c000000d00000002800ffffff000000010000010f0000041ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000041f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c30000041f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 + color image: + collapsed: false + depth/image_rect: + collapsed: false + depth_registered: + collapsed: false diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/mainpage.dox b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/mainpage.dox new file mode 100644 index 0000000000000000000000000000000000000000..6b0414bd074f1f1ff39b22ad0641681a036963a1 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b depth_image_proc is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/package.xml b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..a3910a2bd4c6e1c8270992548eb8bfe7c3ddfa68 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/package.xml @@ -0,0 +1,48 @@ + + + + depth_image_proc + 2.2.1 + + + Contains components for processing depth images such as those + produced by OpenNI camera. Functions include creating disparity + images and point clouds, as well as registering (reprojecting) + a depth image into another camera frame. + + + + Vincent Rabaud + Joshua Whitley + Chris Ye + Jacob Perron + + BSD + https://index.ros.org/p/depth_image_proc/github-ros-perception-image_pipeline/ + https://github.com/ros-perception/image_pipeline/issues + https://github.com/ros-perception/image_pipeline + Patrick Mihelich + + ament_cmake_auto + + message_filters + class_loader + + cv_bridge + image_geometry + image_transport + rclcpp + rclcpp_components + sensor_msgs + stereo_msgs + tf2 + tf2_eigen + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/conversions.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/conversions.cpp new file mode 100644 index 0000000000000000000000000000000000000000..da0e057b06ab200c131cb438b61f00e126249410 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/conversions.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include + +#include +#include + +namespace depth_image_proc +{ + +cv::Mat initMatrix( + cv::Mat cameraMatrix, cv::Mat distCoeffs, + int width, int height, bool radial) +{ + int i, j; + int totalsize = width * height; + cv::Mat pixelVectors(1, totalsize, CV_32FC3); + cv::Mat dst(1, totalsize, CV_32FC3); + + cv::Mat sensorPoints(cv::Size(height, width), CV_32FC2); + cv::Mat undistortedSensorPoints(1, totalsize, CV_32FC2); + + std::vector ch; + for (j = 0; j < height; j++) { + for (i = 0; i < width; i++) { + cv::Vec2f & p = sensorPoints.at(i, j); + p[0] = i; + p[1] = j; + } + } + + sensorPoints = sensorPoints.reshape(2, 1); + + cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs); + + ch.push_back(undistortedSensorPoints); + ch.push_back(cv::Mat::ones(1, totalsize, CV_32FC1)); + cv::merge(ch, pixelVectors); + + if (radial) { + for (i = 0; i < totalsize; i++) { + normalize( + pixelVectors.at(i), + dst.at(i)); + } + pixelVectors = dst; + } + return pixelVectors.reshape(3, width); +} + +void convertRgb( + const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg, + sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, + int red_offset, int green_offset, int blue_offset, int color_step) +{ + sensor_msgs::PointCloud2Iterator iter_r(*cloud_msg, "r"); + sensor_msgs::PointCloud2Iterator iter_g(*cloud_msg, "g"); + sensor_msgs::PointCloud2Iterator iter_b(*cloud_msg, "b"); + const uint8_t * rgb = &rgb_msg->data[0]; + int rgb_skip = rgb_msg->step - rgb_msg->width * color_step; + for (int v = 0; v < static_cast(cloud_msg->height); ++v, rgb += rgb_skip) { + for (int u = 0; u < static_cast(cloud_msg->width); ++u, + rgb += color_step, ++iter_r, ++iter_g, ++iter_b) + { + *iter_r = rgb[red_offset]; + *iter_g = rgb[green_offset]; + *iter_b = rgb[blue_offset]; + } + } +} +} // namespace depth_image_proc diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/convert_metric.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/convert_metric.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6ec473db01231db74ea7a6d989aedbf203783cbc --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/convert_metric.cpp @@ -0,0 +1,143 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class ConvertMetricNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC ConvertMetricNode(const rclcpp::NodeOptions & options); + +private: + // Subscriptions + image_transport::Subscriber sub_raw_; + + // Publications + std::mutex connect_mutex_; + image_transport::Publisher pub_depth_; + + void connectCb(); + + void depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("ConvertMetricNode"); +}; + +ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options) +: Node("ConvertMetricNode", options) +{ + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // image_transport::SubscriberStatusCallback connect_cb = + // std::bind(&ConvertMetricNode::connectCb, this); + connectCb(); + + // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); + pub_depth_ = image_transport::create_publisher(this, "image"); +} + +// Handles (un)subscribing when clients (un)subscribe +void ConvertMetricNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_depth_.getNumSubscribers() == 0) + if (0) { + sub_raw_.shutdown(); + } else if (!sub_raw_) { + image_transport::TransportHints hints(this, "raw"); + sub_raw_ = image_transport::create_subscription( + this, "image_raw", + std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1), + hints.getTransport()); + } +} + +void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) +{ + auto depth_msg = std::make_shared(); + depth_msg->header = raw_msg->header; + depth_msg->height = raw_msg->height; + depth_msg->width = raw_msg->width; + + // Set data, encoding and step after converting the metric. + if (raw_msg->encoding == enc::TYPE_16UC1) { + depth_msg->encoding = enc::TYPE_32FC1; + depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); + depth_msg->data.resize(depth_msg->height * depth_msg->step); + // Fill in the depth image data, converting mm to m + float bad_point = std::numeric_limits::quiet_NaN(); + const uint16_t * raw_data = reinterpret_cast(&raw_msg->data[0]); + float * depth_data = reinterpret_cast(&depth_msg->data[0]); + for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) { + uint16_t raw = raw_data[index]; + depth_data[index] = (raw == 0) ? bad_point : static_cast(raw * 0.001f); + } + } else if (raw_msg->encoding == enc::TYPE_32FC1) { + depth_msg->encoding = enc::TYPE_16UC1; + depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8); + depth_msg->data.resize(depth_msg->height * depth_msg->step); + // Fill in the depth image data, converting m to mm + uint16_t bad_point = 0; + const float * raw_data = reinterpret_cast(&raw_msg->data[0]); + uint16_t * depth_data = reinterpret_cast(&depth_msg->data[0]); + for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index) { + float raw = raw_data[index]; + depth_data[index] = std::isnan(raw) ? bad_point : static_cast(raw * 1000); + } + } else { + RCLCPP_ERROR(logger_, "Unsupported image conversion from %s.", raw_msg->encoding.c_str()); + return; + } + + pub_depth_.publish(depth_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::ConvertMetricNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/crop_foremost.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/crop_foremost.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e10fdad0814686e355aab735fecffdaa1b8a0423 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/crop_foremost.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +namespace enc = sensor_msgs::image_encodings; + +class CropForemostNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC CropForemostNode(const rclcpp::NodeOptions & options); + +private: + // Subscriptions + image_transport::Subscriber sub_raw_; + + // Publications + std::mutex connect_mutex_; + image_transport::Publisher pub_depth_; + + void connectCb(); + + void depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg); + + double distance_; + + rclcpp::Logger logger_ = rclcpp::get_logger("CropForemostNode"); +}; + +CropForemostNode::CropForemostNode(const rclcpp::NodeOptions & options) +: Node("CropForemostNode", options) +{ + distance_ = this->declare_parameter("distance", 0.0); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // image_transport::SubscriberStatusCallback connect_cb = + // std::bind(&CropForemostNode::connectCb, this); + connectCb(); + // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb); + pub_depth_ = image_transport::create_publisher(this, "image"); +} + +// Handles (un)subscribing when clients (un)subscribe +void CropForemostNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_depth_.getNumSubscribers() == 0) + if (0) { + sub_raw_.shutdown(); + } else if (!sub_raw_) { + image_transport::TransportHints hints(this, "raw"); + sub_raw_ = image_transport::create_subscription( + this, "image_raw", + std::bind(&CropForemostNode::depthCb, this, std::placeholders::_1), + hints.getTransport()); + } +} + +void CropForemostNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) +{ + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(raw_msg); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); + return; + } + + // Check the number of channels + if (sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1) { + RCLCPP_ERROR( + logger_, "Only grayscale image is acceptable, got [%s]", + raw_msg->encoding.c_str()); + return; + } + + // search the min value without invalid value "0" + double minVal; + cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0); + + int imtype = cv_bridge::getCvType(raw_msg->encoding); + switch (imtype) { + case CV_8UC1: + case CV_8SC1: + case CV_32F: + cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV); + break; + case CV_16UC1: + case CV_16SC1: + case CV_32SC1: + case CV_64F: + // 8 bit or 32 bit floating array is required to use cv::threshold + cv_ptr->image.convertTo(cv_ptr->image, CV_32F); + cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV); + + cv_ptr->image.convertTo(cv_ptr->image, imtype); + break; + } + + pub_depth_.publish(cv_ptr->toImageMsg()); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::CropForemostNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/disparity.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/disparity.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c87b428e8cb05de505d2134be9e74f2f7e812b4b --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/disparity.cpp @@ -0,0 +1,190 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +using namespace std::placeholders; +namespace enc = sensor_msgs::image_encodings; + +class DisparityNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC DisparityNode(const rclcpp::NodeOptions & options); + +private: + image_transport::SubscriberFilter sub_depth_image_; + message_filters::Subscriber sub_info_; + using Sync = message_filters::TimeSynchronizer; + std::shared_ptr sync_; + + std::mutex connect_mutex_; + using DisparityImage = stereo_msgs::msg::DisparityImage; + rclcpp::Publisher::SharedPtr pub_disparity_; + double min_range_; + double max_range_; + double delta_d_; + + void connectCb(); + + void depthCb( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg); + + template + void convert( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + stereo_msgs::msg::DisparityImage::SharedPtr & disp_msg); + + rclcpp::Logger logger_ = rclcpp::get_logger("DisparityNode"); +}; + +DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) +: Node("DisparityNode", options) +{ + // Read parameters + int queue_size = this->declare_parameter("queue_size", 5); + min_range_ = this->declare_parameter("min_range", 0.0); + max_range_ = this->declare_parameter( + "max_range", + std::numeric_limits::infinity()); + delta_d_ = this->declare_parameter("delta_d", 0.125); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_ = std::make_shared(sub_depth_image_, sub_info_, queue_size); + sync_->registerCallback(std::bind(&DisparityNode::depthCb, this, _1, _2)); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = std::bind(&DisparityNode::connectCb, this); + connectCb(); + + // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_disparity_ = + // left_nh.advertise("disparity", 1, connect_cb, connect_cb); + pub_disparity_ = create_publisher( + "left/disparity", rclcpp::SensorDataQoS()); +} + +// Handles (un)subscribing when clients (un)subscribe +void DisparityNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_disparity_.getNumSubscribers() == 0) + if (0) { + sub_depth_image_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_image_.getSubscriber()) { + image_transport::TransportHints hints(this, "raw"); + sub_depth_image_.subscribe(this, "left/image_rect", hints.getTransport()); + sub_info_.subscribe(this, "right/camera_info"); + } +} + +void DisparityNode::depthCb( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) +{ + auto disp_msg = std::make_shared(); + disp_msg->header = depth_msg->header; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = enc::TYPE_32FC1; + disp_msg->image.height = depth_msg->height; + disp_msg->image.width = depth_msg->width; + disp_msg->image.step = disp_msg->image.width * sizeof(float); + disp_msg->image.data.resize(disp_msg->image.height * disp_msg->image.step, 0.0f); + double fx = info_msg->p[0]; + disp_msg->t = -info_msg->p[3] / fx; + disp_msg->f = fx; + // Remaining fields depend on device characteristics, so rely on user input + disp_msg->min_disparity = disp_msg->f * disp_msg->t / max_range_; + disp_msg->max_disparity = disp_msg->f * disp_msg->t / min_range_; + disp_msg->delta_d = delta_d_; + + if (depth_msg->encoding == enc::TYPE_16UC1) { + convert(depth_msg, disp_msg); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convert(depth_msg, disp_msg); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + pub_disparity_->publish(*disp_msg); +} + +template +void DisparityNode::convert( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + stereo_msgs::msg::DisparityImage::SharedPtr & disp_msg) +{ + // For each depth Z, disparity d = fT / Z + float unit_scaling = DepthTraits::toMeters(T(1) ); + float constant = disp_msg->f * disp_msg->t / unit_scaling; + + const T * depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + float * disp_data = reinterpret_cast(&disp_msg->image.data[0]); + for (int v = 0; v < static_cast(depth_msg->height); ++v) { + for (int u = 0; u < static_cast(depth_msg->width); ++u) { + T depth = depth_row[u]; + if (DepthTraits::valid(depth)) { + *disp_data = constant / depth; + } + ++disp_data; + } + + depth_row += row_step; + } +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::DisparityNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bc7a76b7221f035536be09c066e25a5c007f10aa --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyz.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace depth_image_proc +{ + +PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyzNode", options) +{ + // Read parameters + queue_size_ = this->declare_parameter("queue_size", 5); + output_frame = this->declare_parameter("output_frame", "base_link"); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzNode::connectCb, this); + connectCb(); + + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_point_cloud_ = nh.advertise("points", 1, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyzNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_->getNumSubscribers() == 0) + if (0) { + sub_depth_.shutdown(); + } else if (!sub_depth_) { + auto custom_qos = rmw_qos_profile_sensor_data; + custom_qos.depth = queue_size_; + + sub_depth_ = image_transport::create_camera_subscription( + this, + "image_rect", + std::bind(&PointCloudXyzNode::depthCb, this, std::placeholders::_1, std::placeholders::_2), + "raw", + custom_qos); + } +} + +void PointCloudXyzNode::depthCb( + const Image::ConstSharedPtr & depth_msg, + const CameraInfo::ConstSharedPtr & info_msg) +{ + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; + cloud_msg->header.frame_id = output_frame; + // printf("output_frame is %s\n", output_frame.c_str()); + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Update camera model + model_.fromCameraInfo(info_msg); + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + // RCLCPP_ERROR(logger_, "depthCb 1"); + convertDepth(depth_msg, cloud_msg, model_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + // RCLCPP_ERROR(logger_, "depthCb 2"); + convertDepth(depth_msg, cloud_msg, model_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + // RCLCPP_ERROR(logger_, "depthCb 3"); + + pub_point_cloud_->publish(*cloud_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyzNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz_radial.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz_radial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8ab6245743ddb1b6484d0d528a908e5b5c303b9c --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyz_radial.cpp @@ -0,0 +1,134 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyz_radial.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace depth_image_proc +{ + + +PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyzRadialNode", options) +{ + // Read parameters + queue_size_ = this->declare_parameter("queue_size", 5); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = + // boost::bind(&PointCloudXyzRadialNode::connectCb, this); + connectCb(); + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_point_cloud_ = nh.advertise("points", 1, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher( + "points", rclcpp::SensorDataQoS()); +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyzRadialNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_.getNumSubscribers() == 0) + if (0) { + sub_depth_.shutdown(); + } else if (!sub_depth_) { + auto custom_qos = rmw_qos_profile_system_default; + custom_qos.depth = queue_size_; + + sub_depth_ = image_transport::create_camera_subscription( + this, + "image_raw", + std::bind( + &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1, + std::placeholders::_2), + "raw", + custom_qos); + } +} + +void PointCloudXyzRadialNode::depthCb( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) +{ + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); + + if (info_msg->d != D_ || info_msg->k != K_ || width_ != info_msg->width || + height_ != info_msg->height) + { + D_ = info_msg->d; + K_ = info_msg->k; + width_ = info_msg->width; + height_ = info_msg->height; + transform_ = initMatrix(cv::Mat_(3, 3, &K_[0]), cv::Mat(D_), width_, height_, true); + } + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + pub_point_cloud_->publish(*cloud_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyzRadialNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi.cpp new file mode 100644 index 0000000000000000000000000000000000000000..92450e71301de0b8b1b225389a6713337b0a6c69 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi.cpp @@ -0,0 +1,234 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyzi.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace depth_image_proc +{ + + +PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyziNode", options) +{ + // Read parameters + int queue_size = this->declare_parameter("queue_size", 5); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_ = std::make_shared( + SyncPolicy(queue_size), + sub_depth_, + sub_intensity_, + sub_info_); + sync_->registerCallback( + std::bind( + &PointCloudXyziNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyziNode::connectCb, this); + connectCb(); + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyziNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_->getNumSubscribers() == 0) + if (0) { + sub_depth_.unsubscribe(); + sub_intensity_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_.getSubscriber()) { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + + // depth image can use different transport.(e.g. compressedDepth) + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport()); + + // intensity uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport()); + sub_info_.subscribe(this, "intensity/camera_info"); + } +} + +void PointCloudXyziNode::imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg_in, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) +{ + // Check for bad inputs + if (depth_msg->header.frame_id != intensity_msg_in->header.frame_id) { + RCLCPP_WARN_THROTTLE( + logger_, + *get_clock(), + 10000, // 10 seconds + "Depth image frame id [%s] doesn't match image frame id [%s]", + depth_msg->header.frame_id.c_str(), intensity_msg_in->header.frame_id.c_str()); + } + + // Update camera model + model_.fromCameraInfo(info_msg); + + // Check if the input image has to be resized + sensor_msgs::msg::Image::ConstSharedPtr intensity_msg = intensity_msg_in; + if (depth_msg->width != intensity_msg->width || depth_msg->height != intensity_msg->height) { + sensor_msgs::msg::CameraInfo info_msg_tmp = *info_msg; + info_msg_tmp.width = depth_msg->width; + info_msg_tmp.height = depth_msg->height; + float ratio = static_cast(depth_msg->width) / static_cast(intensity_msg->width); + info_msg_tmp.k[0] *= ratio; + info_msg_tmp.k[2] *= ratio; + info_msg_tmp.k[4] *= ratio; + info_msg_tmp.k[5] *= ratio; + info_msg_tmp.p[0] *= ratio; + info_msg_tmp.p[2] *= ratio; + info_msg_tmp.p[5] *= ratio; + info_msg_tmp.p[6] *= ratio; + model_.fromCameraInfo(info_msg_tmp); + + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(intensity_msg, intensity_msg->encoding); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); + return; + } + cv_bridge::CvImage cv_rsz; + cv_rsz.header = cv_ptr->header; + cv_rsz.encoding = cv_ptr->encoding; + cv::resize( + cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image, + cv::Size(depth_msg->width, depth_msg->height)); + if ((intensity_msg->encoding == enc::MONO8) || (intensity_msg->encoding == enc::MONO16)) { + intensity_msg = cv_rsz.toImageMsg(); + } else { + intensity_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::MONO8)->toImageMsg(); + } + + // RCLCPP_ERROR(logger_, "Depth resolution (%ux%u) does not match resolution (%ux%u)", + // depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height); + // return; + } else { + intensity_msg = intensity_msg_in; + } + + // Supported color encodings: MONO8, MONO16 + if (intensity_msg->encoding != enc::MONO8 || intensity_msg->encoding != enc::MONO16) { + try { + intensity_msg = cv_bridge::toCvCopy(intensity_msg, enc::MONO8)->toImageMsg(); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR( + logger_, "Unsupported encoding [%s]: %s", + intensity_msg->encoding.c_str(), e.what()); + return; + } + } + + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; // Use depth image time stamp + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + // pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "i"); + pcd_modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + convertDepth(depth_msg, cloud_msg, model_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convertDepth(depth_msg, cloud_msg, model_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + // Convert Intensity Image to Pointcloud + if (intensity_msg->encoding == enc::MONO8) { + convertIntensity(intensity_msg, cloud_msg); + } else if (intensity_msg->encoding == enc::MONO16) { + convertIntensity(intensity_msg, cloud_msg); + } else if (intensity_msg->encoding == enc::TYPE_16UC1) { + convertIntensity(intensity_msg, cloud_msg); + } else { + RCLCPP_ERROR( + logger_, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str()); + return; + } + + pub_point_cloud_->publish(*cloud_msg); +} + + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyziNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi_radial.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi_radial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fa6f446a83885ea845548f64b6f88616b89d0c72 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzi_radial.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyzi_radial.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace depth_image_proc +{ + + +PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyziRadialNode", options) +{ + // Read parameters + queue_size_ = this->declare_parameter("queue_size", 5); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_ = std::make_shared( + SyncPolicy(queue_size_), + sub_depth_, + sub_intensity_, + sub_info_); + sync_->registerCallback( + std::bind( + &PointCloudXyziRadialNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = + // boost::bind(&PointCloudXyziRadialNode::connectCb, this); + connectCb(); + + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // pub_point_cloud_ = nh.advertise("points", 20, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher( + "points", rclcpp::SensorDataQoS()); +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyziRadialNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_.getNumSubscribers() == 0) + if (0) { + sub_depth_.unsubscribe(); + sub_intensity_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_.getSubscriber()) { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + + // depth image can use different transport.(e.g. compressedDepth) + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport()); + + // intensity uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport()); + sub_info_.subscribe(this, "intensity/camera_info"); + } +} + +void PointCloudXyziRadialNode::imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & intensity_msg, + const CameraInfo::ConstSharedPtr & info_msg) +{ + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + + + if (info_msg->d != D_ || info_msg->k != K_ || width_ != info_msg->width || + height_ != info_msg->height) + { + D_ = info_msg->d; + K_ = info_msg->k; + width_ = info_msg->width; + height_ = info_msg->height; + transform_ = initMatrix(cv::Mat_(3, 3, &K_[0]), cv::Mat(D_), width_, height_, true); + } + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + if (intensity_msg->encoding == enc::MONO8) { + convertIntensity(intensity_msg, cloud_msg); + } else if (intensity_msg->encoding == enc::MONO16) { + convertIntensity(intensity_msg, cloud_msg); + } else if (intensity_msg->encoding == enc::TYPE_16UC1) { + convertIntensity(intensity_msg, cloud_msg); + } else { + RCLCPP_ERROR( + logger_, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str()); + return; + } + + pub_point_cloud_->publish(*cloud_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyziRadialNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6edcf34bdadaa127184eaaedf6a165f0100b619a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb.cpp @@ -0,0 +1,264 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyzrgb.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace depth_image_proc +{ + + +PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyzrgbNode", options) +{ + // Read parameters + int queue_size = this->declare_parameter("queue_size", 5); + bool use_exact_sync = this->declare_parameter("exact_sync", false); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + if (use_exact_sync) { + exact_sync_ = std::make_shared( + ExactSyncPolicy(queue_size), + sub_depth_, + sub_rgb_, + sub_info_); + exact_sync_->registerCallback( + std::bind( + &PointCloudXyzrgbNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + } else { + sync_ = std::make_shared(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_); + sync_->registerCallback( + std::bind( + &PointCloudXyzrgbNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + } + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNode::connectCb, this); + connectCb(); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available + // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); + // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyzrgbNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_->getNumSubscribers() == 0) + if (0) { + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + sub_depth_.unsubscribe(); + sub_rgb_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_.getSubscriber()) { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + + // depth image can use different transport.(e.g. compressedDepth) + sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); + + // rgb uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); + sub_info_.subscribe(this, "rgb/camera_info"); + } +} + +void PointCloudXyzrgbNode::imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & rgb_msg_in, + const CameraInfo::ConstSharedPtr & info_msg) +{ + // Check for bad inputs + if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id) { + RCLCPP_WARN_THROTTLE( + logger_, + *get_clock(), + 10000, // 10 seconds + "Depth image frame id [%s] doesn't match RGB image frame id [%s]", + depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str()); + } + + // Update camera model + model_.fromCameraInfo(info_msg); + + // Check if the input image has to be resized + Image::ConstSharedPtr rgb_msg = rgb_msg_in; + if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height) { + CameraInfo info_msg_tmp = *info_msg; + info_msg_tmp.width = depth_msg->width; + info_msg_tmp.height = depth_msg->height; + float ratio = static_cast(depth_msg->width) / static_cast(rgb_msg->width); + info_msg_tmp.k[0] *= ratio; + info_msg_tmp.k[2] *= ratio; + info_msg_tmp.k[4] *= ratio; + info_msg_tmp.k[5] *= ratio; + info_msg_tmp.p[0] *= ratio; + info_msg_tmp.p[2] *= ratio; + info_msg_tmp.p[5] *= ratio; + info_msg_tmp.p[6] *= ratio; + model_.fromCameraInfo(info_msg_tmp); + + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); + return; + } + cv_bridge::CvImage cv_rsz; + cv_rsz.header = cv_ptr->header; + cv_rsz.encoding = cv_ptr->encoding; + cv::resize( + cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image, + cv::Size(depth_msg->width, depth_msg->height)); + if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || + (rgb_msg->encoding == enc::MONO8)) + { + rgb_msg = cv_rsz.toImageMsg(); + } else { + rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg(); + } + + RCLCPP_ERROR( + logger_, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)", + depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height); + return; + } else { + rgb_msg = rgb_msg_in; + } + + // Supported color encodings: RGB8, BGR8, MONO8 + int red_offset, green_offset, blue_offset, color_step; + if (rgb_msg->encoding == enc::RGB8) { + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 3; + } else if (rgb_msg->encoding == enc::BGR8) { + red_offset = 2; + green_offset = 1; + blue_offset = 0; + color_step = 3; + } else if (rgb_msg->encoding == enc::MONO8) { + red_offset = 0; + green_offset = 0; + blue_offset = 0; + color_step = 1; + } else { + try { + rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg(); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what()); + return; + } + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 3; + } + + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; // Use depth image time stamp + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + convertDepth(depth_msg, cloud_msg, model_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convertDepth(depth_msg, cloud_msg, model_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + // Convert RGB + if (rgb_msg->encoding == enc::RGB8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else if (rgb_msg->encoding == enc::BGR8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else if (rgb_msg->encoding == enc::MONO8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else { + RCLCPP_ERROR(logger_, "RGB image has unsupported encoding [%s]", rgb_msg->encoding.c_str()); + return; + } + + pub_point_cloud_->publish(*cloud_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyzrgbNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb_radial.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb_radial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..be1d536361b3ca3d518fbc41b0a8c792f1adec09 --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/point_cloud_xyzrgb_radial.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include "depth_image_proc/point_cloud_xyzrgb_radial.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace depth_image_proc +{ + + +PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions & options) +: Node("PointCloudXyzrgbRadialNode", options) +{ + // Read parameters + int queue_size = this->declare_parameter("queue_size", 5); + bool use_exact_sync = this->declare_parameter("exact_sync", false); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + if (use_exact_sync) { + exact_sync_ = std::make_unique( + ExactSyncPolicy(queue_size), + sub_depth_, + sub_rgb_, + sub_info_); + exact_sync_->registerCallback( + std::bind( + &PointCloudXyzrgbRadialNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + } else { + sync_ = + std::make_unique(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_); + sync_->registerCallback( + std::bind( + &PointCloudXyzrgbRadialNode::imageCb, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + } + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // ros::SubscriberStatusCallback connect_cb = + // boost::bind(&PointCloudXyzrgbRadialNode::connectCb, this); + connectCb(); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_ + // std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available + // pub_point_cloud_ = depth_nh.advertise("points", 1, connect_cb, connect_cb); + pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS()); + // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudXyzrgbRadialNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_->getNumSubscribers() == 0) + if (0) { + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + sub_depth_.unsubscribe(); + sub_rgb_.unsubscribe(); + sub_info_.unsubscribe(); + } else if (!sub_depth_.getSubscriber()) { + // parameter for depth_image_transport hint + std::string depth_image_transport_param = "depth_image_transport"; + image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); + + // depth image can use different transport.(e.g. compressedDepth) + sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport()); + + // rgb uses normal ros transport hints. + image_transport::TransportHints hints(this, "raw"); + sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport()); + sub_info_.subscribe(this, "rgb/camera_info"); + } +} + +void PointCloudXyzrgbRadialNode::imageCb( + const Image::ConstSharedPtr & depth_msg, + const Image::ConstSharedPtr & rgb_msg_in, + const CameraInfo::ConstSharedPtr & info_msg) +{ + // Check for bad inputs + if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id) { + RCLCPP_WARN( + logger_, "Depth image frame id [%s] doesn't match RGB image frame id [%s]", + depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str()); + return; + } + + // Update camera model + model_.fromCameraInfo(info_msg); + + // Check if the input image has to be resized + Image::ConstSharedPtr rgb_msg = rgb_msg_in; + if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height) { + CameraInfo info_msg_tmp = *info_msg; + info_msg_tmp.width = depth_msg->width; + info_msg_tmp.height = depth_msg->height; + float ratio = static_cast(depth_msg->width) / static_cast(rgb_msg->width); + info_msg_tmp.k[0] *= ratio; + info_msg_tmp.k[2] *= ratio; + info_msg_tmp.k[4] *= ratio; + info_msg_tmp.k[5] *= ratio; + info_msg_tmp.p[0] *= ratio; + info_msg_tmp.p[2] *= ratio; + info_msg_tmp.p[5] *= ratio; + info_msg_tmp.p[6] *= ratio; + model_.fromCameraInfo(info_msg_tmp); + + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); + return; + } + cv_bridge::CvImage cv_rsz; + cv_rsz.header = cv_ptr->header; + cv_rsz.encoding = cv_ptr->encoding; + cv::resize( + cv_ptr->image.rowRange(0, depth_msg->height / ratio), cv_rsz.image, + cv::Size(depth_msg->width, depth_msg->height)); + if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || + (rgb_msg->encoding == enc::MONO8)) + { + rgb_msg = cv_rsz.toImageMsg(); + } else { + rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg(); + } + + RCLCPP_ERROR( + logger_, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)", + depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height); + return; + } else { + rgb_msg = rgb_msg_in; + } + + if (info_msg->d != D_ || info_msg->k != K_ || width_ != info_msg->width || + height_ != info_msg->height) + { + D_ = info_msg->d; + K_ = info_msg->k; + width_ = info_msg->width; + height_ = info_msg->height; + transform_ = initMatrix(cv::Mat_(3, 3, &K_[0]), cv::Mat(D_), width_, height_, true); + } + + // Supported color encodings: RGB8, BGR8, MONO8 + int red_offset, green_offset, blue_offset, color_step; + if (rgb_msg->encoding == enc::RGB8) { + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 3; + } else if (rgb_msg->encoding == enc::BGR8) { + red_offset = 2; + green_offset = 1; + blue_offset = 0; + color_step = 3; + } else if (rgb_msg->encoding == enc::MONO8) { + red_offset = 0; + green_offset = 0; + blue_offset = 0; + color_step = 1; + } else { + try { + rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg(); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(logger_, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what()); + return; + } + red_offset = 0; + green_offset = 1; + blue_offset = 2; + color_step = 3; + } + + auto cloud_msg = std::make_shared(); + cloud_msg->header = depth_msg->header; // Use depth image time stamp + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + + // Convert Depth Image to Pointcloud + if (depth_msg->encoding == enc::TYPE_16UC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convertDepthRadial(depth_msg, cloud_msg, transform_); + } else { + RCLCPP_ERROR(logger_, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + + // Convert RGB + if (rgb_msg->encoding == enc::RGB8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else if (rgb_msg->encoding == enc::BGR8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else if (rgb_msg->encoding == enc::MONO8) { + convertRgb(rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step); + } else { + RCLCPP_ERROR(logger_, "RGB image has unsupported encoding [%s]", rgb_msg->encoding.c_str()); + return; + } + + pub_point_cloud_->publish(*cloud_msg); +} + +} // namespace depth_image_proc + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +RCLCPP_COMPONENTS_REGISTER_NODE(depth_image_proc::PointCloudXyzrgbRadialNode) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/register.cpp b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/register.cpp new file mode 100644 index 0000000000000000000000000000000000000000..95105228ee138296e34c3b2e934d0f9b182dda1a --- /dev/null +++ b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/depth_image/depth_image_to_point_cloud/src/register.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 the Willow Garage 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. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace depth_image_proc +{ + +using namespace std::placeholders; +namespace enc = sensor_msgs::image_encodings; + +class RegisterNode : public rclcpp::Node +{ +public: + DEPTH_IMAGE_PROC_PUBLIC RegisterNode(); + +private: + using Image = sensor_msgs::msg::Image; + using CameraInfo = sensor_msgs::msg::CameraInfo; + + // Subscriptions + image_transport::SubscriberFilter sub_depth_image_; + message_filters::Subscriber sub_depth_info_, sub_rgb_info_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_; + using SyncPolicy = message_filters::sync_policies::ApproximateTime< + Image, CameraInfo, + CameraInfo>; + using Synchronizer = message_filters::Synchronizer; + std::shared_ptr sync_; + + // Publications + std::mutex connect_mutex_; + image_transport::CameraPublisher pub_registered_; + + image_geometry::PinholeCameraModel depth_model_, rgb_model_; + + void connectCb(); + + void imageCb( + const Image::ConstSharedPtr & depth_image_msg, + const CameraInfo::ConstSharedPtr & depth_info_msg, + const CameraInfo::ConstSharedPtr & rgb_info_msg); + + template + void convert( + const Image::ConstSharedPtr & depth_msg, + const Image::SharedPtr & registered_msg, + const Eigen::Affine3d & depth_to_rgb); + + rclcpp::Logger logger_ = rclcpp::get_logger("RegisterNode"); +}; + +RegisterNode::RegisterNode() +: Node("RegisterNode") +{ + rclcpp::Clock::SharedPtr clock = this->get_clock(); + tf_buffer_ = std::make_shared(clock); + tf_ = std::make_shared(*tf_buffer_); + + // Read parameters + int queue_size = this->declare_parameter("queue_size", 5); + + // Synchronize inputs. Topic subscriptions happen on demand in the connection callback. + sync_ = std::make_shared( + SyncPolicy(queue_size), + sub_depth_image_, + sub_depth_info_, + sub_rgb_info_); + sync_->registerCallback(std::bind(&RegisterNode::imageCb, this, _1, _2, _3)); + + // Monitor whether anyone is subscribed to the output + // TODO(ros2) Implement when SubscriberStatusCallback is available + // image_transport::SubscriberStatusCallback image_connect_cb + // boost::bind(&RegisterNode::connectCb, this); + // ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNode::connectCb, this); + connectCb(); + // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_ + std::lock_guard lock(connect_mutex_); + // pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1, + // image_connect_cb, image_connect_cb, + // info_connect_cb, info_connect_cb); + pub_registered_ = image_transport::create_camera_publisher(this, "depth_registered/image_rect"); +} + +// Handles (un)subscribing when clients (un)subscribe +void RegisterNode::connectCb() +{ + std::lock_guard lock(connect_mutex_); + // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it + // if (pub_point_cloud_->getNumSubscribers() == 0) + if (0) { + sub_depth_image_.unsubscribe(); + sub_depth_info_.unsubscribe(); + sub_rgb_info_.unsubscribe(); + } else if (!sub_depth_image_.getSubscriber()) { + image_transport::TransportHints hints(this, "raw"); + sub_depth_image_.subscribe(this, "depth/image_rect", hints.getTransport()); + sub_depth_info_.subscribe(this, "depth/camera_info"); + sub_rgb_info_.subscribe(this, "rgb/camera_info"); + } +} + +void RegisterNode::imageCb( + const Image::ConstSharedPtr & depth_image_msg, + const CameraInfo::ConstSharedPtr & depth_info_msg, + const CameraInfo::ConstSharedPtr & rgb_info_msg) +{ + // Update camera models - these take binning & ROI into account + depth_model_.fromCameraInfo(depth_info_msg); + rgb_model_.fromCameraInfo(rgb_info_msg); + + // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame + Eigen::Affine3d depth_to_rgb; + try { + tf2::TimePoint tf2_time(std::chrono::nanoseconds(depth_info_msg->header.stamp.nanosec) + + std::chrono::duration_cast( + std::chrono::seconds( + depth_info_msg->header.stamp.sec))); + geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( + rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id, + tf2_time); + + depth_to_rgb = tf2::transformToEigen(transform); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "TF2 exception:\n%s", ex.what()); + return; + /// @todo Can take on order of a minute to register a disconnect callback when we + /// don't call publish() in this cb. What's going on roscpp? + } + + auto registered_msg = std::make_shared(); + registered_msg->header.stamp = depth_image_msg->header.stamp; + registered_msg->header.frame_id = rgb_info_msg->header.frame_id; + registered_msg->encoding = depth_image_msg->encoding; + + cv::Size resolution = rgb_model_.reducedResolution(); + registered_msg->height = resolution.height; + registered_msg->width = resolution.width; + // step and data set in convert(), depend on depth data type + + if (depth_image_msg->encoding == enc::TYPE_16UC1) { + convert(depth_image_msg, registered_msg, depth_to_rgb); + } else if (depth_image_msg->encoding == enc::TYPE_32FC1) { + convert(depth_image_msg, registered_msg, depth_to_rgb); + } else { + RCLCPP_ERROR( + logger_, "Depth image has unsupported encoding [%s]", + depth_image_msg->encoding.c_str()); + return; + } + + // Registered camera info is the same as the RGB info, but uses the depth timestamp + auto registered_info_msg = std::make_shared(*rgb_info_msg); + registered_info_msg->header.stamp = registered_msg->header.stamp; + + pub_registered_.publish(registered_msg, registered_info_msg); +} + +template +void RegisterNode::convert( + const Image::ConstSharedPtr & depth_msg, + const Image::SharedPtr & registered_msg, + const Eigen::Affine3d & depth_to_rgb) +{ + // Allocate memory for registered depth image + registered_msg->step = registered_msg->width * sizeof(T); + registered_msg->data.resize(registered_msg->height * registered_msg->step); + // data is already zero-filled in the uint16 case, + // but for floats we want to initialize everything to NaN. + DepthTraits::initializeBuffer(registered_msg->data); + + // Extract all the parameters we need + double inv_depth_fx = 1.0 / depth_model_.fx(); + double inv_depth_fy = 1.0 / depth_model_.fy(); + double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy(); + double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty(); + double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy(); + double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy(); + double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty(); + + // Transform the depth values into the RGB frame + /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the + // registered image + const T * depth_row = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + T * registered_data = reinterpret_cast(®istered_msg->data[0]); + int raw_index = 0; + for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step) { + for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index) { + T raw_depth = depth_row[u]; + if (!DepthTraits::valid(raw_depth)) { + continue; + } + + double depth = DepthTraits::toMeters(raw_depth); + + /// @todo Combine all operations into one matrix multiply on (u,v,d) + // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame + Eigen::Vector4d xyz_depth; + xyz_depth << ((u - depth_cx) * depth - depth_Tx) * inv_depth_fx, + ((v - depth_cy) * depth - depth_Ty) * inv_depth_fy, + depth, + 1; + + // Transform to RGB camera frame + Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth; + + // Project to (u,v) in RGB image + double inv_Z = 1.0 / xyz_rgb.z(); + int u_rgb = (rgb_fx * xyz_rgb.x() + rgb_Tx) * inv_Z + rgb_cx + 0.5; + int v_rgb = (rgb_fy * xyz_rgb.y() + rgb_Ty) * inv_Z + rgb_cy + 0.5; + + if (u_rgb < 0 || u_rgb >= static_cast(registered_msg->width) || + v_rgb < 0 || v_rgb >= static_cast(registered_msg->height)) + { + continue; + } + + T & reg_depth = registered_data[v_rgb * registered_msg->width + u_rgb]; + T new_depth = DepthTraits::fromMeters(xyz_rgb.z()); + // Validity and Z-buffer checks + if (!DepthTraits::valid(reg_depth) || reg_depth > new_depth) { + reg_depth = new_depth; + } + } + } +} + +} // namespace depth_image_proc + +#include "class_loader/register_macro.hpp" + +// Register the component with class_loader. +CLASS_LOADER_REGISTER_CLASS(depth_image_proc::RegisterNode, rclcpp::Node) diff --git a/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/dtof_client_node/.keep b/hi-pilot/ws_eulercar/src/sensing/dtof_sensor/src/dtof_client_node/.keep new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391