diff --git a/gazebo_dev/cmake/gazebo_dev-extras.cmake b/gazebo_dev/cmake/gazebo_dev-extras.cmake index d4bdd3b81..3d2259a04 100644 --- a/gazebo_dev/cmake/gazebo_dev-extras.cmake +++ b/gazebo_dev/cmake/gazebo_dev-extras.cmake @@ -3,11 +3,38 @@ find_package(gazebo REQUIRED) message(STATUS "Gazebo version: ${GAZEBO_VERSION}") +# Workaround for https://github.com/ros-simulation/gazebo_ros_pkgs/issues/1372 +# Real fix should go into Gazebo11 https://github.com/osrf/gazebo/issues/3008 +# On Ubuntu Jammy Gazebo11 is used directly from Ubuntu, before removing this +# please be sure that the package is updated there. +pkg_check_modules(TBB tbb) +set(TBB_PKG_CONFIG "tbb") +if(NOT TBB_FOUND) + message(STATUS "TBB not found, attempting to detect manually") + set(TBB_PKG_CONFIG "") + + # Workaround for CMake bug https://gitlab.kitware.com/cmake/cmake/issues/17135 + unset(TBB_FOUND CACHE) + + find_package(TBB CONFIG) + if(TBB_FOUND) + set(TBB_LIBRARIES TBB::tbb) + else() + find_library(tbb_library tbb ENV LD_LIBRARY_PATH) + if(tbb_library) + set(TBB_FOUND true) + set(TBB_LIBRARIES ${tbb_library}) + else() + message(FATAL_ERROR "Missing: TBB - Threading Building Blocks") + endif() + endif() +endif() + # The following lines will tell catkin to add the Gazebo directories and libraries to the # respective catkin_* cmake variables. set(gazebo_dev_INCLUDE_DIRS ${GAZEBO_INCLUDE_DIRS}) -set(gazebo_dev_LIBRARY_DIRS ${GAZEBO_LIBRARY_DIRS}) -set(gazebo_dev_LIBRARIES ${GAZEBO_LIBRARIES}) +set(gazebo_dev_LIBRARY_DIRS ${GAZEBO_LIBRARY_DIRS} ${TBB_LIBRARY_DIRS}) +set(gazebo_dev_LIBRARIES ${GAZEBO_LIBRARIES} ${TBB_LIBRARIES}) # Append gazebo CXX_FLAGS to CMAKE_CXX_FLAGS (c++11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 5758592b2..c5251234f 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -184,7 +184,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ if (impl_->sensor_type_ != GazeboRosCameraPrivate::MULTICAMERA) { // Image publisher // TODO(louise) Migrate image_connect logic once SubscriberStatusCallback is ported to ROS2 - const std::string camera_topic = impl_->camera_name_ + "/image_raw"; + const std::string camera_topic = "~/image_raw"; impl_->image_pub_.push_back( image_transport::create_publisher( impl_->ros_node_.get(), camera_topic, qos.get_publisher_qos( @@ -197,7 +197,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ // Camera info publisher // TODO(louise) Migrate ImageConnect logic once SubscriberStatusCallback is ported to ROS2 - const std::string camera_info_topic = impl_->camera_name_ + "/camera_info"; + const std::string camera_info_topic = "~/camera_info"; impl_->camera_info_pub_.push_back( impl_->ros_node_->create_publisher( camera_info_topic, qos.get_publisher_qos( @@ -210,7 +210,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ } else { for (uint64_t i = 0; i < impl_->num_cameras_; ++i) { const std::string camera_topic = - impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw"; + "~/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw"; // Image publisher impl_->image_pub_.push_back( image_transport::create_publisher( @@ -223,7 +223,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ // impl_->image_pub_.back().getTopic()); const std::string camera_info_topic = - impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/camera_info"; + "~/" + MultiCameraPlugin::camera_[i]->Name() + "/camera_info"; // Camera info publisher impl_->camera_info_pub_.push_back( impl_->ros_node_->create_publisher( @@ -238,7 +238,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ } if (impl_->sensor_type_ == GazeboRosCameraPrivate::DEPTH) { - const std::string depth_topic = impl_->camera_name_ + "/depth/image_raw"; + const std::string depth_topic = "~/depth/image_raw"; // Depth image publisher impl_->depth_image_pub_ = image_transport::create_publisher( impl_->ros_node_.get(), depth_topic, qos.get_publisher_qos( @@ -247,7 +247,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ // RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing depth images to [%s]", // impl_->depth_image_pub_.getTopic().c_str()); - const std::string depth_info_topic = impl_->camera_name_ + "/depth/camera_info"; + const std::string depth_info_topic = "~/depth/camera_info"; // Depth info publisher impl_->depth_camera_info_pub_ = impl_->ros_node_->create_publisher( @@ -257,7 +257,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ impl_->ros_node_->get_logger(), "Publishing depth camera info to [%s]", impl_->depth_camera_info_pub_->get_topic_name()); - const std::string point_cloud_topic = impl_->camera_name_ + "/points"; + const std::string point_cloud_topic = "~/points"; // Point cloud publisher impl_->point_cloud_pub_ = impl_->ros_node_->create_publisher( point_cloud_topic, qos.get_publisher_qos(point_cloud_topic, rclcpp::QoS(1))); @@ -269,7 +269,7 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _ // Trigger if (_sdf->Get("triggered", false).first) { - const std::string trigger_topic = impl_->camera_name_ + "/image_trigger"; + const std::string trigger_topic = "~/image_trigger"; impl_->trigger_sub_ = impl_->ros_node_->create_subscription( trigger_topic, qos.get_subscription_qos(trigger_topic, rclcpp::QoS(1)), std::bind(&GazeboRosCamera::OnTrigger, this, std::placeholders::_1));