diff --git a/CMakeLists.txt b/CMakeLists.txt index 31682d4..adb4559 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,19 +1,20 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(gl_depth_sim) - set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) add_compile_options(-Wall -Wextra) -find_package(catkin REQUIRED COMPONENTS - roscpp # Used for ROS examples - pcl_ros # Used for interfaces extension - tf2_ros # Used for ROS example - tf2_eigen # Used for ROS example - visualization_msgs # Used for ROS example -) +# Used for ROS examples +find_package(ament_cmake REQUIRED) +find_package(PCL REQUIRED QUIET) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) # Required for core functionality find_package(glfw3 REQUIRED) @@ -24,21 +25,8 @@ find_package(Eigen3 REQUIRED) find_package(assimp REQUIRED) # Just used for loading models in mesh_loader.h find_package(OpenCV REQUIRED) # Used for interface extension -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_interfaces - CATKIN_DEPENDS - roscpp - pcl_ros - tf2_ros - tf2_eigen - visualization_msgs - DEPENDS OpenCV -) - include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) @@ -53,33 +41,46 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}/glad.c src/${PROJECT_NAME}/glfw_guard.cpp ) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${OPENGL_LIBRARIES} ${ASSIMP_LIBRARIES} dl glfw ) - -# Libaries for interfacing with opencv and pcl +# ament_target_dependencies(${PROJECT_NAME} +# rclcpp +# pcl_conversions +# pcl_ros +# tf2_ros +# tf2_eigen +# visualization_msgs +# OpenCV +# ) + +# Libraries for interfacing with opencv and pcl add_library(${PROJECT_NAME}_interfaces src/interfaces/pcl_interface.cpp src/interfaces/opencv_interface.cpp ) -add_dependencies(${PROJECT_NAME}_interfaces ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_interfaces ${PROJECT_NAME} - ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) +ament_target_dependencies(${PROJECT_NAME}_interfaces + rclcpp + pcl_conversions + pcl_ros + tf2_ros + tf2_eigen + visualization_msgs + OpenCV +) # Simulated laser scanner library add_library(${PROJECT_NAME}_sim_laser_scanner src/${PROJECT_NAME}/sim_laser_scanner.cpp ) -add_dependencies(${PROJECT_NAME}_interfaces ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_sim_laser_scanner - ${catkin_LIBRARIES} ${PROJECT_NAME} ${PROJECT_NAME}_interfaces ) @@ -90,7 +91,6 @@ set_target_properties(${PROJECT_NAME}_test PROPERTIES OUTPUT_NAME depth_example target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} ${PROJECT_NAME}_interfaces - ${catkin_LIBRARIES} ) # Example showing an orbiting camera @@ -99,7 +99,6 @@ set_target_properties(${PROJECT_NAME}_orbit PROPERTIES OUTPUT_NAME orbit_example target_link_libraries(${PROJECT_NAME}_orbit ${PROJECT_NAME} ${PROJECT_NAME}_interfaces - ${catkin_LIBRARIES} ) # Example showing orbiting camera with point cloud publishing in ROS @@ -108,7 +107,6 @@ set_target_properties(${PROJECT_NAME}_ros_orbit PROPERTIES OUTPUT_NAME ros_examp target_link_libraries(${PROJECT_NAME}_ros_orbit ${PROJECT_NAME} ${PROJECT_NAME}_interfaces - ${catkin_LIBRARIES} ) # Example showing sweeping laser scan with point cloud publishing in ROS @@ -116,9 +114,9 @@ add_executable(${PROJECT_NAME}_laser_example src/laser_example.cpp) set_target_properties(${PROJECT_NAME}_laser_example PROPERTIES OUTPUT_NAME laser_example PREFIX "") target_link_libraries(${PROJECT_NAME}_laser_example ${PROJECT_NAME}_sim_laser_scanner - ${catkin_LIBRARIES} ) +# Install targets install( TARGETS ${PROJECT_NAME} @@ -128,23 +126,16 @@ install( ${PROJECT_NAME}_orbit ${PROJECT_NAME}_ros_orbit ${PROJECT_NAME}_laser_example - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + +install(DIRECTORY launch test + DESTINATION share/${PROJECT_NAME}/ ) -############# -## Testing ## -############# -#if(CATKIN_ENABLE_TESTING) -# find_package(rostest REQUIRED) -# catkin_add_gtest(utest_node test/utest.cpp) -# target_link_libraries(utest_node -# ${catkin_LIBRARIES} -# ${PROJECT_NAME}_sim_laser_scanner -# ) -#endif() +ament_package() diff --git a/README.md b/README.md index 44b02f3..7075339 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ This scene shows the classic Stanford Dragon seen by a depth camera orbiting aro You can run this example in ROS by building this package in your workspace and running: ``` -rosrun gl_depth_sim ros_example _mesh:= +ros2 run gl_depth_sim ros_example _mesh:= ``` You can also set the `_z` and `_radius` parameters. @@ -37,7 +37,7 @@ Various properties of the laser scanner, such as minimum and maximum range and a You can run this example in ROS by building this package in your workspace and running: ``` -roslaunch gl_depth_sim laser_example.launch +ros2 launch gl_depth_sim laser_example.launch.py ``` ## Usage diff --git a/launch/laser_example.launch b/launch/laser_example.launch deleted file mode 100644 index 04663b2..0000000 --- a/launch/laser_example.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/laser_example.launch.py b/launch/laser_example.launch.py new file mode 100644 index 0000000..384c863 --- /dev/null +++ b/launch/laser_example.launch.py @@ -0,0 +1,48 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + rviz_config = LaunchConfiguration('rviz_config') + mesh_filename = LaunchConfiguration('mesh_filename') + + return LaunchDescription([ + DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join( + get_package_share_directory('gl_depth_sim'), + 'launch', 'laser_example.rviz' + ), + description='Path to the RViz config file' + ), + DeclareLaunchArgument( + 'mesh_filename', + default_value=os.path.join( + get_package_share_directory('gl_depth_sim'), + 'test', 'stanford_dragon.stl' + ), + description='Path to the mesh file' + ), + Node( + package='gl_depth_sim', + executable='laser_example', + name='laser_example', + output='screen', + parameters=[{ + 'mesh_filename': mesh_filename, + 'min_range': 0.1, + 'max_range': 30.0, + 'angular_resolution': 0.001 + }] + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz', + output='screen', + arguments=['-d', rviz_config] + ) + ]) diff --git a/launch/laser_example.rviz b/launch/laser_example.rviz index ef2267f..fe0f29f 100644 --- a/launch/laser_example.rviz +++ b/launch/laser_example.rviz @@ -1,41 +1,37 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 719 - - Class: rviz/Selection + Tree Height: 555 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: PointCloud2 -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -51,20 +47,22 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: - All Enabled: false + All Enabled: true camera: Value: true world: Value: true - Marker Scale: 0.5 + Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true - Show Names: true + Show Names: false Tree: world: camera: @@ -79,7 +77,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz/PointCloud2 + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 @@ -91,76 +89,106 @@ Visualization Manager: Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ - Queue Size: 10 Selectable: true Size (Pixels): 1 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /object Name: Marker Namespaces: - mesh: true - Queue Size: 100 + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /object Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - 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 + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 6.967314720153809 + Class: rviz_default_plugins/Orbit + Distance: 6.079007148742676 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 + X: -0.1208057850599289 + Y: -0.08937393128871918 + Z: 0.17805616557598114 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6347970962524414 + Pitch: 0.7003980875015259 Target Frame: Value: Orbit (rviz) - Yaw: 0.6035810112953186 + Yaw: 0.6853981018066406 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1000 + Height: 846 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001ac0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000027000002e4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf00000044fc0100000002fb0000000800540069006d00650100000000000004bf000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000030d0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -169,6 +197,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1215 - X: 65 - Y: 24 + Width: 1200 + X: 60 + Y: 60 diff --git a/package.xml b/package.xml index 5518c5c..0551c7f 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + gl_depth_sim 0.0.1 The gl_depth_sim package @@ -8,12 +8,13 @@ LGPLv3 - catkin - roscpp + ament_cmake + rclcpp tf2_ros tf2_eigen + pcl_conversions pcl_ros visualization_msgs @@ -22,4 +23,7 @@ assimp libopencv-dev + + ament_cmake + diff --git a/src/camera_ros_example.cpp b/src/camera_ros_example.cpp index 97f2f79..3bf88ca 100644 --- a/src/camera_ros_example.cpp +++ b/src/camera_ros_example.cpp @@ -2,12 +2,12 @@ #include "gl_depth_sim/mesh_loader.h" #include "gl_depth_sim/interfaces/pcl_interface.h" -#include +#include #include -#include +#include #include -#include +#include #include #include "gl_depth_sim/interfaces/opencv_interface.h" @@ -31,38 +31,48 @@ static Eigen::Isometry3d lookat(const Eigen::Vector3d& origin, const Eigen::Vect int main(int argc, char** argv) { - ros::init(argc, argv, "ros_depth_sim_orbit"); - ros::NodeHandle nh, pnh ("~"); + rclcpp::init(argc, argv); + auto node = std::make_shared("ros_depth_sim_orbit"); + auto logger = node->get_logger(); // Setup ROS interfaces - ros::Publisher cloud_pub = nh.advertise>("cloud", 1, true); + auto cloud_pub = node->create_publisher("cloud", 1); - tf2_ros::TransformBroadcaster broadcaster; + tf2_ros::TransformBroadcaster broadcaster(*node); + + // Declare ROS parameters + node->declare_parameter("mesh"); + node->declare_parameter("base_frame", "world"); + node->declare_parameter("camera_frame", "camera"); + node->declare_parameter("radius", 1.0); + node->declare_parameter("z", 1.0); + node->declare_parameter("focal_length", 550.0); + node->declare_parameter("width", 640); + node->declare_parameter("height", 480); // Load ROS parameters std::string mesh_path; - if (!pnh.getParam("mesh", mesh_path)) + if (!node->get_parameter("mesh", mesh_path)) { - ROS_ERROR_STREAM("User must set the 'mesh' private parameter"); + RCLCPP_ERROR(logger, "User must set the 'mesh' parameter"); + rclcpp::shutdown(); return 1; } - nh.getParam("/ros_example/mesh", mesh_path); - - std::string base_frame = pnh.param("base_frame", "world"); - std::string camera_frame = pnh.param("camera_frame", "camera"); - double radius = pnh.param("radius", 1.0); - double z = pnh.param("z", 1.0); + std::string base_frame = node->get_parameter("base_frame").as_string(); + std::string camera_frame = node->get_parameter("camera_frame").as_string(); - double focal_length = pnh.param("focal_length", 550.0); - int width = pnh.param("width", 640); - int height = pnh.param("height", 480); + double radius = node->get_parameter("radius").as_double(); + double z = node->get_parameter("z").as_double(); + double focal_length = node->get_parameter("focal_length").as_double(); + int width = node->get_parameter("width").as_int(); + int height = node->get_parameter("height").as_int(); auto mesh_ptr = gl_depth_sim::loadMesh(mesh_path); - if (!mesh_ptr) { - ROS_ERROR_STREAM("Unable to load mesh from path: " << mesh_path); + RCLCPP_ERROR(logger, "Unable to load mesh from path: %s", mesh_path.c_str()); + rclcpp::shutdown(); return 1; } @@ -77,7 +87,7 @@ int main(int argc, char** argv) props.z_far = 10.0f; // Create the simulation - gl_depth_sim::SimDepthCamera sim (props); + gl_depth_sim::SimDepthCamera sim(props); sim.add("mesh_identifier", *mesh_ptr, Eigen::Isometry3d::Identity()); @@ -86,10 +96,7 @@ int main(int argc, char** argv) // In the main (rendering) thread, begin orbiting... const auto start = std::chrono::steady_clock::now(); - pcl::PointCloud cloud; - cloud.header.frame_id = camera_frame; - - while (ros::ok()) + while (rclcpp::ok()) { double dt = std::chrono::duration(std::chrono::steady_clock::now() - start).count(); @@ -111,14 +118,18 @@ int main(int argc, char** argv) } // Step 1: Publish the cloud + pcl::PointCloud cloud; gl_depth_sim::toPointCloudXYZ(props, depth_img, cloud); - pcl_conversions::toPCL(ros::Time::now(), cloud.header.stamp); - cloud_pub.publish(cloud); + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + cloud_msg.header.frame_id = camera_frame; + cloud_msg.header.stamp = node->now(); + cloud_pub->publish(cloud_msg); // Step 2: Publish the TF so we can see it in RViz - geometry_msgs::TransformStamped transform = tf2::eigenToTransform(pose); + auto transform = tf2::eigenToTransform(pose); transform.header.frame_id = base_frame; - transform.header.stamp = ros::Time::now(); + transform.header.stamp = node->now(); transform.child_frame_id = camera_frame; broadcaster.sendTransform(transform); @@ -126,7 +137,7 @@ int main(int argc, char** argv) gl_depth_sim::toCvImage16u(depth_img, img); cv::imwrite("img.png", img); - ros::spinOnce(); + rclcpp::spin_some(node); } return 0; diff --git a/src/laser_example.cpp b/src/laser_example.cpp index d28d29a..ff470d7 100644 --- a/src/laser_example.cpp +++ b/src/laser_example.cpp @@ -2,36 +2,25 @@ #include #include -#include +#include #include -#include +#include #include -#include -#include +#include +#include -template -bool get(const ros::NodeHandle &nh, const std::string &key, T &val) +visualization_msgs::msg::Marker createMeshMarker(const Eigen::Isometry3d &pose, + const std::string &frame, + const std::string &mesh_resource) { - if (!nh.getParam(key, val)) - { - ROS_ERROR_STREAM("Failed to get '" << key << "' parameter"); - return false; - } - return true; -} - -visualization_msgs::Marker createMeshMarker(const Eigen::Isometry3d &pose, - const std::string &frame, - const std::string &mesh_resource) -{ - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = frame; marker.id = 0; marker.ns = "mesh"; - marker.action = visualization_msgs::Marker::ADD; + marker.action = visualization_msgs::msg::Marker::ADD; - marker.type = visualization_msgs::Marker::MESH_RESOURCE; + marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; // Check if the mesh resource does not use the "file://" or "package://" URI if (mesh_resource.find("file://") == std::string::npos @@ -62,27 +51,45 @@ visualization_msgs::Marker createMeshMarker(const Eigen::Isometry3d &pose, int main(int argc, char **argv) { - ros::init(argc, argv, "laser_node"); - ros::NodeHandle nh, pnh("~"); - ros::AsyncSpinner spinner(1); - spinner.start(); - - ros::Publisher cloud_pub = nh.advertise>("cloud", 1); - tf2_ros::TransformBroadcaster broadcaster; - std::string base_frame, camera_frame; - pnh.param("base_frame", base_frame, "world"); - pnh.param("camera_frame", camera_frame, "camera"); - - std::string mesh_filename; - if (!get(pnh, "mesh_filename", mesh_filename)) + rclcpp::init(argc, argv); + auto node = std::make_shared("laser_node"); + auto logger = node->get_logger(); + + // Create single-threaded executor and spin in a separate thread + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread spin_thread([&executor]() { executor.spin(); }); + + auto cloud_pub = node->create_publisher("cloud", 1); + auto broadcaster = tf2_ros::TransformBroadcaster(*node); + + // Declare parameters + node->declare_parameter("base_frame", "world"); + node->declare_parameter("camera_frame", "camera"); + node->declare_parameter("mesh_filename"); + node->declare_parameter("min_range"); + node->declare_parameter("max_range"); + node->declare_parameter("angular_resolution"); + + // Get parameters + std::string base_frame, camera_frame, mesh_filename; + node->get_parameter("base_frame", base_frame); + node->get_parameter("camera_frame", camera_frame); + + if (!node->get_parameter("mesh_filename", mesh_filename)) + { + RCLCPP_ERROR(logger, "Parameter 'mesh_filename' is required."); + rclcpp::shutdown(); return -1; + } // Get the laser scanner properties gl_depth_sim::LaserScannerProperties laser_scan_props; - if (!get(pnh, "min_range", laser_scan_props.min_range) - || !get(pnh, "max_range", laser_scan_props.max_range) - || !get(pnh, "angular_resolution", laser_scan_props.angular_resolution)) + if (!node->get_parameter("min_range", laser_scan_props.min_range) + || !node->get_parameter("max_range", laser_scan_props.max_range) + || !node->get_parameter("angular_resolution", laser_scan_props.angular_resolution)) { + rclcpp::shutdown(); return -1; } @@ -95,9 +102,9 @@ int main(int argc, char **argv) laser.add(*mesh_ptr, mesh_pose); // Publish a message with the mesh for visualization - ros::Publisher pub = nh.advertise("object", 1, true); - visualization_msgs::Marker marker = createMeshMarker(mesh_pose, base_frame, mesh_filename); - pub.publish(marker); + auto pub = node->create_publisher("object", rclcpp::QoS(1).transient_local()); + auto marker = createMeshMarker(mesh_pose, base_frame, mesh_filename); + pub->publish(marker); // Sweep the laser scanner back and forth across the surface of a part in the world y-axis direction // @@ -108,30 +115,38 @@ int main(int argc, char **argv) std::size_t counter = 0; Eigen::Isometry3d scanner_pose(nominal_scanner_pose); - while (ros::ok()) + while (rclcpp::ok()) { pcl::PointCloud scan = laser.render(scanner_pose); // Publish the scan cloud - scan.header.frame_id = camera_frame; - pcl_conversions::toPCL(ros::Time::now(), scan.header.stamp); - cloud_pub.publish(scan); + sensor_msgs::msg::PointCloud2 scan_msg; + pcl::toROSMsg(scan, scan_msg); + scan_msg.header.frame_id = camera_frame; + scan_msg.header.stamp = node->now(); + cloud_pub->publish(scan_msg); // Wait - ros::Duration(0.005).sleep(); + rclcpp::sleep_for(std::chrono::milliseconds(5)); // Update the transform ++counter; double z = std::sin(static_cast(counter) / 180.0) * sweep_distance; scanner_pose = nominal_scanner_pose * Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, z)); - geometry_msgs::TransformStamped transform = tf2::eigenToTransform(scanner_pose); + auto transform = tf2::eigenToTransform(scanner_pose); transform.header.frame_id = base_frame; - transform.header.stamp = ros::Time::now(); + transform.header.stamp = node->now(); transform.child_frame_id = camera_frame; broadcaster.sendTransform(transform); } - ros::waitForShutdown(); + executor.cancel(); + spin_thread.join(); + + // Reset node before shutdown to avoid segfault (see ROS 2 issues) + node.reset(); + + rclcpp::shutdown(); return 0; }