Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ros2] Port video plugin to ros2 #899

Merged
merged 2 commits into from May 21, 2019

Conversation

@shiveshkhaitan
Copy link

commented Mar 5, 2019

Port gazebo_ros_video plugin to ROS2.

Example:

<plugin name="display_video_controller" filename="libgazebo_ros_video.so">
  <ros>
    <namespace>rrbot</namespace>
    <argument>~/image_raw:=/image_raw</argument>
  </ros>
  <height>1000</height>
  <width>1000</width>
</plugin>

@chapulina chapulina added the ros2 label Mar 5, 2019

@chapulina chapulina self-assigned this Mar 5, 2019

@shiveshkhaitan

This comment has been minimized.

Copy link
Author

commented Mar 5, 2019

#898 will persist in the current implementation. @chapulina is it a good idea to keep a count of the number of video plugins initialized and append it to the entity name? This way two instances of the plugin will initialize the Ogre::MovableObject with different names. I have tried this and it fixes the problem. Or is there any other fix?

@chapulina
Copy link
Contributor

left a comment

Thank you for the contribution!

I realize that creating a test for this could end up being more complicated than the port itself, but maybe we can add a test that just loads the plugin and publishes an image message to make sure the functions are called? We could use a world similar to the demo one I created with a camera. The idea is to at least make sure there's no crash or other strange behaviour.

Another thing to fix are the linter errors, make sure to run colcon test and check the failed results under log/latest_test/gazebo_plugins/stdout.log. There are currently some cpplint, lint_cmake and uncrustify errors there.

Let me know if you have any questions, cheers!

gazebo_plugins/src/gazebo_ros_video.cpp Outdated Show resolved Hide resolved
gazebo_plugins/src/gazebo_ros_video.cpp Outdated Show resolved Hide resolved
gazebo_plugins/worlds/gazebo_ros_video.world Outdated Show resolved Hide resolved

@shiveshkhaitan shiveshkhaitan force-pushed the shiveshkhaitan:ros2 branch from bda53e1 to dfac88f Mar 14, 2019

@shiveshkhaitan

This comment has been minimized.

Copy link
Author

commented Mar 14, 2019

@chapulina thank you for the review. I have made the required changes in the latest commit. Please review the same.
The test currently only publishes an image and essentially checks if the plugin executes without an error. Please let me know if I can add anything else to be tested.

@chapulina
Copy link
Contributor

left a comment

Thanks for iterating! I'm ready to approve once the test is updated.

When you address these comments, it would be convenient if you added a commit on top of the existing one so I can review just the new changes and don't need to go over the whole code again. We can squash them together when merging the pull request.

gazebo_plugins/package.xml Outdated Show resolved Hide resolved
@@ -0,0 +1,83 @@
// Copyright 2018 Open Source Robotics Foundation, Inc.

This comment has been minimized.

Copy link
@chapulina

chapulina May 11, 2019

Contributor

Thanks for writing the test! I put some print statements in the plugin and noticed that it wasn't being loaded while running the test though. Then I remembered that when dealing with visual plugins, we should use the RenderingFixture instead of ServerFixture. Unfortunately, this is not documented 😢

So I propose some changes to the test:

  • Use RenderingFixture
  • Instead of stepping the physics, "step" the rendering by running full pre/render/post cycles
  • Check that all visuals are properly created. When checking this, I noticed that the video visual was not being tracked by the scene, so I added a call to Scene::AddVisual in the plugin. This allows us to get it with Scene::GetVisual.

Let me know what you think!

diff --git a/gazebo_plugins/src/gazebo_ros_video.cpp b/gazebo_plugins/src/gazebo_ros_video.cpp
index f8d8a0f..002ae03 100644
--- a/gazebo_plugins/src/gazebo_ros_video.cpp
+++ b/gazebo_plugins/src/gazebo_ros_video.cpp
@@ -200,8 +200,9 @@ void GazeboRosVideo::Load(
 
   int width = _sdf->Get<int>("width", 320).first;
 
-  impl_->video_visual_ =
-    std::make_shared<VideoVisual>("visual", _parent, height, width);
+  impl_->video_visual_ = std::make_shared<VideoVisual>(_parent->Name() + "::video_visual",
+      _parent, height, width);
+  _parent->GetScene()->AddVisual(impl_->video_visual_);
 
   // Subscribe to the image topic
   impl_->camera_subscriber_ =
@@ -214,7 +215,9 @@ void GazeboRosVideo::Load(
   impl_->update_connection_ = gazebo::event::Events::ConnectPreRender(
     std::bind(&GazeboRosVideoPrivate::onUpdate, impl_.get()));
 
-  RCLCPP_INFO(impl_->rosnode_->get_logger(), "GazeboRosVideo has started");
+  RCLCPP_INFO(impl_->rosnode_->get_logger(),
+    "GazeboRosVideo has started. Subscribed to [%s]",
+    impl_->camera_subscriber_->get_topic_name());
 }
 
 void GazeboRosVideoPrivate::onUpdate()
diff --git a/gazebo_plugins/test/test_gazebo_ros_video.cpp b/gazebo_plugins/test/test_gazebo_ros_video.cpp
index 32d41b7..24eed5a 100644
--- a/gazebo_plugins/test/test_gazebo_ros_video.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_video.cpp
@@ -21,7 +21,7 @@
 
 using namespace std::literals::chrono_literals; // NOLINT
 
-class GazeboRosVideoTest : public gazebo::ServerFixture
+class GazeboRosVideoTest : public gazebo::RenderingFixture
 {
 public:
   void TearDown() override
@@ -36,16 +36,33 @@ TEST_F(GazeboRosVideoTest, VideoSubscribeTest)
   // Load test world and start paused
   this->Load("worlds/gazebo_ros_video.world", true);
 
-  // World
-  auto world = gazebo::physics::get_world();
-  ASSERT_NE(nullptr, world);
+  // Get the scene
+  auto scene = gazebo::rendering::get_scene();
+  ASSERT_TRUE(scene != nullptr);
 
-  // Model box
-  auto box = world->ModelByName("box_display");
-  ASSERT_NE(nullptr, box);
+  // Trigger render events until model is loaded
+  int sleep = 0;
+  int maxSleep = 30;
+  gazebo::rendering::VisualPtr modelVis;
+  gazebo::rendering::VisualPtr linkVis;
+  gazebo::rendering::VisualPtr visualVis;
+  gazebo::rendering::VisualPtr videoVis;
+  for (; !modelVis && sleep < maxSleep; ++sleep) {
+    gazebo::event::Events::preRender();
+    gazebo::event::Events::render();
+    gazebo::event::Events::postRender();
 
-  // Step a bit for model to settle
-  world->Step(100);
+    modelVis = scene->GetVisual("box_display");
+    linkVis = scene->GetVisual("box_display::base_link");
+    visualVis = scene->GetVisual("box_display::base_link::visual");
+    videoVis = scene->GetVisual("box_display::base_link::visual::video_visual");
+    gazebo::common::Time::MSleep(100);
+  }
+  EXPECT_LT(sleep, maxSleep);
+  EXPECT_NE(nullptr, modelVis);
+  EXPECT_NE(nullptr, linkVis);
+  EXPECT_NE(nullptr, visualVis);
+  EXPECT_NE(nullptr, videoVis);
 
   // Create node and executor
   auto node = std::make_shared<rclcpp::Node>("gazebo_ros_video_test");
@@ -67,10 +84,23 @@ TEST_F(GazeboRosVideoTest, VideoSubscribeTest)
     msg.data.push_back(255);
     msg.data.push_back(255);
   }
-  pub->publish(msg);
-  executor.spin_once(100ms);
-  // Wait for it to be processed
-  world->Step(1000);
+
+  // Give iterations for it to be processed. Right now we don't have a quick way to verify the
+  // output, so we just make sure there's no crash
+  sleep = 0;
+  maxSleep = 10;
+  for (; sleep < maxSleep; ++sleep) {
+    pub->publish(msg);
+    executor.spin_once(100ms);
+
+    gazebo::event::Events::preRender();
+    gazebo::event::Events::render();
+    gazebo::event::Events::postRender();
+
+    EXPECT_NE(nullptr, scene->GetVisual("box_display"));
+    EXPECT_NE(nullptr,
+      scene->GetVisual("box_display::base_link::visual::video_visual"));
+  }
 }
 
 int main(int argc, char ** argv)
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_video.world b/gazebo_plugins/test/worlds/gazebo_ros_video.world
index b30bfc1..293aaa2 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_video.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_video.world
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <!--
-  Gazebo ROS video plugin demo
+  Gazebo ROS video plugin test
 
     The visual on top of the box is displaying the images coming from a camera.
 -->
@@ -28,7 +28,7 @@
             <ros>
               <!-- Remap for image topic to be subscribed -->
               <namespace>/test</namespace>
-              <argument>~/image_raw:=/test/video_test</argument>
+              <argument>~/image_raw:=video_test</argument>
             </ros>
             <height>120</height>
             <width>160</width>
@@ -46,4 +46,4 @@
     </model>
 
   </world>
-</sdf>
\ No newline at end of file
+</sdf>
@chapulina
Copy link
Contributor

left a comment

Great work!

@chapulina chapulina merged commit ae092ff into ros-simulation:ros2 May 21, 2019

@chapulina

This comment has been minimized.

Copy link
Contributor

commented May 21, 2019

@shiveshkhaitan , mind adding a migration page to the wiki? It doesn't need to be too elaborate, you can follow the pattern of existing pages, such as ROS 2 Migration: Diff-drive.

@shiveshkhaitan shiveshkhaitan deleted the shiveshkhaitan:ros2 branch May 22, 2019

@shiveshkhaitan

This comment has been minimized.

Copy link
Author

commented May 22, 2019

Migration guide is available at ROS 2 Migration: Video

@chapulina

This comment has been minimized.

Copy link
Contributor

commented Jun 15, 2019

is it a good idea to keep a count of the number of video plugins initialized and append it to the entity name

I just realized I never replied to this question. Counting should be ok. We could also use the visual's scoped name, which is guaranteed to be unique.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
2 participants
You can’t perform that action at this time.