diff --git a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp index 38697448f..363802c87 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp @@ -34,8 +34,8 @@ namespace webots_ros2_driver mLaserPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); const int resolution = mLidar->getHorizontalResolution(); mLaserMessage.header.frame_id = mFrameName; - mLaserMessage.angle_increment = -mLidar->getFov() / resolution; - mLaserMessage.angle_min = mLidar->getFov() / 2.0 - mLaserMessage.angle_increment; + mLaserMessage.angle_increment = -mLidar->getFov() / (resolution - 1); + mLaserMessage.angle_min = mLidar->getFov() / 2.0; mLaserMessage.angle_max = -mLidar->getFov() / 2.0; mLaserMessage.time_increment = (double)mLidar->getSamplingPeriod() / (1000.0 * resolution); mLaserMessage.scan_time = (double)mLidar->getSamplingPeriod() / 1000.0; @@ -86,7 +86,7 @@ namespace webots_ros2_driver if (mAlwaysOn) return; - + const bool shouldPointCloudBeEnabled = mPointCloudPublisher->get_subscription_count() > 0; const bool shouldSensorBeEnabled = shouldPointCloudBeEnabled || (mLaserPublisher != nullptr && mLaserPublisher->get_subscription_count() > 0);