[ros2] Port elevator to ROS2

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

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 <gazebo/plugins/ElevatorPlugin.hh>
+#include <std_msgs/msg/string.hpp>
+
+#include <memory>
+
+namespace gazebo_plugins
+{
+class GazeboRosElevatorPrivate;
+
+/// A elevator plugin for gazebo.
+/**
+  Example Usage:
+  \code{.xml}
+    <!-- Plugin to control the elevator -->
+    <plugin name="elevator" filename="libgazebo_ros_elevator.so">
+      <ros>
+        <namespace>demo</namespace>
+
+        <!-- topic remapping -->
+        <argument>elevator:=elevator_demo</argument>
+      </ros>
+
+      <!-- min and max floor constraints -->
+      <bottom_floor>0</bottom_floor>
+      <top_floor>1</top_floor>
+  \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<GazeboRosElevatorPrivate> impl_;
+};
+}  // namespace gazebo_plugins
+
+#endif  // GAZEBO_PLUGINS__GAZEBO_ROS_ELEVATOR_HPP_

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 <gazebo_plugins/gazebo_ros_elevator.hpp>
+#include <gazebo_ros/node.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <limits>
+#include <memory>
+
+namespace gazebo_plugins
+{
+class GazeboRosElevatorPrivate
+{
+public:
+  /// A pointer to the GazeboROS node.
+  gazebo_ros::Node::SharedPtr ros_node_;
+
+  /// Subscriber to elevator commands
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
+
+  /// Lower most floor
+  int bottom_;
+
+  /// Top most floor
+  int top_;
+};
+
+GazeboRosElevator::GazeboRosElevator()
+: impl_(std::make_unique<GazeboRosElevatorPrivate>())
+{
+}
+
+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<std_msgs::msg::String>(
+    "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<int>("bottom_floor", std::numeric_limits<int>::min()).first;
+
+  impl_->top_ = _sdf->Get<int>("top_floor", std::numeric_limits<int>::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

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()