Plugin for attachable cart

rmf_robot_sim_gz_plugins/CMakeLists.txt

@@ -181,6 +181,29 @@ target_include_directories(LightTuning
   ${Qt5Quick_INCLUDE_DIRS}
 )
 
+###############################
+# AttachableCart Plugin
+###############################
+
+add_library(attachable_cart SHARED src/attachable_cart.cpp)
+
+ament_target_dependencies(attachable_cart
+    ignition-gazebo${IGN_GAZEBO_VER}
+    ignition-plugin${IGN_PLUGIN_VER}
+    ignition-msgs${IGN_MSGS_VER}
+    rmf_fleet_msgs
+    rclcpp
+    rmf_robot_sim_common
+    Eigen3
+)
+
+target_include_directories(teleport_dispenser
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+    ${EIGEN3_INCLUDE_DIRS}
+)
+
 ###############################
 # Install Targets
 ###############################
@@ -192,6 +215,7 @@ install(
     readonly
     slotcar
     LightTuning
+    attachable_cart
   LIBRARY DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION lib/${PROJECT_NAME}
 )

rmf_robot_sim_gz_plugins/src/attachable_cart.cpp

@@ -0,0 +1,230 @@
+/*
+ * Copyright (C) 2022 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 <ignition/gazebo/Util.hh>
+#include <ignition/gazebo/components/DetachableJoint.hh>
+#include <ignition/gazebo/components/Link.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/components/Model.hh>
+
+#include "attachable_cart.hpp"
+
+using namespace ignition::gazebo;
+
+namespace attachable_cart {
+
+//=================================================
+void AttachableCartPlugin::Configure(
+  const Entity& entity,
+  const std::shared_ptr<const sdf::Element>& /*sdf*/,
+  EntityComponentManager& ecm,
+  EventManager& /*event_mgr*/)
+{
+  auto model = Model(entity);
+  char const** argv = NULL;
+  _name = model.Name(ecm);
+  // Entity has to be to the model's link
+  auto entities = ecm.ChildrenByComponents(entity, components::Link());
+  if (entities.size() != 1)
+  {
+    ignwarn << "Cart should only have one link, using first" << std::endl;
+  }
+  _entity = entities[0];
+  if (!rclcpp::ok())
+    rclcpp::init(0, argv);
+  std::string plugin_name("plugin_" + _name);
+  _ros_node = std::make_shared<rclcpp::Node>(plugin_name);
+
+  _dispenser_result_pub =
+    _ros_node->create_publisher<DispenserResult>(
+    "dispenser_results", 10);
+
+  _ingestor_result_pub =
+    _ros_node->create_publisher<IngestorResult>(
+    "ingestor_results", 10);
+
+  _dispenser_state_pub =
+    _ros_node->create_publisher<DispenserState>(
+    "dispenser_states", 10);
+
+  _ingestor_state_pub =
+    _ros_node->create_publisher<IngestorState>(
+    "ingestor_states", 10);
+
+  _dispenser_state.guid = _name;
+  _dispenser_state.mode = _dispenser_state.IDLE;
+  _ingestor_state.guid = _name;
+  _ingestor_state.mode = _ingestor_state.IDLE;
+
+  _dispenser_request_sub = _ros_node->create_subscription<DispenserRequest>(
+    "dispenser_requests", rclcpp::SystemDefaultsQoS(),
+    [&](DispenserRequest::UniquePtr msg)
+    {
+      if (msg->target_guid == _name)
+      {
+        ignmsg << "Received dispensing request for " << _name << std::endl;
+        if (msg->items.size() != 1)
+        {
+          ignwarn << "Unexpected items size " << msg->items.size() << " aborting..." << std::endl;
+          return;
+        }
+        else if (_joint_entity != kNullEntity)
+        {
+          ignmsg << "Joint already added, skipping" << std::endl;
+          return;
+        }
+        _pending_request = AttachingRequest::ATTACH;
+        _request_guid = msg->request_guid;
+        _target_robot = msg->items[0].type_guid;
+      }
+    });
+
+  _ingestor_request_sub = _ros_node->create_subscription<IngestorRequest>(
+    "ingestor_requests", rclcpp::SystemDefaultsQoS(),
+    [&](IngestorRequest::UniquePtr msg)
+    {
+      if (msg->target_guid == _name)
+      {
+        ignmsg << "Received ingesting request for " << _name << std::endl;
+        if (msg->items.size() != 1)
+        {
+          ignwarn << "Unexpected items size " << msg->items.size() << " aborting..." << std::endl;
+          return;
+        }
+        if (msg->items[0].type_guid != _target_robot)
+        {
+          ignwarn << "Requested to detach from " << msg->items[0].type_guid <<
+            " but item is attached to " << _target_robot << " ignoring..." << std::endl;
+          return;
+        }
+        else if (_joint_entity == kNullEntity)
+        {
+          ignmsg << "Joint already removed, skipping" << std::endl;
+          return;
+        }
+        _pending_request = AttachingRequest::DETACH;
+        _request_guid = msg->request_guid;
+        _target_robot = "";
+      }
+    });
+}
+
+//=================================================
+void AttachableCartPlugin::AttachToRobot(EntityComponentManager& ecm,
+    const std::string& robot_name,
+    bool attach,
+    double cur_t)
+{
+  if (attach)
+  {
+    DispenserResult res;
+    res.time.sec = int(cur_t);
+    res.time.nanosec = (cur_t - int(cur_t)) *1e9;
+    res.request_guid = _request_guid;
+    res.source_guid = _name;
+    res.status = res.FAILED;
+    // ATTACH
+    _joint_entity = ecm.CreateEntity();
+    // Get the robot by name, then its parent
+    auto robot_entity = ecm.EntityByComponents(components::Name(robot_name),
+        components::Model());
+
+    if (robot_entity == kNullEntity)
+    {
+      ignerr << "Robot " << robot_name << " not found, not attaching" << std::endl;
+      _dispenser_result_pub->publish(res);
+      return;
+    }
+
+    auto robot_link_entities = ecm.ChildrenByComponents(robot_entity, components::Link());
+    if (robot_link_entities.size() != 1)
+    {
+      ignwarn << "Robot should only have one link, using first" << std::endl;
+    }
+    auto robot_link_entity = robot_link_entities[0];
+
+    ecm.CreateComponent(
+      _joint_entity,
+      components::DetachableJoint({robot_link_entity,
+                                   _entity, "fixed"}));
+    ignmsg << "Added joint between " << _name << " and " << robot_name << std::endl;
+    res.status = res.ACKNOWLEDGED;
+    _dispenser_result_pub->publish(res);
+    res.status = res.SUCCESS;
+    _dispenser_result_pub->publish(res);
+    _request_guid = "";
+  }
+  else
+  {
+    // DETACH
+    ecm.RequestRemoveEntity(_joint_entity);
+    _joint_entity = kNullEntity;
+    ignmsg << "Removed joint between " << _name << " and " << robot_name << std::endl;
+    IngestorResult res;
+    res.time.sec = int(cur_t);
+    res.time.nanosec = (cur_t - int(cur_t)) *1e9;
+    res.request_guid = _request_guid;
+    res.source_guid = _name;
+    res.status = res.ACKNOWLEDGED;
+    _ingestor_result_pub->publish(res);
+    res.status = res.SUCCESS;
+    _ingestor_result_pub->publish(res);
+    _request_guid = "";
+  }
+}
+
+//=================================================
+void AttachableCartPlugin::PreUpdate(
+  const UpdateInfo& info,
+  EntityComponentManager& ecm)
+{
+  double cur_t = info.simTime.count() / 1e9;
+  rclcpp::spin_some(_ros_node);
+
+  if (info.paused)
+    return;
+
+  if (_pending_request == AttachingRequest::ATTACH)
+    AttachToRobot(ecm, _target_robot, true, cur_t);
+  else if (_pending_request == AttachingRequest::DETACH)
+    AttachToRobot(ecm, _target_robot, false, cur_t);
+  // Reset the request
+  _pending_request = AttachingRequest::NONE;
+
+
+  if (cur_t - _last_state_pub > STATE_PUBLISH_DT)
+  {
+     _dispenser_state.time.sec = int(cur_t);
+     _dispenser_state.time.nanosec = (cur_t - int(cur_t)) *1e9;
+     _ingestor_state.time.sec = int(cur_t);
+     _ingestor_state.time.nanosec = (cur_t - int(cur_t)) *1e9;
+    _dispenser_state_pub->publish(_dispenser_state);
+    _ingestor_state_pub->publish(_ingestor_state);
+    _last_state_pub = cur_t;
+  }
+}
+
+IGNITION_ADD_PLUGIN(
+  AttachableCartPlugin,
+  System,
+  AttachableCartPlugin::ISystemConfigure,
+  AttachableCartPlugin::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(AttachableCartPlugin,
+  "attachable_cart")
+
+} //namespace attachable_cart

rmf_robot_sim_gz_plugins/src/attachable_cart.hpp

@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2022 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 <rclcpp/rclcpp.hpp>
+
+#include <ignition/plugin/Register.hh>
+#include <ignition/gazebo/System.hh>
+#include <ignition/gazebo/Model.hh>
+
+#include <rmf_dispenser_msgs/msg/dispenser_request.hpp>
+#include <rmf_dispenser_msgs/msg/dispenser_result.hpp>
+#include <rmf_dispenser_msgs/msg/dispenser_state.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_request.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_result.hpp>
+#include <rmf_ingestor_msgs/msg/ingestor_state.hpp>
+
+
+namespace attachable_cart {
+
+
+class IGNITION_GAZEBO_VISIBLE AttachableCartPlugin
+  : public ignition::gazebo::System,
+  public ignition::gazebo::ISystemConfigure,
+  public ignition::gazebo::ISystemPreUpdate
+{
+public:
+  using DispenserRequest = rmf_dispenser_msgs::msg::DispenserRequest;
+  using DispenserResult = rmf_dispenser_msgs::msg::DispenserResult;
+  using DispenserState = rmf_dispenser_msgs::msg::DispenserState;
+  using IngestorRequest = rmf_ingestor_msgs::msg::IngestorRequest;
+  using IngestorResult = rmf_ingestor_msgs::msg::IngestorResult;
+  using IngestorState = rmf_ingestor_msgs::msg::IngestorState;
+
+  void Configure(const ignition::gazebo::Entity& entity,
+    const std::shared_ptr<const sdf::Element>& sdf,
+    ignition::gazebo::EntityComponentManager& ecm,
+    ignition::gazebo::EventManager& event_mgr) override;
+
+  // inherit from ISystemPreUpdate
+  void PreUpdate(const ignition::gazebo::UpdateInfo& info,
+    ignition::gazebo::EntityComponentManager& ecm) override;
+
+private:
+
+  static constexpr double STATE_PUBLISH_DT = 1.0;
+
+  enum class AttachingRequest
+  {
+    NONE,
+    ATTACH,
+    DETACH
+  };
+
+  void AttachToRobot(ignition::gazebo::EntityComponentManager& ecm,
+      const std::string& robot_name,
+      bool attach,
+      double cur_t);
+
+  rclcpp::Node::SharedPtr _ros_node;
+
+  std::string _name;
+  ignition::gazebo::Entity _entity;
+
+  rclcpp::Subscription<DispenserRequest>::SharedPtr _dispenser_request_sub;
+  rclcpp::Subscription<IngestorRequest>::SharedPtr _ingestor_request_sub;
+
+  rclcpp::Publisher<DispenserResult>::SharedPtr _dispenser_result_pub;
+  rclcpp::Publisher<IngestorResult>::SharedPtr _ingestor_result_pub;
+  rclcpp::Publisher<DispenserState>::SharedPtr _dispenser_state_pub;
+  rclcpp::Publisher<IngestorState>::SharedPtr _ingestor_state_pub;
+
+  AttachingRequest _pending_request = AttachingRequest::NONE;
+  std::string _target_robot;
+  std::string _request_guid;
+
+  ignition::gazebo::Entity _joint_entity = ignition::gazebo::kNullEntity;
+
+  DispenserState _dispenser_state;
+  IngestorState _ingestor_state;
+
+  double _last_state_pub = 0.0;
+};
+
+} //namespace attachable_cart