diff --git a/realsense_ros2_camera/CMakeLists.txt b/realsense_ros2_camera/CMakeLists.txt index b1d0b5d..0f7b711 100644 --- a/realsense_ros2_camera/CMakeLists.txt +++ b/realsense_ros2_camera/CMakeLists.txt @@ -49,8 +49,8 @@ if(UNIX OR APPLE) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") endif() -find_package(realsense2) -if(NOT realsense2_FOUND) +find_package(librealsense2) +if(NOT librealsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") endif() @@ -86,7 +86,7 @@ include_directories( ${cv_bridge_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} - ${realsense2_INCLUDE_DIRS} + ${librealsense2_INCLUDE_DIRS} ) add_executable(${PROJECT_NAME} @@ -103,7 +103,7 @@ target_link_libraries(${PROJECT_NAME} ${tf2_LIBRARIES} ${realsense_camera_msgs_LIBRARIES} ${cv_bridge_LIBRARIES} - realsense2 + ${librealsense2_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME} rclcpp @@ -116,7 +116,7 @@ ament_target_dependencies(${PROJECT_NAME} cv_bridge OpenCV image_transport - realsense2 + librealsense2 ) # Install binaries diff --git a/realsense_ros2_camera/src/realsense_camera_node.cpp b/realsense_ros2_camera/src/realsense_camera_node.cpp index ad0bd98..d978cf0 100644 --- a/realsense_ros2_camera/src/realsense_camera_node.cpp +++ b/realsense_ros2_camera/src/realsense_camera_node.cpp @@ -101,8 +101,8 @@ class RealSenseCameraNode : public rclcpp::Node : Node("RealSenseCameraNode"), _ros_clock(RCL_ROS_TIME), _serial_no(""), - _static_tf_broadcaster(std::shared_ptr(this)), _base_frame_id(""), + qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)), _intialize_time_base(false) { RCLCPP_INFO(logger_, "RealSense ROS v%s", REALSENSE_ROS_VERSION_STR); @@ -419,21 +419,21 @@ class RealSenseCameraNode : public rclcpp::Node _info_publisher[FISHEYE] = this->create_publisher( "camera/fisheye/camera_info", 1); _fe_to_depth_publisher = this->create_publisher( - "camera/extrinsics/fisheye2depth", rmw_qos_profile_default); + "camera/extrinsics/fisheye2depth", qos); } if (true == _enable[GYRO]) { _imu_publishers[GYRO] = this->create_publisher("camera/gyro/sample", 100); _imu_info_publisher[GYRO] = this->create_publisher( - "camera/gyro/imu_info", rmw_qos_profile_default); + "camera/gyro/imu_info", qos); } if (true == _enable[ACCEL]) { _imu_publishers[ACCEL] = this->create_publisher("camera/accel/sample", 100); _imu_info_publisher[ACCEL] = this->create_publisher( - "camera/accel/imu_info", rmw_qos_profile_default); + "camera/accel/imu_info", qos); } if (true == _enable[FISHEYE] && @@ -441,8 +441,10 @@ class RealSenseCameraNode : public rclcpp::Node true == _enable[ACCEL])) { _fe_to_imu_publisher = this->create_publisher( - "camera/extrinsics/fisheye2imu", rmw_qos_profile_default); + "camera/extrinsics/fisheye2imu", qos); } + _static_tf_broadcaster_ = + std::make_shared(shared_from_this()); } void setupStreams() @@ -812,7 +814,7 @@ class RealSenseCameraNode : public rclcpp::Node b2d_msg.transform.rotation.y = 0; b2d_msg.transform.rotation.z = 0; b2d_msg.transform.rotation.w = 1; - _static_tf_broadcaster.sendTransform(b2d_msg); + _static_tf_broadcaster_->sendTransform(b2d_msg); // Transform depth frame to depth optical frame q_d2do.setRPY(-M_PI / 2, 0.0, -M_PI / 2); @@ -826,7 +828,7 @@ class RealSenseCameraNode : public rclcpp::Node d2do_msg.transform.rotation.y = q_d2do.getY(); d2do_msg.transform.rotation.z = q_d2do.getZ(); d2do_msg.transform.rotation.w = q_d2do.getW(); - _static_tf_broadcaster.sendTransform(d2do_msg); + _static_tf_broadcaster_->sendTransform(d2do_msg); if (true == _enable[COLOR]) { @@ -843,7 +845,7 @@ class RealSenseCameraNode : public rclcpp::Node b2c_msg.transform.rotation.y = q.y(); b2c_msg.transform.rotation.z = q.z(); b2c_msg.transform.rotation.w = q.w(); - _static_tf_broadcaster.sendTransform(b2c_msg); + _static_tf_broadcaster_->sendTransform(b2c_msg); // Transform color frame to color optical frame q_c2co.setRPY(-M_PI / 2, 0.0, -M_PI / 2); @@ -857,7 +859,7 @@ class RealSenseCameraNode : public rclcpp::Node c2co_msg.transform.rotation.y = q_c2co.getY(); c2co_msg.transform.rotation.z = q_c2co.getZ(); c2co_msg.transform.rotation.w = q_c2co.getW(); - _static_tf_broadcaster.sendTransform(c2co_msg); + _static_tf_broadcaster_->sendTransform(c2co_msg); } if (_enable[DEPTH]) { @@ -883,7 +885,7 @@ class RealSenseCameraNode : public rclcpp::Node b2ir1_msg.transform.rotation.y = q.y(); b2ir1_msg.transform.rotation.z = q.z(); b2ir1_msg.transform.rotation.w = q.w(); - _static_tf_broadcaster.sendTransform(b2ir1_msg); + _static_tf_broadcaster_->sendTransform(b2ir1_msg); // Transform infra1 frame to infra1 optical frame ir1_2_ir1o.setRPY(-M_PI / 2, 0.0, -M_PI / 2); @@ -897,7 +899,7 @@ class RealSenseCameraNode : public rclcpp::Node ir1_2_ir1o_msg.transform.rotation.y = ir1_2_ir1o.getY(); ir1_2_ir1o_msg.transform.rotation.z = ir1_2_ir1o.getZ(); ir1_2_ir1o_msg.transform.rotation.w = ir1_2_ir1o.getW(); - _static_tf_broadcaster.sendTransform(ir1_2_ir1o_msg); + _static_tf_broadcaster_->sendTransform(ir1_2_ir1o_msg); } if (true == _enable[INFRA2]) { @@ -915,7 +917,7 @@ class RealSenseCameraNode : public rclcpp::Node b2ir2_msg.transform.rotation.y = q.y(); b2ir2_msg.transform.rotation.z = q.z(); b2ir2_msg.transform.rotation.w = q.w(); - _static_tf_broadcaster.sendTransform(b2ir2_msg); + _static_tf_broadcaster_->sendTransform(b2ir2_msg); // Transform infra2 frame to infra1 optical frame ir2_2_ir2o.setRPY(-M_PI / 2, 0.0, -M_PI / 2); @@ -929,7 +931,7 @@ class RealSenseCameraNode : public rclcpp::Node ir2_2_ir2o_msg.transform.rotation.y = ir2_2_ir2o.getY(); ir2_2_ir2o_msg.transform.rotation.z = ir2_2_ir2o.getZ(); ir2_2_ir2o_msg.transform.rotation.w = ir2_2_ir2o.getW(); - _static_tf_broadcaster.sendTransform(ir2_2_ir2o_msg); + _static_tf_broadcaster_->sendTransform(ir2_2_ir2o_msg); } } // Publish Fisheye TF @@ -1337,7 +1339,8 @@ class RealSenseCameraNode : public rclcpp::Node std::map _fps; std::map _enable; std::map _stream_name; - tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster; + + std::shared_ptr _static_tf_broadcaster_; std::map _image_publishers; std::map::SharedPtr> _imu_publishers; @@ -1358,6 +1361,8 @@ class RealSenseCameraNode : public rclcpp::Node std::map _camera_info; rclcpp::Publisher::SharedPtr _fe_to_depth_publisher, _fe_to_imu_publisher; + + rclcpp::QoS qos; bool _intialize_time_base; double _camera_time_base; std::map> _enabled_profiles;