Implement ROS standard simulation interfaces

ros_gz_sim/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(gz-sim REQUIRED)
 find_package(gz_transport_vendor REQUIRED)
 find_package(gz-transport REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(ros_gz_interfaces REQUIRED)
@@ -83,6 +84,7 @@ target_link_libraries(gzserver_component PUBLIC
   ${simulation_interfaces_TARGETS}
   gz-sim::core
   rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
   rclcpp_components::component
   rclcpp_components::component_manager
 )

ros_gz_sim/package.xml

@@ -35,6 +35,7 @@
   <depend>launch_ros</depend>
   <depend>libgflags-dev</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>rclcpp_components</depend>
   <depend>rcpputils</depend>
   <depend>ros_gz_interfaces</depend>

ros_gz_sim/src/simulation_interfaces.cpp

@@ -22,6 +22,7 @@
 #include <gz/msgs/world_control.pb.h>
 #include <gz/msgs/world_stats.pb.h>
 
+#include <cstdint>
 #include <functional>
 #include <gz/math/Pose3.hh>
 #include <gz/sim/EntityComponentManager.hh>
@@ -30,9 +31,16 @@
 #include <gz/sim/components/Name.hh>
 #include <gz/transport/Node.hh>
 #include <iostream>
+#include <limits>
 #include <memory>
 #include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/utilities.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/server.hpp>
 
+#include "simulation_interfaces/action//simulate_steps.hpp"
+#include "simulation_interfaces/action/simulate_steps.hpp"
 #include "simulation_interfaces/msg/result.hpp"
 #include "simulation_interfaces/msg/simulation_state.hpp"
 #include "simulation_interfaces/msg/simulator_features.hpp"
@@ -45,6 +53,7 @@
 #include "simulation_interfaces/srv/reset_simulation.hpp"
 #include "simulation_interfaces/srv/set_simulation_state.hpp"
 #include "simulation_interfaces/srv/spawn_entity.hpp"
+#include "simulation_interfaces/srv/step_simulation.hpp"
 
 namespace components = gz::sim::components;
 
@@ -54,6 +63,7 @@ namespace ros_gz_sim
 class SimulationInterfaces::Implementation
 {
 public:
+  // Services
   using DeleteEntity = simulation_interfaces::srv::DeleteEntity;
   using GetEntities = simulation_interfaces::srv::GetEntities;
   using GetEntityState = simulation_interfaces::srv::GetEntityState;
@@ -63,13 +73,25 @@ class SimulationInterfaces::Implementation
   using ResetSimulation = simulation_interfaces::srv::ResetSimulation;
   using SetSimulationState = simulation_interfaces::srv::SetSimulationState;
   using SpawnEntity = simulation_interfaces::srv::SpawnEntity;
+  using StepSimulation = simulation_interfaces::srv::StepSimulation;
+
+  // Actions
+  using SimulateSteps = simulation_interfaces::action::SimulateSteps;
+  using GoalHandleSimulateSteps = rclcpp_action::ServerGoalHandle<SimulateSteps>;
 
   void Run(rclcpp::Node & node);
   std::string PrefixTopic(const char * topic);
   void UpdateStateFromMsg(const gz::msgs::SerializedStepMap & msg);
   void CreateServices(rclcpp::Node & node);
   template <typename Service, typename HandlerFunc>
   void AddService(rclcpp::Node & node, const char * service_name, HandlerFunc && callback);
+  template <
+    typename Action, typename GoalHandlerFunc, typename CancelHandlerFunc,
+    typename AcceptedHandlerFunc>
+  void AddAction(
+    rclcpp::Node & node, const char * service_name, GoalHandlerFunc && goal_callback,
+    CancelHandlerFunc && cancel_handler, AcceptedHandlerFunc && accepted_handler);
+
   bool InitializeGazeboParameters();
 
   // Service handlers
@@ -96,13 +118,23 @@ class SimulationInterfaces::Implementation
     SetSimulationState::Response::SharedPtr response);
   void SpawnEntityCb(
     SpawnEntity::Request::ConstSharedPtr request, SpawnEntity::Response::SharedPtr response);
+  void StepSimulationCb(
+    StepSimulation::Request::ConstSharedPtr request, StepSimulation::Response::SharedPtr response);
+
+  // Action handlers
+  rclcpp_action::GoalResponse SimulateStepsGoalCb(
+    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const SimulateSteps::Goal> goal);
+  rclcpp_action::CancelResponse SimulateStepsCancelCb(
+    const std::shared_ptr<GoalHandleSimulateSteps> goal_handle);
+  void SimulateStepsAcceptedCb(const std::shared_ptr<GoalHandleSimulateSteps> goal_handle);
 
   // Service helpers
   bool PopulateStateFromEcm(
     const gz::sim::Entity & entity, simulation_interfaces::msg::EntityState & state,
     simulation_interfaces::msg::Result & result);
 
 private:
+  // TODO(azeey) Consider storing the ROS node
   gz::transport::Node gz_node_;
   const unsigned int kTimeout_{5000};
   std::string world_name_;
@@ -111,6 +143,7 @@ class SimulationInterfaces::Implementation
   gz::msgs::WorldStatistics world_stats_;
 
   std::vector<std::shared_ptr<rclcpp::ServiceBase>> services_handles_;
+  std::vector<std::shared_ptr<rclcpp_action::ServerBase>> action_handles_;
 };
 
 void SimulationInterfaces::Implementation::Run(rclcpp::Node & node)
@@ -122,8 +155,9 @@ void SimulationInterfaces::Implementation::Run(rclcpp::Node & node)
     }
 
     // TODO(azeey) Consider adding the UserCommands system if not already present.
-    // TODO(azeey) Wait for critical services to be available (e.g. /world/*/create, /world/*/control)
-    // Request the initial state of the world. This will block until Gazebo is initialized
+    // TODO(azeey) Wait for critical services to be available (e.g. /world/*/create,
+    // /world/*/control) Request the initial state of the world. This will block until Gazebo is
+    // initialized
     gz::msgs::SerializedStepMap reply;
     bool result;
     if (!this->gz_node_.Request(this->PrefixTopic("state"), 30000, reply, result)) {
@@ -171,7 +205,8 @@ void SimulationInterfaces::Implementation::UpdateStateFromMsg(
   this->ecm_.ProcessRemoveEntityRequests();
   this->world_stats_ = msg.stats();
 
-  // TODO(azeey) Consider using a condition variable to notify services that there is new data so as to avoid using stale data
+  // TODO(azeey) Consider using a condition variable to notify services that there is new data so as
+  // to avoid using stale data
 }
 
 void SimulationInterfaces::Implementation::CreateServices(rclcpp::Node & node)
@@ -190,6 +225,11 @@ void SimulationInterfaces::Implementation::CreateServices(rclcpp::Node & node)
   this->AddService<SetSimulationState>(
     node, "set_simulation_state", &Implementation::SetSimulationStateCb);
   this->AddService<SpawnEntity>(node, "spawn_entity", &Implementation::SpawnEntityCb);
+  this->AddService<StepSimulation>(node, "step_simulation", &Implementation::StepSimulationCb);
+
+  this->AddAction<SimulateSteps>(
+    node, "simulate_steps", &Implementation::SimulateStepsGoalCb,
+    &Implementation::SimulateStepsCancelCb, &Implementation::SimulateStepsAcceptedCb);
 }
 
 template <typename Service, typename HandlerFunc>
@@ -202,6 +242,23 @@ void SimulationInterfaces::Implementation::AddService(
   RCLCPP_INFO_STREAM(node.get_logger(), "Created service " << service_name);
 }
 
+template <
+  typename Action, typename GoalHandlerFunc, typename CancelHandlerFunc,
+  typename AcceptedHandlerFunc>
+void SimulationInterfaces::Implementation::AddAction(
+  rclcpp::Node & node, const char * action_name, GoalHandlerFunc && goal_callback,
+  CancelHandlerFunc && cancel_callback, AcceptedHandlerFunc && accepted_callback)
+{
+  this->action_handles_.push_back(
+    rclcpp_action::create_server<Action>(
+      &node, action_name,
+      std::bind(goal_callback, this, std::placeholders::_1, std::placeholders::_2),
+      std::bind(cancel_callback, this, std::placeholders::_1),
+      std::bind(accepted_callback, this, std::placeholders::_1)));
+
+  RCLCPP_INFO_STREAM(node.get_logger(), "Created action " << action_name);
+}
+
 bool SimulationInterfaces::Implementation::InitializeGazeboParameters()
 {
   gz::msgs::StringMsg_V worlds_msg;
@@ -231,7 +288,8 @@ void SimulationInterfaces::Implementation::DeleteEntityCb(
       return;
     }
   }
-  // TODO(azeey) Add specific error codes depending on what went wrong and add more thorough error messages.
+  // TODO(azeey) Add specific error codes depending on what went wrong and add more thorough error
+  // messages.
   response->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
   response->result.error_message = "Error while trying to remove entity";
 }
@@ -259,7 +317,8 @@ void SimulationInterfaces::Implementation::GetEntityStateCb(
   GetEntityState::Request::ConstSharedPtr request, GetEntityState::Response::SharedPtr response)
 {
   std::lock_guard<std::mutex> lk(this->stateSyncMutex_);
-  // TODO (azeey) Since the name might not be unique across Gazebo entity types, ensure that the matched entity is a model.
+  // TODO (azeey) Since the name might not be unique across Gazebo entity types, ensure that the
+  // matched entity is a model.
   auto entity = this->ecm_.EntityByName(request->entity);
   if (entity) {
     this->PopulateStateFromEcm(*entity, response->state, response->result);
@@ -312,7 +371,8 @@ void SimulationInterfaces::Implementation::GetSimulationStateCb(
   if (this->world_stats_.paused()) {
     response->state.state = simulation_interfaces::msg::SimulationState::STATE_PAUSED;
     if (this->world_stats_.iterations() == 0) {
-      // The simulation is in its initial state after loading a world or being reset, which will assign to the STATE_STOPPED state
+      // The simulation is in its initial state after loading a world or being reset, which will
+      // assign to the STATE_STOPPED state
       response->state.state = simulation_interfaces::msg::SimulationState::STATE_STOPPED;
     }
   } else {
@@ -477,12 +537,167 @@ void SimulationInterfaces::Implementation::SpawnEntityCb(
     // We'd have to make sure that the ECM has been updated at least once after the `create` request
     response->entity_name = request->name;
   } else {
-    // TODO(azeey) SpawnEntity has additional error codes to allow surfacing more informative error messages.
-    // However, the `create` service in UserCommands only returns a boolean.
+    // TODO(azeey) SpawnEntity has additional error codes to allow surfacing more informative error
+    // messages. However, the `create` service in UserCommands only returns a boolean.
     response->result.result = Result::RESULT_OPERATION_FAILED;
     response->result.error_message = "Unknown error while tryint to reset simulation";
   }
 }
+
+void SimulationInterfaces::Implementation::StepSimulationCb(
+  StepSimulation::Request::ConstSharedPtr request, StepSimulation::Response::SharedPtr response)
+{
+  using Result = simulation_interfaces::msg::Result;
+  bool sim_paused;
+  {
+    std::lock_guard<std::mutex> lk(this->stateSyncMutex_);
+    sim_paused = this->world_stats_.paused();
+  }
+  if (!sim_paused) {
+    response->result.result = Result::RESULT_OPERATION_FAILED;
+    response->result.error_message = "Simulation has to be paused before stepping";
+    return;
+  }
+
+  // The spec uses a uint64, but the service provided by Gazebo uses a uint32 so we bail out if the
+  // requested number of steps cannot be represented properly.
+  if (request->steps > std::numeric_limits<uint32_t>::max()) {
+    response->result.result = Result::RESULT_OPERATION_FAILED;
+    response->result.error_message =
+      "The requested number of steps exceeds the maximum supported value (max uint32)";
+    return;
+  }
+
+  gz::msgs::WorldControl gz_request;
+  gz_request.set_pause(true);
+  gz_request.set_step(true);
+  gz_request.set_multi_step(request->steps);
+  bool result;
+  gz::msgs::Boolean reply;
+  bool executed =
+    this->gz_node_.Request(this->PrefixTopic("control"), gz_request, 30000, reply, result);
+  if (!executed) {
+    response->result.result = Result::RESULT_OPERATION_FAILED;
+    response->result.error_message = "Timed out while trying to reset simulation";
+  } else if (result && reply.data()) {
+    response->result.result = Result::RESULT_OK;
+  } else {
+    response->result.result = Result::RESULT_OPERATION_FAILED;
+    response->result.error_message = "Unknown error while trying to reset simulation";
+  }
+}
+
+rclcpp_action::GoalResponse SimulationInterfaces::Implementation::SimulateStepsGoalCb(
+  const rclcpp_action::GoalUUID &, std::shared_ptr<const SimulateSteps::Goal>)
+{
+  // TODO(azeey) Add console message that we've received the goal
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse SimulationInterfaces::Implementation::SimulateStepsCancelCb(
+  const std::shared_ptr<GoalHandleSimulateSteps>)
+{
+  // TODO(azeey) Add console message that we've received the cancellation
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void SimulationInterfaces::Implementation::SimulateStepsAcceptedCb(
+  const std::shared_ptr<GoalHandleSimulateSteps> goal_handle)
+{
+  // Note that this is not the same as SimulateSteps::Result, which is the typename associated with
+  // the Result of the action, which in turn contains a simulation_interfaces::msg::Result
+  using Result = simulation_interfaces::msg::Result;
+
+  // Execute the task in a separate thread and return immediately.
+  auto thread = std::thread([this, goal_handle] {
+    gz::transport::Node action_gz_node;
+    const auto goal = goal_handle->get_goal();
+    auto action_result = std::make_shared<SimulateSteps::Result>();
+
+    if (goal->steps > std::numeric_limits<uint32_t>::max()) {
+      action_result->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
+      action_result->result.error_message =
+        "The requested number of steps exceeds the maximum supported value (max uint32)";
+      goal_handle->abort(action_result);
+      return;
+    }
+
+    bool sim_paused;
+    {
+      std::lock_guard<std::mutex> lk(this->stateSyncMutex_);
+      sim_paused = this->world_stats_.paused();
+    }
+    if (!sim_paused) {
+      action_result->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED;
+      action_result->result.error_message = "Simulation has to be paused before stepping";
+      goal_handle->abort(action_result);
+      return;
+    }
+
+    uint64_t num_iters_start = 0;
+    {
+      std::lock_guard<std::mutex> lk(this->stateSyncMutex_);
+      num_iters_start = this->world_stats_.iterations();
+    }
+
+    // TODO(azeey) Refactor this code since it's also used in the StepSimulation service.
+    gz::msgs::WorldControl gz_request;
+    gz_request.set_pause(true);
+    gz_request.set_step(true);
+    gz_request.set_multi_step(goal->steps);
+    bool gz_result;
+    gz::msgs::Boolean reply;
+    bool executed =
+      action_gz_node.Request(this->PrefixTopic("control"), gz_request, 30000, reply, gz_result);
+    if (!executed) {
+      action_result->result.result = Result::RESULT_OPERATION_FAILED;
+      action_result->result.error_message = "Timed out while trying to reset simulation";
+      goal_handle->abort(action_result);
+    } else if (action_result && reply.data()) {
+      // The request has succeeded, so now we listen to the stats and provide feedback.
+      action_result->result.result = Result::RESULT_OK;
+      auto feedback = std::make_shared<SimulateSteps::Feedback>();
+
+      while (rclcpp::ok()) {
+        std::lock_guard<std::mutex> lk(this->stateSyncMutex_);
+        auto iterations = this->world_stats_.iterations();
+        feedback->completed_steps = iterations - num_iters_start;
+        feedback->remaining_steps = goal->steps - feedback->completed_steps;
+        goal_handle->publish_feedback(feedback);
+        // TODO(azeey) There is a bug in Gazebo where the stepping field is set to true only once
+        // immediately after the request to step instead of being true for the whole duration of
+        // steps. So we can't use this right now to determine if we need to break out early
+        // if (!this->world_stats_.stepping()) {
+        //   break;
+        // }
+        if (feedback->remaining_steps == 0) {
+          break;
+        }
+
+        if (goal_handle->is_canceling()) {
+          goal_handle->canceled(action_result);
+          return;
+        }
+      }
+
+      if (rclcpp::ok()) {
+        if (feedback->remaining_steps == 0) {
+          goal_handle->succeed(action_result);
+        } else {
+          action_result->result.result = Result::RESULT_OPERATION_FAILED;
+          action_result->result.error_message = "SimulateSteps was interrupted";
+          goal_handle->abort(action_result);
+        }
+      }
+    } else {
+      action_result->result.result = Result::RESULT_OPERATION_FAILED;
+      action_result->result.error_message = "Unknown error while trying to reset simulation";
+      goal_handle->abort(action_result);
+    }
+  });
+  thread.detach();
+}
+
 SimulationInterfaces::SimulationInterfaces(rclcpp::Node & node)
 : dataPtr(gz::utils::MakeUniqueImpl<Implementation>())
 {