diff --git a/README.md b/README.md index 1607cb1..b028dc7 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,5 @@ # Intel RealSense Gazebo ROS plugin and model -## _**Important note**_ - -**This branch only supports ROS Melodic with Gazebo 9.** - -**To use this plugin with ROS Kinetic and Gazebo 7 you should use the [`kinetic-devel` branch](https://github.com/SyrianSpock/realsense_gazebo_plugin/tree/kinetic-devel).** - ## Quickstart Build the plugin diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 29fc2cd..a8af473 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -120,7 +120,11 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // Setup Transport Node this->transportNode = transport::NodePtr(new transport::Node()); +#if GAZEBO_MAJOR_VERSION >= 9 this->transportNode->Init(this->world->Name()); +#else + this->transportNode->Init(this->world->GetName()); +#endif // Setup Publishers std::string rsTopicRoot = @@ -166,7 +170,11 @@ void RealSensePlugin::OnNewFrame(const rendering::CameraPtr cam, msgs::ImageStamped msg; // Set Simulation Time +#if GAZEBO_MAJOR_VERSION >= 9 msgs::Set(msg.mutable_time(), this->world->SimTime()); +#else + msgs::Set(msg.mutable_time(), this->world->GetSimTime()); +#endif // Set Image Dimensions msg.mutable_image()->set_width(cam->ImageWidth()); @@ -215,7 +223,11 @@ void RealSensePlugin::OnNewDepthFrame() } // Pack realsense scaled depth map +#if GAZEBO_MAJOR_VERSION >= 9 msgs::Set(msg.mutable_time(), this->world->SimTime()); +#else + msgs::Set(msg.mutable_time(), this->world->GetSimTime()); +#endif msg.mutable_image()->set_width(this->depthCam->ImageWidth()); msg.mutable_image()->set_height(this->depthCam->ImageHeight()); msg.mutable_image()->set_pixel_format(common::Image::L_INT16); diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index aecd06f..6c6b2c6 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -51,7 +51,11 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, const transport::PublisherPtr pub) { +#if GAZEBO_MAJOR_VERSION >= 9 common::Time current_time = this->world->SimTime(); +#else + common::Time current_time = this->world->GetSimTime(); +#endif // identify camera std::string camera_id = extractCameraName(cam->Name()); @@ -96,7 +100,11 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, void GazeboRosRealsense::OnNewDepthFrame() { // get current time +#if GAZEBO_MAJOR_VERSION >= 9 common::Time current_time = this->world->SimTime(); +#else + common::Time current_time = this->world->GetSimTime(); +#endif RealSensePlugin::OnNewDepthFrame();