From b785a528bdadfd252a1a1871f1caa814e5d8241b Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 16 Jul 2019 13:50:33 +0530 Subject: [PATCH 1/2] [ros2] Port projector to ROS2 --- .../gazebo_plugins/gazebo_ros_projector.h | 127 ------------- .../src/gazebo_ros_projector.cpp | 176 ------------------ gazebo_plugins/CMakeLists.txt | 13 ++ .../gazebo_plugins/gazebo_ros_projector.hpp | 63 +++++++ gazebo_plugins/src/gazebo_ros_projector.cpp | 109 +++++++++++ gazebo_plugins/test/CMakeLists.txt | 1 + .../test/test_gazebo_ros_projector.cpp | 101 ++++++++++ .../test/worlds/gazebo_ros_projector.world | 50 +++++ .../worlds/gazebo_ros_projector_demo.world | 64 +++++++ 9 files changed, 401 insertions(+), 303 deletions(-) delete mode 100644 .ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h delete mode 100644 .ros1_unported/gazebo_plugins/src/gazebo_ros_projector.cpp create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.hpp create mode 100644 gazebo_plugins/src/gazebo_ros_projector.cpp create mode 100644 gazebo_plugins/test/test_gazebo_ros_projector.cpp create mode 100644 gazebo_plugins/test/worlds/gazebo_ros_projector.world create mode 100644 gazebo_plugins/worlds/gazebo_ros_projector_demo.world diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h deleted file mode 100644 index 5d72dce12..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Desc: A dynamic controller plugin that controls texture projection. - * Author: Jared Duke - * Date: 17 June 2010 - * SVN: $Id$ - */ -#ifndef GAZEBO_ROS_PROJECTOR_HH -#define GAZEBO_ROS_PROJECTOR_HH - -// Custom Callback Queue -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace Ogre -{ - class PlaneBoundedVolumeListSceneQuery; - class Frustum; - class Pass; - class SceneNode; -} - -namespace gazebo -{ - -/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins -/// @{ -/** \defgroup GazeboRosProjector Plugin XML Reference and Example - - \brief Ros Texture Projector Controller. - - This is a controller that controls texture projection into the world from a given body. - - Example Usage: - \verbatim - - - ... - - true - 15.0 - stereo_projection_pattern_alpha.png - stereo_projection_pattern_filter.png - projector_controller/image - projector_controller/projector - 0.785398163 - 0.1 - 10 - - - - - \endverbatim - -\{ -*/ - -class GazeboRosProjector : public ModelPlugin -{ - /// \brief Constructor - /// \param parent The parent entity, must be a Model - public: GazeboRosProjector(); - - /// \brief Destructor - public: virtual ~GazeboRosProjector(); - - /// \brief Load the controller - /// \param node XML config node - protected: virtual void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); - - /// \brief pointer to the world - private: physics::WorldPtr world_; - - /// \brief Callback when a texture is published - private: void LoadImage(const std_msgs::String::ConstPtr& imageMsg); - - /// \brief Callbakc when a projector toggle is published - private: void ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg); - - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. - private: ros::NodeHandle* rosnode_; - private: ros::Subscriber imageSubscriber_; - private: ros::Subscriber projectorSubscriber_; - - /// \brief ROS texture topic name - private: std::string texture_topic_name_; - - /// \brief ROS projector topic name - private: std::string projector_topic_name_; - - /// \brief For setting ROS name space - private: std::string robot_namespace_; - - // Custom Callback Queue - private: ros::CallbackQueue queue_; - private: void QueueThread(); - private: boost::thread callback_queue_thread_; - - private: event::ConnectionPtr add_model_event_; - - private: transport::NodePtr node_; - private: transport::PublisherPtr projector_pub_; -}; - -/** \} */ -/// @} - -} -#endif - diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_projector.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_projector.cpp deleted file mode 100644 index ec05cc072..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_projector.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* - * Copyright 2013 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -/* - Desc: GazeboRosTextureProjector plugin that controls texture projection - Author: Jared Duke - Date: 17 Jun 2010 - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -namespace gazebo -{ -GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector); - -typedef std::map OgrePassMap; -typedef OgrePassMap::iterator OgrePassMapIterator; - - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosProjector::GazeboRosProjector() -{ - this->rosnode_ = NULL; -} - - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosProjector::~GazeboRosProjector() -{ - // Custom Callback Queue - this->queue_.clear(); - this->queue_.disable(); - this->rosnode_->shutdown(); - this->callback_queue_thread_.join(); - - delete this->rosnode_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) -{ - this->world_ = _parent->GetWorld(); - - - - - // Create a new transport node for talking to the projector - this->node_.reset(new transport::Node()); - // Initialize the node with the world name -#if GAZEBO_MAJOR_VERSION >= 8 - this->node_->Init(this->world_->Name()); -#else - this->node_->Init(this->world_->GetName()); -#endif - // Setting projector topic - std::string name = std::string("~/") + _parent->GetName() + "/" + - _sdf->Get("projector"); - // Create a publisher on the ~/physics topic - this->projector_pub_ = node_->Advertise(name); - - - - // load parameters - this->robot_namespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; - - this->texture_topic_name_ = ""; - if (_sdf->HasElement("textureTopicName")) - this->texture_topic_name_ = _sdf->GetElement("textureTopicName")->Get(); - - this->projector_topic_name_ = ""; - if (_sdf->HasElement("projectorTopicName")) - this->projector_topic_name_ = _sdf->GetElement("projectorTopicName")->Get(); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("projector", "A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - - // Custom Callback Queue - ros::SubscribeOptions so = ros::SubscribeOptions::create( - this->projector_topic_name_,1, - boost::bind( &GazeboRosProjector::ToggleProjector,this,_1), - ros::VoidPtr(), &this->queue_); - this->projectorSubscriber_ = this->rosnode_->subscribe(so); - - ros::SubscribeOptions so2 = ros::SubscribeOptions::create( - this->texture_topic_name_,1, - boost::bind( &GazeboRosProjector::LoadImage,this,_1), - ros::VoidPtr(), &this->queue_); - this->imageSubscriber_ = this->rosnode_->subscribe(so2); - - - // Custom Callback Queue - this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) ); - -} - - -//////////////////////////////////////////////////////////////////////////////// -// Load a texture into the projector -void GazeboRosProjector::LoadImage(const std_msgs::String::ConstPtr& imageMsg) -{ - msgs::Projector msg; - msg.set_name("texture_projector"); - msg.set_texture(imageMsg->data); - this->projector_pub_->Publish(msg); -} - -//////////////////////////////////////////////////////////////////////////////// -// Toggle the activation of the projector -void GazeboRosProjector::ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg) -{ - msgs::Projector msg; - msg.set_name("texture_projector"); - msg.set_enabled(projectorMsg->data); - this->projector_pub_->Publish(msg); -} - - -//////////////////////////////////////////////////////////////////////////////// -// Custom callback queue thread -void GazeboRosProjector::QueueThread() -{ - static const double timeout = 0.01; - - while (this->rosnode_->ok()) - { - this->queue_.callAvailable(ros::WallDuration(timeout)); - } -} - -} diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7f5a56c64..77fd50b7f 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -201,6 +201,18 @@ target_link_libraries(gazebo_ros_elevator ) ament_export_libraries(gazebo_ros_elevator) +# gazebo_ros_projector +add_library(gazebo_ros_projector SHARED + src/gazebo_ros_projector.cpp +) +ament_target_dependencies(gazebo_ros_projector + "gazebo_ros" + "gazebo_dev" + "rclcpp" + "std_msgs" +) +ament_export_libraries(gazebo_ros_projector) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -225,6 +237,7 @@ install(TARGETS gazebo_ros_ft_sensor gazebo_ros_imu_sensor gazebo_ros_joint_state_publisher + gazebo_ros_projector gazebo_ros_ray_sensor gazebo_ros_p3d gazebo_ros_template diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.hpp new file mode 100644 index 000000000..b7fd0dacf --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.hpp @@ -0,0 +1,63 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GAZEBO_PLUGINS__GAZEBO_ROS_PROJECTOR_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_PROJECTOR_HPP_ + +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosProjectorPrivate; + +/// A projector plugin for gazebo. +/** + Example Usage: + \code{.xml} + + + + demo + + + switch:=switch_demo + + projector_link + my_projector + + + \endcode +*/ +class GazeboRosProjector : public gazebo::ModelPlugin +{ +public: + /// Constructor + GazeboRosProjector(); + + /// Destructor + ~GazeboRosProjector(); + +protected: + // Documentation inherited + void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // namespace gazebo_plugins + +#endif // GAZEBO_PLUGINS__GAZEBO_ROS_PROJECTOR_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_plugins/src/gazebo_ros_projector.cpp new file mode 100644 index 000000000..8c5fa7a16 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_projector.cpp @@ -0,0 +1,109 @@ +// Copyright 2019 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * \brief Switch for Gazebo Texture Projector + * + * \author Jared Duke + * + * \date 17 Jun 2010 + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosProjectorPrivate +{ +public: + /// Callback for switching the projector on/off + /// \param[in] msg Bool switch message + void ToggleProjector(std_msgs::msg::Bool::SharedPtr msg); + + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Gazebo node used to talk to projector + gazebo::transport::NodePtr gazebo_node_; + + /// Publisher for gazebo projector + gazebo::transport::PublisherPtr projector_pub_; + + /// Subscriber to projector switch + rclcpp::Subscription::SharedPtr toggle_sub_; +}; + +GazeboRosProjector::GazeboRosProjector() +: impl_(std::make_unique()) +{ +} + +GazeboRosProjector::~GazeboRosProjector() +{ +} + +void GazeboRosProjector::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + // Create gazebo transport node + impl_->gazebo_node_ = boost::make_shared(); + impl_->gazebo_node_->Init(_model->GetWorld()->Name()); + + auto link_name = _sdf->Get("projector_link", "projector_link").first; + auto projector_name = _sdf->Get("projector_name", "projector").first; + + // Setting projector topic + auto name = "~/" + _model->GetName() + "/" + link_name + "/" + projector_name; + + // Create a publisher on the ~/physics topic + impl_->projector_pub_ = impl_->gazebo_node_->Advertise(name); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Controlling projector at [%s]", impl_->projector_pub_->GetTopic().c_str()); + + impl_->toggle_sub_ = impl_->ros_node_->create_subscription( + "switch", rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosProjectorPrivate::ToggleProjector, impl_.get(), std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), + "Subscribed to [%s]", impl_->toggle_sub_->get_topic_name()); +} + +void GazeboRosProjectorPrivate::ToggleProjector(const std_msgs::msg::Bool::SharedPtr switch_msg) +{ + if (switch_msg->data) { + RCLCPP_INFO(ros_node_->get_logger(), "Switching on projector"); + } else { + RCLCPP_INFO(ros_node_->get_logger(), "Switching off projector"); + } + + gazebo::msgs::Projector msg; + msg.set_name("texture_projector"); + msg.set_enabled(switch_msg->data); + projector_pub_->Publish(msg); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index ab54dd2c4..7fedb9044 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -29,6 +29,7 @@ set(tests ${tests} # TODO(louise) Test hangs on teardown while destroying 2nd node # test_gazebo_ros_camera_distortion test_gazebo_ros_camera_triggered + test_gazebo_ros_projector test_gazebo_ros_video ) endif() diff --git a/gazebo_plugins/test/test_gazebo_ros_projector.cpp b/gazebo_plugins/test/test_gazebo_ros_projector.cpp new file mode 100644 index 000000000..89bd7774b --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_projector.cpp @@ -0,0 +1,101 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +using namespace std::literals::chrono_literals; // NOLINT + +class GazeboRosProjectorTest : public gazebo::RenderingFixture +{ +public: + void TearDown() override + { + // Make sure they're destroyed even if test fails by ASSERT + RenderingFixture::TearDown(); + } +}; + +TEST_F(GazeboRosProjectorTest, ProjectorPublisherTest) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_projector.world", true); + + // Get the scene + auto scene = gazebo::rendering::get_scene(); + ASSERT_NE(nullptr, scene); + + // Trigger render events until model is loaded + int sleep = 0; + int max_sleep = 30; + gazebo::rendering::VisualPtr model_vis; + gazebo::rendering::VisualPtr link_vis; + gazebo::rendering::VisualPtr visual_vis; + for (; !model_vis && sleep < max_sleep; ++sleep) { + gazebo::event::Events::preRender(); + gazebo::event::Events::render(); + gazebo::event::Events::postRender(); + model_vis = scene->GetVisual("projector_model"); + link_vis = scene->GetVisual("projector_model::projector_link"); + visual_vis = scene->GetVisual("projector_model::projector_link::visual"); + gazebo::common::Time::MSleep(100); + } + + EXPECT_LT(sleep, max_sleep); + EXPECT_NE(nullptr, model_vis); + EXPECT_NE(nullptr, link_vis); + EXPECT_NE(nullptr, visual_vis); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_projector_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Send switch commands + auto pub = node->create_publisher( + "test/switch_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto msg = std_msgs::msg::Bool(); + msg.data = false; + + // Give iterations for it to be processed. Make sure there's no crash. + sleep = 0; + max_sleep = 10; + for (; sleep < max_sleep; ++sleep) { + pub->publish(msg); + msg.data = !msg.data; + executor.spin_once(100ms); + + gazebo::event::Events::preRender(); + gazebo::event::Events::render(); + gazebo::event::Events::postRender(); + + EXPECT_NE(nullptr, scene->GetVisual("projector_model")); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_projector.world b/gazebo_plugins/test/worlds/gazebo_ros_projector.world new file mode 100644 index 000000000..6b31ba95c --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_projector.world @@ -0,0 +1,50 @@ + + + + + + model://ground_plane + + + + 0 0 1 0.0 0.0 0 + + + + + 0.1 + + + + + + + 0.1 + + + + + + + + 0 0 0 0 0 0 + bricks.png + 0.959931088597 + 0.1 + 10 + + + true + + + + /test + switch:=switch_test + + projector_link + texture_projector + + + + + \ No newline at end of file diff --git a/gazebo_plugins/worlds/gazebo_ros_projector_demo.world b/gazebo_plugins/worlds/gazebo_ros_projector_demo.world new file mode 100644 index 000000000..5562722f1 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_projector_demo.world @@ -0,0 +1,64 @@ + + + + + + + + model://ground_plane + + + + model://sun + + + + 0 0 1 0.0 0.0 0 + + + + + 0.1 + + + + + + + 0.1 + + + + + + + + 0 0 0 0 0 0 + bricks.png + 0.959931088597 + 0.1 + 10 + + + true + + + + /demo + switch:=switch_demo + + projector_link + texture_projector + + + + + \ No newline at end of file From 902f15052ba08a05f69a3bddb689c930850605ea Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Sat, 27 Jul 2019 15:20:55 -0700 Subject: [PATCH 2/2] fix small typo --- gazebo_plugins/src/gazebo_ros_projector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_plugins/src/gazebo_ros_projector.cpp index 8c5fa7a16..fc4ef5fca 100644 --- a/gazebo_plugins/src/gazebo_ros_projector.cpp +++ b/gazebo_plugins/src/gazebo_ros_projector.cpp @@ -77,7 +77,7 @@ void GazeboRosProjector::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr // Setting projector topic auto name = "~/" + _model->GetName() + "/" + link_name + "/" + projector_name; - // Create a publisher on the ~/physics topic + // Create a Gazebo publisher to switch the projector impl_->projector_pub_ = impl_->gazebo_node_->Advertise(name); RCLCPP_INFO(impl_->ros_node_->get_logger(),