diff --git a/.gitignore b/.gitignore index d7c3a849..80319fbd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ .vscode build -package.xml __pycache__ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 99a8cccf..6ea1a4fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -222,8 +222,10 @@ else(ROS_EDITION STREQUAL "ROS2") # further dependencies manually. # find_package( REQUIRED) find_package(ament_cmake_auto REQUIRED) + find_package(livox_sdk2 REQUIRED) ament_auto_find_build_dependencies() find_package(PCL REQUIRED) + find_package(pcl_conversions REQUIRED) find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -246,7 +248,7 @@ else(ROS_EDITION STREQUAL "ROS2") ) ## make sure the livox_lidar_sdk_shared library is installed - find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED) + find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so REQUIRED) ## find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR @@ -282,15 +284,11 @@ else(ROS_EDITION STREQUAL "ROS2") target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS}) # get include directories of custom msg headers - if(HUMBLE_ROS STREQUAL "humble") - rosidl_get_typesupport_target(cpp_typesupport_target - ${LIVOX_INTERFACES} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") - else() - set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp") - add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES}) - get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES) - endif() + + rosidl_get_typesupport_target(cpp_typesupport_target + ${LIVOX_INTERFACES} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") + # include file direcotry target_include_directories(${PROJECT_NAME} PUBLIC @@ -330,7 +328,7 @@ else(ROS_EDITION STREQUAL "ROS2") ament_auto_package(INSTALL_TO_SHARE config - launch_ROS2 + launch ) endif() \ No newline at end of file diff --git a/config/MID360_config.json b/config/MID360_config.json index e3df5d17..82ac4a82 100644 --- a/config/MID360_config.json +++ b/config/MID360_config.json @@ -1,34 +1,34 @@ { - "lidar_summary_info" : { + "lidar_summary_info": { "lidar_type": 8 }, "MID360": { - "lidar_net_info" : { + "lidar_net_info": { "cmd_data_port": 56100, "push_msg_port": 56200, "point_data_port": 56300, "imu_data_port": 56400, "log_data_port": 56500 }, - "host_net_info" : { - "cmd_data_ip" : "192.168.1.5", + "host_net_info": { + "cmd_data_ip": "192.168.1.50", "cmd_data_port": 56101, - "push_msg_ip": "192.168.1.5", + "push_msg_ip": "192.168.1.50", "push_msg_port": 56201, - "point_data_ip": "192.168.1.5", + "point_data_ip": "192.168.1.50", "point_data_port": 56301, - "imu_data_ip" : "192.168.1.5", + "imu_data_ip": "192.168.1.50", "imu_data_port": 56401, - "log_data_ip" : "", + "log_data_ip": "", "log_data_port": 56501 } }, - "lidar_configs" : [ + "lidar_configs": [ { - "ip" : "192.168.1.12", - "pcl_data_type" : 1, - "pattern_mode" : 0, - "extrinsic_parameter" : { + "ip": "192.168.1.175", + "pcl_data_type": 1, + "pattern_mode": 0, + "extrinsic_parameter": { "roll": 0.0, "pitch": 0.0, "yaw": 0.0, @@ -38,5 +38,4 @@ } } ] -} - +} \ No newline at end of file diff --git a/launch/livox_mid360_driver.launch.py b/launch/livox_mid360_driver.launch.py new file mode 100644 index 00000000..4be4b3d3 --- /dev/null +++ b/launch/livox_mid360_driver.launch.py @@ -0,0 +1,47 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +config_path = os.path.join(get_package_share_directory("livox_ros_driver2"), "config") +user_config_path = os.path.join(config_path, "MID360_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + return LaunchDescription( + [ + livox_driver, + ] + ) diff --git a/launch/msg_HAP_launch.py b/launch/msg_HAP_launch.py new file mode 100644 index 00000000..0ec4695c --- /dev/null +++ b/launch/msg_HAP_launch.py @@ -0,0 +1,57 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +rviz_config_path = os.path.join(cur_config_path, "livox_lidar.rviz") +user_config_path = os.path.join(cur_config_path, "HAP_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + return LaunchDescription( + [ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/launch/msg_MID360_launch.py b/launch/msg_MID360_launch.py new file mode 100644 index 00000000..c4dc54c9 --- /dev/null +++ b/launch/msg_MID360_launch.py @@ -0,0 +1,56 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +user_config_path = os.path.join(cur_config_path, "MID360_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + return LaunchDescription( + [ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/launch/rviz_HAP_launch.py b/launch/rviz_HAP_launch.py new file mode 100644 index 00000000..7b3f6b5a --- /dev/null +++ b/launch/rviz_HAP_launch.py @@ -0,0 +1,65 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +rviz_config_path = os.path.join(cur_config_path, "display_point_cloud_ROS2.rviz") +user_config_path = os.path.join(cur_config_path, "HAP_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + livox_rviz = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=["--display-config", rviz_config_path], + ) + + return LaunchDescription( + [ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/launch/rviz_MID360_launch.py b/launch/rviz_MID360_launch.py new file mode 100644 index 00000000..9ce5020c --- /dev/null +++ b/launch/rviz_MID360_launch.py @@ -0,0 +1,65 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +rviz_config_path = os.path.join(cur_config_path, "display_point_cloud_ROS2.rviz") +user_config_path = os.path.join(cur_config_path, "MID360_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + livox_rviz = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=["--display-config", rviz_config_path], + ) + + return LaunchDescription( + [ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/launch/rviz_mixed.py b/launch/rviz_mixed.py new file mode 100644 index 00000000..558876a6 --- /dev/null +++ b/launch/rviz_mixed.py @@ -0,0 +1,65 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +import launch + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +rviz_config_path = os.path.join(cur_config_path, "display_point_cloud_ROS2.rviz") +user_config_path = os.path.join(cur_config_path, "mixed_HAP_MID360_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + livox_rviz = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=["--display-config", rviz_config_path], + ) + + return LaunchDescription( + [ + livox_driver, + livox_rviz, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/launch_ROS1/msg_HAP.launch b/launch_ROS1/msg_HAP.launch deleted file mode 100644 index 005e038e..00000000 --- a/launch_ROS1/msg_HAP.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/msg_MID360.launch b/launch_ROS1/msg_MID360.launch deleted file mode 100644 index 1e087455..00000000 --- a/launch_ROS1/msg_MID360.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/msg_mixed.launch b/launch_ROS1/msg_mixed.launch deleted file mode 100644 index bd4fe79a..00000000 --- a/launch_ROS1/msg_mixed.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_HAP.launch b/launch_ROS1/rviz_HAP.launch deleted file mode 100644 index 572098ae..00000000 --- a/launch_ROS1/rviz_HAP.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_MID360.launch b/launch_ROS1/rviz_MID360.launch deleted file mode 100644 index 44c8bff7..00000000 --- a/launch_ROS1/rviz_MID360.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_mixed.launch b/launch_ROS1/rviz_mixed.launch deleted file mode 100644 index d1851e82..00000000 --- a/launch_ROS1/rviz_mixed.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS2/msg_HAP_launch.py b/launch_ROS2/msg_HAP_launch.py deleted file mode 100644 index 2ff3d66a..00000000 --- a/launch_ROS2/msg_HAP_launch.py +++ /dev/null @@ -1,55 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch - -################### user configure parameters for ros2 start ################### -xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = 'livox_frame' -lvx_file_path = '/home/livox/livox_test.lvx' -cmdline_bd_code = 'livox0000000001' - -cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' -cur_config_path = cur_path + '../config' -rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz') -user_config_path = os.path.join(cur_config_path, 'HAP_config.json') -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} -] - - -def generate_launch_description(): - livox_driver = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - return LaunchDescription([ - livox_driver, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ]) diff --git a/launch_ROS2/msg_MID360_launch.py b/launch_ROS2/msg_MID360_launch.py deleted file mode 100644 index 8492c495..00000000 --- a/launch_ROS2/msg_MID360_launch.py +++ /dev/null @@ -1,54 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch - -################### user configure parameters for ros2 start ################### -xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = 'livox_frame' -lvx_file_path = '/home/livox/livox_test.lvx' -cmdline_bd_code = 'livox0000000001' - -cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' -cur_config_path = cur_path + '../config' -user_config_path = os.path.join(cur_config_path, 'MID360_config.json') -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} -] - - -def generate_launch_description(): - livox_driver = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - return LaunchDescription([ - livox_driver, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ]) diff --git a/launch_ROS2/rviz_HAP_launch.py b/launch_ROS2/rviz_HAP_launch.py deleted file mode 100644 index 834a5452..00000000 --- a/launch_ROS2/rviz_HAP_launch.py +++ /dev/null @@ -1,63 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch - -################### user configure parameters for ros2 start ################### -xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = 'livox_frame' -lvx_file_path = '/home/livox/livox_test.lvx' -cmdline_bd_code = 'livox0000000001' - -cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' -cur_config_path = cur_path + '../config' -rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') -user_config_path = os.path.join(cur_config_path, 'HAP_config.json') -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} -] - - -def generate_launch_description(): - livox_driver = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - livox_rviz = Node( - package='rviz2', - executable='rviz2', - output='screen', - arguments=['--display-config', rviz_config_path] - ) - - return LaunchDescription([ - livox_driver, - livox_rviz, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ]) diff --git a/launch_ROS2/rviz_MID360_launch.py b/launch_ROS2/rviz_MID360_launch.py deleted file mode 100644 index 31d3480b..00000000 --- a/launch_ROS2/rviz_MID360_launch.py +++ /dev/null @@ -1,63 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch - -################### user configure parameters for ros2 start ################### -xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = 'livox_frame' -lvx_file_path = '/home/livox/livox_test.lvx' -cmdline_bd_code = 'livox0000000001' - -cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' -cur_config_path = cur_path + '../config' -rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') -user_config_path = os.path.join(cur_config_path, 'MID360_config.json') -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} -] - - -def generate_launch_description(): - livox_driver = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - livox_rviz = Node( - package='rviz2', - executable='rviz2', - output='screen', - arguments=['--display-config', rviz_config_path] - ) - - return LaunchDescription([ - livox_driver, - livox_rviz, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ]) diff --git a/launch_ROS2/rviz_mixed.py b/launch_ROS2/rviz_mixed.py deleted file mode 100644 index 46a39c79..00000000 --- a/launch_ROS2/rviz_mixed.py +++ /dev/null @@ -1,63 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -import launch - -################### user configure parameters for ros2 start ################### -xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = 'livox_frame' -lvx_file_path = '/home/livox/livox_test.lvx' -cmdline_bd_code = 'livox0000000001' - -cur_path = os.path.split(os.path.realpath(__file__))[0] + '/' -cur_config_path = cur_path + '../config' -rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz') -user_config_path = os.path.join(cur_config_path, 'mixed_HAP_MID360_config.json') -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code} -] - - -def generate_launch_description(): - livox_driver = Node( - package='livox_ros_driver2', - executable='livox_ros_driver2_node', - name='livox_lidar_publisher', - output='screen', - parameters=livox_ros2_params - ) - - livox_rviz = Node( - package='rviz2', - executable='rviz2', - output='screen', - arguments=['--display-config', rviz_config_path] - ) - - return LaunchDescription([ - livox_driver, - livox_rviz, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ]) diff --git a/package_ROS2.xml b/package.xml similarity index 97% rename from package_ROS2.xml rename to package.xml index 96f57623..5fef5d2c 100644 --- a/package_ROS2.xml +++ b/package.xml @@ -19,6 +19,7 @@ pcl_conversions rcl_interfaces libpcl-all-dev + livox_sdk2 rosbag2 rosidl_default_runtime diff --git a/package_ROS1.xml b/package_ROS1.xml deleted file mode 100644 index 9af21781..00000000 --- a/package_ROS1.xml +++ /dev/null @@ -1,82 +0,0 @@ - - - livox_ros_driver2 - 1.0.0 - The ROS device driver for Livox 3D LiDARs - - - - - Livox Dev Team - - - - - - MIT - - - - - - - - - - - - - Livox Dev Team - - - - - - - - - - - - - - - - - - - - - - catkin - - roscpp - rospy - std_msgs - message_generation - rosbag - pcl_ros - - roscpp - rospy - std_msgs - rosbag - pcl_ros - - roscpp - rospy - std_msgs - message_runtime - rosbag - pcl_ros - - sensor_msgs - git - apr - - - - - - -