diff --git a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h b/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h deleted file mode 100644 index 14d77437d..000000000 --- a/.ros1_unported/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Copyright 2015 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_PLUGINS_ELEVATOR_PLUGIN_H_ -#define _GAZEBO_ROS_PLUGINS_ELEVATOR_PLUGIN_H_ - -#include - -// Gazebo -#include - -// ROS -#include -#include -#include -#include - -namespace gazebo -{ - /// \brief ROS implementation of the Elevator plugin - class GazeboRosElevator : public ElevatorPlugin - { - /// \brief Constructor - public: GazeboRosElevator(); - - /// \brief Destructor - public: virtual ~GazeboRosElevator(); - - /// \brief Load the plugin. - /// \param[in] _model Pointer to the Model - /// \param[in] _sdf Pointer to the SDF element of the plugin. - public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); - - /// \brief Receives messages on the elevator's topic. - /// \param[in] _msg The string message that contains a command. - public: void OnElevator(const std_msgs::String::ConstPtr &_msg); - - /// \brief Queu to handle callbacks. - private: void QueueThread(); - - /// \brief for setting ROS name space - private: std::string robotNamespace_; - - /// \brief ros node handle - private: ros::NodeHandle *rosnode_; - - /// \brief Subscribes to a topic that controls the elevator. - private: ros::Subscriber elevatorSub_; - - /// \brief Custom Callback Queue - private: ros::CallbackQueue queue_; - - // \brief Custom Callback Queue thread - private: boost::thread callbackQueueThread_; - }; -} -#endif diff --git a/.ros1_unported/gazebo_plugins/src/gazebo_ros_elevator.cpp b/.ros1_unported/gazebo_plugins/src/gazebo_ros_elevator.cpp deleted file mode 100644 index 080d8fbe8..000000000 --- a/.ros1_unported/gazebo_plugins/src/gazebo_ros_elevator.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Copyright 2015 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 "gazebo_plugins/gazebo_ros_elevator.h" - -using namespace gazebo; - -GZ_REGISTER_MODEL_PLUGIN(GazeboRosElevator); - -///////////////////////////////////////////////// -GazeboRosElevator::GazeboRosElevator() -{ -} - -///////////////////////////////////////////////// -GazeboRosElevator::~GazeboRosElevator() -{ - this->queue_.clear(); - this->queue_.disable(); - this->rosnode_->shutdown(); - this->callbackQueueThread_.join(); - - delete this->rosnode_; -} - -///////////////////////////////////////////////// -void GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) -{ - // load parameters - this->robotNamespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - { - this->robotNamespace_ = _sdf->GetElement( - "robotNamespace")->Get() + "/"; - } - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM_NAMED("elevator", "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; - } - - std::string topic = "elevator"; - if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); - - ElevatorPlugin::Load(_parent, _sdf); - - this->rosnode_ = new ros::NodeHandle(this->robotNamespace_); - - ros::SubscribeOptions so = - ros::SubscribeOptions::create(topic, 1, - boost::bind(&GazeboRosElevator::OnElevator, this, _1), - ros::VoidPtr(), &this->queue_); - - this->elevatorSub_ = this->rosnode_->subscribe(so); - - // start custom queue for elevator - this->callbackQueueThread_ = - boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this)); -} - -///////////////////////////////////////////////// -void GazeboRosElevator::OnElevator(const std_msgs::String::ConstPtr &_msg) -{ - this->MoveToFloor(std::stoi(_msg->data)); -} - -///////////////////////////////////////////////// -void GazeboRosElevator::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 1d530e94a..7f5a56c64 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_elevator +add_library(gazebo_ros_elevator SHARED + src/gazebo_ros_elevator.cpp +) +ament_target_dependencies(gazebo_ros_elevator + "gazebo_ros" + "gazebo_dev" + "rclcpp" + "std_msgs" +) +target_link_libraries(gazebo_ros_elevator + ElevatorPlugin +) +ament_export_libraries(gazebo_ros_elevator) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -205,6 +220,7 @@ install(DIRECTORY include/ install(TARGETS gazebo_ros_camera gazebo_ros_diff_drive + gazebo_ros_elevator gazebo_ros_force gazebo_ros_ft_sensor gazebo_ros_imu_sensor diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.hpp new file mode 100644 index 000000000..a1d7efbaf --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.hpp @@ -0,0 +1,68 @@ +// 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_ELEVATOR_HPP_ +#define GAZEBO_PLUGINS__GAZEBO_ROS_ELEVATOR_HPP_ + +#include +#include + +#include + +namespace gazebo_plugins +{ +class GazeboRosElevatorPrivate; + +/// A elevator plugin for gazebo. +/** + Example Usage: + \code{.xml} + + + + demo + + + elevator:=elevator_demo + + + + 0 + 1 + \endcode +*/ +class GazeboRosElevator : public gazebo::ElevatorPlugin +{ +public: + /// Constructor + GazeboRosElevator(); + + /// Destructor + ~GazeboRosElevator(); + +protected: + /// Callback for receiving message on elevator's topic. + /// \param[in] msg String message that contains a elevator command. + void OnElevator(const std_msgs::msg::String::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_ELEVATOR_HPP_ diff --git a/gazebo_plugins/src/gazebo_ros_elevator.cpp b/gazebo_plugins/src/gazebo_ros_elevator.cpp new file mode 100644 index 000000000..494a91997 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_elevator.cpp @@ -0,0 +1,86 @@ +// 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 + +namespace gazebo_plugins +{ +class GazeboRosElevatorPrivate +{ +public: + /// A pointer to the GazeboROS node. + gazebo_ros::Node::SharedPtr ros_node_; + + /// Subscriber to elevator commands + rclcpp::Subscription::SharedPtr sub_; + + /// Lower most floor + int bottom_; + + /// Top most floor + int top_; +}; + +GazeboRosElevator::GazeboRosElevator() +: impl_(std::make_unique()) +{ +} + +GazeboRosElevator::~GazeboRosElevator() +{ +} + +void GazeboRosElevator::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + ElevatorPlugin::Load(_model, _sdf); + + // Initialize ROS node + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->sub_ = impl_->ros_node_->create_subscription( + "elevator", rclcpp::QoS(rclcpp::KeepLast(1)), + std::bind(&GazeboRosElevator::OnElevator, this, std::placeholders::_1)); + + RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]", impl_->sub_->get_topic_name()); + + impl_->bottom_ = _sdf->Get("bottom_floor", std::numeric_limits::min()).first; + + impl_->top_ = _sdf->Get("top_floor", std::numeric_limits::max()).first; +} + +void GazeboRosElevator::OnElevator(const std_msgs::msg::String::ConstSharedPtr msg) +{ + try { + int target_floor = std::stoi(msg->data); + if (target_floor < impl_->bottom_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Target floor number below lowermost floor"); + return; + } + if (target_floor > impl_->top_) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Target floor number above topmost floor"); + return; + } + MoveToFloor(target_floor); + } catch (const std::exception & /*e*/) { + RCLCPP_ERROR(impl_->ros_node_->get_logger(), "Invalid floor number for elevator"); + } +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosElevator) +} // namespace gazebo_plugins diff --git a/gazebo_plugins/test/CMakeLists.txt b/gazebo_plugins/test/CMakeLists.txt index 7ac918f6d..ab54dd2c4 100644 --- a/gazebo_plugins/test/CMakeLists.txt +++ b/gazebo_plugins/test/CMakeLists.txt @@ -13,6 +13,7 @@ endforeach() # Tests set(tests test_gazebo_ros_diff_drive + test_gazebo_ros_elevator test_gazebo_ros_force test_gazebo_ros_ft_sensor test_gazebo_ros_imu_sensor @@ -53,6 +54,7 @@ foreach(test ${tests}) "nav_msgs" "rclcpp" "sensor_msgs" + "std_msgs" ) endforeach() diff --git a/gazebo_plugins/test/test_gazebo_ros_elevator.cpp b/gazebo_plugins/test/test_gazebo_ros_elevator.cpp new file mode 100644 index 000000000..1a23931f5 --- /dev/null +++ b/gazebo_plugins/test/test_gazebo_ros_elevator.cpp @@ -0,0 +1,88 @@ +// 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 GazeboRosElevatorTest : public gazebo::ServerFixture +{ +}; + +TEST_F(GazeboRosElevatorTest, Publishing) +{ + // Load test world and start paused + this->Load("worlds/gazebo_ros_elevator.world", true); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto elevator = world->ModelByName("elevator"); + ASSERT_NE(nullptr, elevator); + + // Link + auto link = elevator->GetLink("link"); + ASSERT_NE(nullptr, link); + + // Step a bit for model to settle + world->Step(100); + + // Check elevator state + EXPECT_NEAR(0.0, link->WorldPose().Pos().X(), tol); + EXPECT_NEAR(0.0, link->WorldPose().Pos().Y(), tol); + EXPECT_NEAR(0.075, link->WorldPose().Pos().Z(), tol); + + // Create node and executor + auto node = std::make_shared("gazebo_ros_elevator_test"); + ASSERT_NE(nullptr, node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Publisher for elevator command + auto pub = node->create_publisher( + "test/elevator_test", rclcpp::QoS(rclcpp::KeepLast(1))); + + auto msg = std_msgs::msg::String(); + msg.data = "1"; + + double sleep = 0; + double max_sleep = 100; + for (; sleep < max_sleep; ++sleep) { + pub->publish(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(3.075, 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_elevator.world b/gazebo_plugins/test/worlds/gazebo_ros_elevator.world new file mode 100644 index 000000000..054bf2013 --- /dev/null +++ b/gazebo_plugins/test/worlds/gazebo_ros_elevator.world @@ -0,0 +1,284 @@ + + + + + model://ground_plane + + + + 0 0 0.075 0 0 0 + + + 800 + + + + + + 2.25 2.25 0.15 + + + + + + + 2.25 2.25 0.15 + + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + + + + + + 1.0745 -0.5 1.125 0 0 0 + + + + 0.08 1.0 2.25 + + + + + + + 0.08 1.0 2.25 + + + + + + + link + door + + 0 1 0 + + 0 + 1 + 10 + + + + 2 + + + + + + world + link + + 0 0 1 + + 0 + 10 + 100000 + + + + 50 + + + + + 1 + + + + + + + + test + elevator:=elevator_test + + 0 + 1 + + elevator::lift + elevator::door + 3.075 + + + 10 + + + + + + true + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + + + 2.25 0 0.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + + 2.25 0 3.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + + diff --git a/gazebo_plugins/worlds/gazebo_ros_elevator_demo.world b/gazebo_plugins/worlds/gazebo_ros_elevator_demo.world new file mode 100644 index 000000000..bd0c95391 --- /dev/null +++ b/gazebo_plugins/worlds/gazebo_ros_elevator_demo.world @@ -0,0 +1,300 @@ + + + + + + model://sun + + + + model://ground_plane + + + + + 0 0 0.075 0 0 0 + + + 800 + + + + + + 2.25 2.25 0.15 + + + + + + + 2.25 2.25 0.15 + + + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + + + + + + 1.0745 -0.5 1.125 0 0 0 + + + + 0.08 1.0 2.25 + + + + + + + 0.08 1.0 2.25 + + + + + + + link + door + + 0 1 0 + + 0 + 1 + 10 + + + + 2 + + + + + + world + link + + 0 0 1 + + 0 + 10 + 100000 + + + + 50 + + + + + 1 + + + + + + + + demo + elevator:=elevator_demo + + 0 + 1 + + elevator::lift + elevator::door + 3.075 + + + 10 + + + + + + true + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + + + 2.25 0 0.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + + 2.25 0 3.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + +