diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h deleted file mode 100644 index 17e0aa4e0..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright (C) 2016 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_ROS_HARNESS_H -#define GAZEBO_ROS_HARNESS_H - -// Custom Callback Queue -#include -#include - -#include -#include -#include - -#include - -namespace gazebo -{ -/// \brief See the Gazebo documentation about the HarnessPlugin. This ROS -/// wrapper exposes two topics: -/// -/// 1. //harness/velocity -/// - Message Type: std_msgs::Float32 -/// - Purpose: Set target winch velocity -/// -/// 2. //harness/detach -/// - Message Type: std_msgs::Bool -/// - Purpose: Detach the joint. -class GazeboRosHarness : public HarnessPlugin -{ - /// \brief Constructor - public: GazeboRosHarness(); - - /// \brief Destructor - public: virtual ~GazeboRosHarness(); - - /// \brief Load the plugin - public: virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); - - /// \brief Receive winch velocity control messages. - /// \param[in] msg Float message that is the target winch velocity. - private: virtual void OnVelocity(const std_msgs::Float32::ConstPtr &msg); - - /// \brief Receive detach messages - /// \param[in] msg Boolean detach message. Detach joints if data is - /// true. - private: virtual void OnDetach(const std_msgs::Bool::ConstPtr &msg); - - /// \brief Custom callback queue thread - private: void QueueThread(); - - /// \brief pointer to ros node - private: ros::NodeHandle *rosnode_; - - /// \brief Subscriber to velocity control messages. - private: ros::Subscriber velocitySub_; - - /// \brief Subscriber to detach control messages. - private: ros::Subscriber detachSub_; - - /// \brief for setting ROS name space - private: std::string robotNamespace_; - private: ros::CallbackQueue queue_; - private: boost::thread callbackQueueThread_; -}; -} -#endif diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_harness.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_harness.cpp deleted file mode 100644 index d661ec832..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_harness.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Copyright (C) 2016 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. - * -*/ -#include -#include -#include - -#include "gazebo_plugins/gazebo_ros_harness.h" - -namespace gazebo -{ - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboRosHarness) - -///////////////////////////////////////////////// -GazeboRosHarness::GazeboRosHarness() -{ -} - -///////////////////////////////////////////////// -GazeboRosHarness::~GazeboRosHarness() -{ - // Custom Callback Queue - this->queue_.clear(); - this->queue_.disable(); - - this->rosnode_->shutdown(); - delete this->rosnode_; -} - -///////////////////////////////////////////////// -// Load the controller -void GazeboRosHarness::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) -{ - // Load the plugin - HarnessPlugin::Load(_parent, _sdf); - - this->robotNamespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - this->robotNamespace_ = _sdf->Get("robotNamespace") + "/"; - - // Init ROS - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("harness", "Not loading plugin since ROS hasn't been " - << "properly initialized. Try starting gazebo with ros plugin:\n" - << " gazebo -s libgazebo_ros_api_plugin.so\n"); - return; - } - - this->rosnode_ = new ros::NodeHandle(this->robotNamespace_); - - ros::SubscribeOptions so = ros::SubscribeOptions::create( - "/" + _parent->GetName() + "/harness/velocity", 1, - boost::bind(&GazeboRosHarness::OnVelocity, this, _1), - ros::VoidPtr(), &this->queue_); - this->velocitySub_ = this->rosnode_->subscribe(so); - - so = ros::SubscribeOptions::create( - "/" + _parent->GetName() + "/harness/detach", 1, - boost::bind(&GazeboRosHarness::OnDetach, this, _1), - ros::VoidPtr(), &this->queue_); - this->detachSub_ = this->rosnode_->subscribe(so); - - // Custom Callback Queue - this->callbackQueueThread_ = - boost::thread(boost::bind(&GazeboRosHarness::QueueThread, this)); -} - -///////////////////////////////////////////////// -void GazeboRosHarness::OnVelocity(const std_msgs::Float32::ConstPtr &msg) -{ - // Set the target winch velocity - this->SetWinchVelocity(msg->data); -} - -///////////////////////////////////////////////// -void GazeboRosHarness::OnDetach(const std_msgs::Bool::ConstPtr &msg) -{ - // Detach if true - if (msg->data) - this->Detach(); -} - -///////////////////////////////////////////////// -void GazeboRosHarness::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 046148394..2f745ed16 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -186,6 +186,21 @@ ament_target_dependencies(gazebo_ros_ft_sensor ) ament_export_libraries(gazebo_ros_ft_sensor) +# gazebo_ros_harness +add_library(gazebo_ros_harness SHARED + src/gazebo_ros_harness.cpp +) +ament_target_dependencies(gazebo_ros_harness + "gazebo_dev" + "gazebo_ros" + "rclcpp" + "std_msgs" +) +target_link_libraries(gazebo_ros_harness + HarnessPlugin +) +ament_export_libraries(gazebo_ros_harness) + # gazebo_ros_ackermann_drive add_library(gazebo_ros_ackermann_drive SHARED src/gazebo_ros_ackermann_drive.cpp @@ -267,6 +282,7 @@ install(TARGETS gazebo_ros_elevator gazebo_ros_force gazebo_ros_ft_sensor + gazebo_ros_harness gazebo_ros_imu_sensor gazebo_ros_joint_state_publisher gazebo_ros_planar_move diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.hpp new file mode 100644 index 000000000..6a8b34856 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.hpp @@ -0,0 +1,129 @@ +// 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_HARNESS_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_HARNESS_HPP_ + +#include +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosHarnessPrivate; + +/// Harness plugin for gazebo. +/// Used to lower a model in a smooth manner +/** + Example Usage: + \code{.xml} + + + + + demo + box/harness/velocity:=velocity_demo + box/harness/detach:=detach_demo + + + + + + + 0 0 0 0 0 0 + world + link + + 0 0 1 + + 10 + + + -1.5 + 1.5 + 10000 + -1 + 0 + 0 + + + + + 1 + + 0.0 + 0.0 + + + + + + + joint1 + + +

1000000

+ 0 + 0 + 0 + 0 + -10000 + 10000 + + + +

10000

+ 0 + 0 + 0 + 0 + 0 + 10000 + + + + + joint1 + + + \endcode +*/ +class GazeboRosHarness : public gazebo::HarnessPlugin +{ +public: + /// Constructor + GazeboRosHarness(); + + /// Destructor + ~GazeboRosHarness(); + +protected: + /// Callback for receiving detach messages. + /// \param[in] msg Empty message. + void OnDetach(const std_msgs::msg::Empty::ConstSharedPtr msg); + + // 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_HARNESS_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_harness.cpp b/gazebo_plugins/src/gazebo_ros_harness.cpp new file mode 100644 index 000000000..14f73e882 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_harness.cpp @@ -0,0 +1,101 @@ +// 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. + +#include +#include +#include +#include +#include + +#include +#include + +namespace gazebo_plugins +{ +class GazeboRosHarnessPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Subscriber to velocity messages + rclcpp::Subscription::SharedPtr vel_sub_; + + /// Subscriber to detach messages + rclcpp::Subscription::SharedPtr detach_sub_; + + /// Model name + std::string model_; + + /// Detach status + bool detached_; +}; + +GazeboRosHarness::GazeboRosHarness() +: impl_(std::make_unique()) +{ +} + +GazeboRosHarness::~GazeboRosHarness() +{ + impl_->ros_node_.reset(); +} + +void GazeboRosHarness::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + gazebo::HarnessPlugin::Load(_model, _sdf); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->model_ = _model->GetName(); + + impl_->vel_sub_ = impl_->ros_node_->create_subscription( + impl_->model_ + "/harness/velocity", rclcpp::QoS(rclcpp::KeepLast(1)), + [this](const std_msgs::msg::Float32::ConstSharedPtr msg) { + SetWinchVelocity(msg->data); + }); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", + impl_->vel_sub_->get_topic_name()); + + impl_->detach_sub_ = impl_->ros_node_->create_subscription( + impl_->model_ + "/harness/detach", rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosHarness::OnDetach, this, std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", + impl_->detach_sub_->get_topic_name()); + + if (_sdf->HasElement("init_vel")) { + auto init_vel = _sdf->Get("init_vel"); + SetWinchVelocity(init_vel); + RCLCPP_INFO( + impl_->ros_node_->get_logger(), "Setting initial harness velocity to [%.2f]m/s", init_vel); + } +} + +void GazeboRosHarness::OnDetach(const std_msgs::msg::Empty::ConstSharedPtr /*msg*/) +{ + if (impl_->detached_) { + RCLCPP_WARN(impl_->ros_node_->get_logger(), + "[%s] is already detached from harness", impl_->model_.c_str()); + return; + } + Detach(); + RCLCPP_INFO(impl_->ros_node_->get_logger(), "[%s] detached from harness", impl_->model_.c_str()); + impl_->detached_ = true; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosHarness) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index da1ea72c4..d83d19897 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -17,6 +17,7 @@ set(tests test_gazebo_ros_elevator test_gazebo_ros_force test_gazebo_ros_ft_sensor + test_gazebo_ros_harness test_gazebo_ros_imu_sensor test_gazebo_ros_joint_state_publisher test_gazebo_ros_p3d diff --git a/gazebo_plugins/test/test_gazebo_ros_harness.cpp b/gazebo_plugins/test/test_gazebo_ros_harness.cpp new file mode 100644 index 000000000..8f7eb3e15 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_harness.cpp @@ -0,0 +1,108 @@ +// 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 + +#define tol 10e-2 + +using namespace std::literals::chrono_literals; // NOLINT + +class GazeboRosHarnessTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosHarnessTest, Publishing) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_harness.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto box = world->ModelByName("box"); + ASSERT_NE(nullptr, box); + + // Link + auto link = box->GetLink("link"); + ASSERT_NE(nullptr, link); + + // Step a bit for model to settle + world->Step(100); + + // Check model state + EXPECT_NEAR(0.0, link->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, link->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(3.0, link->WorldPose().Pos().Z(), tol); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_harness_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Send velocity + auto vel_pub = node->create_publisher( + "test/velocity_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto vel_msg = std_msgs::msg::Float32(); + vel_msg.data = -1.0; + + double sleep = 0; + double max_sleep = 100; + for (; sleep < max_sleep; ++sleep) { + vel_pub->publish(vel_msg); + executor.spin_once(100ms); + world->Step(100); + } + + // Check model state + EXPECT_NEAR(0.0, link->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, link->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(1.5, link->WorldPose().Pos().Z(), tol); + + + // Send detach command + auto detach_pub = node->create_publisher( + "test/detach_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto detach_msg = std_msgs::msg::Empty(); + + sleep = 0; + max_sleep = 10; + for (; sleep < max_sleep && link->WorldPose().Pos().Z() != 0.5; ++sleep) { + detach_pub->publish(detach_msg); + executor.spin_once(100ms); + world->Step(100); + } + + // Check model state + EXPECT_NEAR(0.0, link->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, link->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(0.5, link->WorldPose().Pos().Z(), tol); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/worlds/gazebo_ros_harness.world b/gazebo_plugins/test/worlds/gazebo_ros_harness.world new file mode 100644 index 000000000..51f685b68 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_harness.world @@ -0,0 +1,87 @@ + + + + + model://ground_plane + + + + 0 0 3.0 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + test + box/harness/velocity:=velocity_test + box/harness/detach:=detach_test + + + 0 0 0 0 0 0 + world + link + + 0 0 1 + + 10 + + + -1.5 + 1.5 + 10000 + -1 + 0 + 0 + + + + + 1 + + 0.0 + 0.0 + + + + + + + joint1 + +

1000000

+ 0 + 0 + 0 + 0 + -10000 + 10000 + + +

10000

+ 0 + 0 + 0 + 0 + 0 + 10000 + + + + joint1 + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_harness_demo.world b/gazebo_plugins/worlds/gazebo_ros_harness_demo.world new file mode 100644 index 000000000..aa879f1dc --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_harness_demo.world @@ -0,0 +1,111 @@ + + + + + + + model://ground_plane + + + + model://sun + + + + + 0 0 1.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + demo + box/harness/velocity:=velocity_demo + box/harness/detach:=detach_demo + + + + + + + 0 0 0 0 0 0 + world + link + + 0 0 1 + + 10 + + + -1.5 + 1.5 + 10000 + -1 + 0 + 0 + + + + + 1 + + 0.0 + 0.0 + + + + + + + joint1 + + +

1000000

+ 0 + 0 + 0 + 0 + -10000 + 10000 + + + +

10000

+ 0 + 0 + 0 + 0 + 0 + 10000 + + + + + joint1 + + + +