[Feature] Pause simulation while dragging interactive markers and via service calls

flatland_server/include/flatland_server/interactive_marker_manager.h

@@ -50,6 +50,8 @@ class InteractiveMarkerManager {
    */
   void update();
 
+  bool isManipulating() { return manipulating_model_; }
+
  private:
   interactive_markers::MenuHandler
       menu_handler_;  ///< Handler for the interactive marker context menus
@@ -59,14 +61,40 @@ class InteractiveMarkerManager {
       models_;  ///< Pointer to the model list in the World class
   PluginManager*
       plugin_manager_;  ///< Pointer to the plugin manager in the World class
+  bool manipulating_model_;  ///< Boolean flag indicating if the user is
+  /// manipulating a model with its interactive marker
+  ros::WallTime pose_update_stamp_;  ///< Timestamp of the last received pose
+  /// update feedback. Used to handle when the
+  /// interactive marker server stops
+  /// manipulating without triggering a
+  /// MOUSE_UP event.
 
   /**
   * @brief Process interactive feedback on a MOUSE_UP event and use it
   * to move the appropriate model to the new pose
   * @param[in] feedback The feedback structure containing the name of the
   * manipulated model and the new pose
   */
-  void processInteractiveFeedback(
+  void processMouseUpFeedback(
+      const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
+
+  /**
+  * @brief Process interactive feedback on a MOUSE_DOWN event and use it
+  * to set a flag indicating that the user is manipulating a model with
+  * its interactive marker
+  * @param[in] feedback The feedback structure containing the name of the
+  * manipulated model and the current pose. Not used in this callback
+  */
+  void processMouseDownFeedback(
+      const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
+
+  /**
+  * @brief Process interactive feedback on a POSE_UPDATE event and record
+  * the current wall time to detect a timeout in the update method
+  * @param[in] feedback The feedback structure containing the name of the
+  * manipulated model and the current pose. Not used in this method
+  */
+  void processPoseUpdateFeedback(
       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
 
   /**

flatland_server/include/flatland_server/service_manager.h

@@ -50,6 +50,7 @@
 #include <flatland_server/simulation_manager.h>
 #include <flatland_server/world.h>
 #include <ros/ros.h>
+#include <std_srvs/Empty.h>
 
 #ifndef FLATLAND_PLUGIN_SERVICE_MANAGER_H
 #define FLATLAND_PLUGIN_SERVICE_MANAGER_H
@@ -70,6 +71,10 @@ class ServiceManager {
   ros::ServiceServer spawn_model_service_;   ///< service for spawning models
   ros::ServiceServer delete_model_service_;  ///< service for deleting models
   ros::ServiceServer move_model_service_;    ///< service for moving models
+  ros::ServiceServer pause_service_;   ///< service for pausing the simulation
+  ros::ServiceServer resume_service_;  ///< service for resuming the simulation
+  ros::ServiceServer toggle_pause_service_;  ///< service for toggling the
+                                             /// pause state of the simulation
 
   /**
    * @brief Service manager constructor
@@ -101,6 +106,24 @@ class ServiceManager {
    */
   bool MoveModel(flatland_msgs::MoveModel::Request &request,
                  flatland_msgs::MoveModel::Response &response);
+
+  /**
+   * @brief Callback for the pause service
+   */
+  bool Pause(std_srvs::Empty::Request &request,
+             std_srvs::Empty::Response &response);
+
+  /**
+   * @brief Callback for the resume service
+   */
+  bool Resume(std_srvs::Empty::Request &request,
+              std_srvs::Empty::Response &response);
+
+  /**
+   * @brief Callback for the pause toggle service
+   */
+  bool TogglePause(std_srvs::Empty::Request &request,
+                   std_srvs::Empty::Response &response);
 };
 };
 #endif

flatland_server/include/flatland_server/world.h

@@ -77,6 +77,8 @@ class World : public b2ContactListener {
       int_marker_manager_;  ///< for dynamically moving models from Rviz
   int physics_position_iterations_;  ///< Box2D solver param
   int physics_velocity_iterations_;  ///< Box2D solver param
+  bool service_paused_;  ///< indicates if simulation is paused by a service
+                         /// call or not
 
   /**
    * @brief Constructor for the world class. All data required for
@@ -156,6 +158,27 @@ class World : public b2ContactListener {
    */
   void MoveModel(const std::string &name, const Pose &pose);
 
+  /**
+   * @brief set the paused state of the simulation to true
+   */
+  void Pause();
+
+  /**
+   * @brief set the paused state of the simulation to false
+   */
+  void Resume();
+
+  /**
+   * @brief toggle the paused state of the simulation
+   */
+  void TogglePaused();
+
+  /**
+   * @brief returns true if service_paused_ is true or an interactive marker is
+   * currently being dragged
+   */
+  bool IsPaused();
+
   /**
    * @brief factory method to create a instance of the world class. Cleans all
    * the inputs before instantiation of the class. TThrows YAMLException.

flatland_server/src/interactive_marker_manager.cpp

@@ -6,6 +6,7 @@ InteractiveMarkerManager::InteractiveMarkerManager(
     std::vector<Model *> *model_list_ptr, PluginManager *plugin_manager_ptr) {
   models_ = model_list_ptr;
   plugin_manager_ = plugin_manager_ptr;
+  manipulating_model_ = false;
 
   // Initialize interactive marker server
   interactive_marker_server_.reset(
@@ -117,12 +118,21 @@ void InteractiveMarkerManager::createInteractiveMarker(
   new_interactive_marker.controls.push_back(no_control);
   interactive_marker_server_->insert(new_interactive_marker);
 
-  // Bind feedback callback for the new interactive marker
+  // Bind feedback callbacks for the new interactive marker
   interactive_marker_server_->setCallback(
       model_name,
-      boost::bind(&InteractiveMarkerManager::processInteractiveFeedback, this,
-                  _1),
+      boost::bind(&InteractiveMarkerManager::processMouseUpFeedback, this, _1),
       visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
+  interactive_marker_server_->setCallback(
+      model_name,
+      boost::bind(&InteractiveMarkerManager::processMouseDownFeedback, this,
+                  _1),
+      visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN);
+  interactive_marker_server_->setCallback(
+      model_name,
+      boost::bind(&InteractiveMarkerManager::processPoseUpdateFeedback, this,
+                  _1),
+      visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
 
   // Add context menu to the new interactive marker
   menu_handler_.apply(*interactive_marker_server_, model_name);
@@ -160,7 +170,7 @@ void InteractiveMarkerManager::deleteInteractiveMarker(
   interactive_marker_server_->applyChanges();
 }
 
-void InteractiveMarkerManager::processInteractiveFeedback(
+void InteractiveMarkerManager::processMouseUpFeedback(
     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
   // Update model that was manipulated the same way
   // as when the MoveModel service is called
@@ -178,23 +188,48 @@ void InteractiveMarkerManager::processInteractiveFeedback(
       break;
     }
   }
+  manipulating_model_ = false;
   interactive_marker_server_->applyChanges();
 }
 
+void InteractiveMarkerManager::processMouseDownFeedback(
+    const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
+  manipulating_model_ = true;
+}
+
+void InteractiveMarkerManager::processPoseUpdateFeedback(
+    const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
+  pose_update_stamp_ = ros::WallTime::now();
+}
+
 void InteractiveMarkerManager::update() {
   // Loop through each model, extract the pose of the root body,
-  // and use it to update the interactive marker pose
-  for (size_t i = 0; i < (*models_).size(); i++) {
-    geometry_msgs::Pose new_pose;
-    new_pose.position.x =
-        (*models_)[i]->bodies_[0]->physics_body_->GetPosition().x;
-    new_pose.position.y =
-        (*models_)[i]->bodies_[0]->physics_body_->GetPosition().y;
-    double theta = (*models_)[i]->bodies_[0]->physics_body_->GetAngle();
-    new_pose.orientation.w = cos(0.5 * theta);
-    new_pose.orientation.z = sin(0.5 * theta);
-    interactive_marker_server_->setPose((*models_)[i]->GetName(), new_pose);
-    interactive_marker_server_->applyChanges();
+  // and use it to update the interactive marker pose. Only
+  // necessary to compute if user is not currently dragging
+  // an interactive marker
+  if (!manipulating_model_) {
+    for (size_t i = 0; i < (*models_).size(); i++) {
+      geometry_msgs::Pose new_pose;
+      new_pose.position.x =
+          (*models_)[i]->bodies_[0]->physics_body_->GetPosition().x;
+      new_pose.position.y =
+          (*models_)[i]->bodies_[0]->physics_body_->GetPosition().y;
+      double theta = (*models_)[i]->bodies_[0]->physics_body_->GetAngle();
+      new_pose.orientation.w = cos(0.5 * theta);
+      new_pose.orientation.z = sin(0.5 * theta);
+      interactive_marker_server_->setPose((*models_)[i]->GetName(), new_pose);
+      interactive_marker_server_->applyChanges();
+    }
+  }
+
+  // Detect when interaction stops without triggering a MOUSE_UP event by
+  // monitoring the time since the last pose update feedback, which comes
+  // in at 33 Hz if the user is dragging the marker.  When the stream of
+  // pose update feedback stops, automatically clear the manipulating_model_
+  // flag to unpause the simulation.
+  double dt = (ros::WallTime::now() - pose_update_stamp_).toSec();
+  if (manipulating_model_ && dt > 0.1 && dt < 1.0) {
+    manipulating_model_ = false;
   }
 }
 

flatland_server/src/service_manager.cpp

@@ -60,6 +60,11 @@ ServiceManager::ServiceManager(SimulationManager *sim_man, World *world)
       nh.advertiseService("delete_model", &ServiceManager::DeleteModel, this);
   move_model_service_ =
       nh.advertiseService("move_model", &ServiceManager::MoveModel, this);
+  pause_service_ = nh.advertiseService("pause", &ServiceManager::Pause, this);
+  resume_service_ =
+      nh.advertiseService("resume", &ServiceManager::Resume, this);
+  toggle_pause_service_ =
+      nh.advertiseService("toggle_pause", &ServiceManager::TogglePause, this);
 
   if (spawn_model_service_) {
     ROS_INFO_NAMED("Service Manager", "Model spawning service ready to go");
@@ -141,4 +146,22 @@ bool ServiceManager::MoveModel(flatland_msgs::MoveModel::Request &request,
 
   return true;
 }
+
+bool ServiceManager::Pause(std_srvs::Empty::Request &request,
+                           std_srvs::Empty::Response &response) {
+  world_->Pause();
+  return true;
+}
+
+bool ServiceManager::Resume(std_srvs::Empty::Request &request,
+                            std_srvs::Empty::Response &response) {
+  world_->Resume();
+  return true;
+}
+
+bool ServiceManager::TogglePause(std_srvs::Empty::Request &request,
+                                 std_srvs::Empty::Response &response) {
+  world_->TogglePaused();
+  return true;
+}
 };

flatland_server/src/world.cpp

@@ -59,7 +59,9 @@
 namespace flatland_server {
 
 World::World()
-    : gravity_(0, 0), int_marker_manager_(&models_, &plugin_manager_) {
+    : gravity_(0, 0),
+      service_paused_(false),
+      int_marker_manager_(&models_, &plugin_manager_) {
   physics_world_ = new b2World(gravity_);
   physics_world_->SetContactListener(this);
 }
@@ -97,12 +99,13 @@ World::~World() {
 }
 
 void World::Update(Timekeeper &timekeeper) {
-  plugin_manager_.BeforePhysicsStep(timekeeper);
-  physics_world_->Step(timekeeper.GetStepSize(), physics_velocity_iterations_,
-                       physics_position_iterations_);
-  timekeeper.StepTime();
-  plugin_manager_.AfterPhysicsStep(timekeeper);
-
+  if (!IsPaused()) {
+    plugin_manager_.BeforePhysicsStep(timekeeper);
+    physics_world_->Step(timekeeper.GetStepSize(), physics_velocity_iterations_,
+                         physics_position_iterations_);
+    timekeeper.StepTime();
+    plugin_manager_.AfterPhysicsStep(timekeeper);
+  }
   int_marker_manager_.update();
 }
 
@@ -311,6 +314,16 @@ void World::MoveModel(const std::string &name, const Pose &pose) {
   }
 }
 
+void World::Pause() { service_paused_ = true; }
+
+void World::Resume() { service_paused_ = false; }
+
+void World::TogglePaused() { service_paused_ = !service_paused_; }
+
+bool World::IsPaused() {
+  return service_paused_ || int_marker_manager_.isManipulating();
+}
+
 void World::DebugVisualize(bool update_layers) {
   if (update_layers) {
     for (const auto &layer : layers_) {

flatland_viz/CMakeLists.txt

@@ -98,6 +98,8 @@ add_executable(flatland_viz
   include/flatland_viz/load_model_dialog.h
   src/spawn_model_tool.cpp
   include/flatland_viz/spawn_model_tool.h
+  src/pause_sim_tool.cpp
+  include/flatland_viz/pause_sim_tool.h
 )
 add_dependencies(flatland_viz ${catkin_EXPORTED_TARGETS})
 
@@ -110,7 +112,9 @@ add_library(flatland_viz_plugins
   src/load_model_dialog.cpp
   include/flatland_viz/load_model_dialog.h
   src/spawn_model_tool.cpp
-  include/flatland_viz/spawn_model_tool.h)
+  include/flatland_viz/spawn_model_tool.h
+  src/pause_sim_tool.cpp
+  include/flatland_viz/pause_sim_tool.h)
 
 add_dependencies(flatland_viz_plugins ${catkin_EXPORTED_TARGETS})
 

flatland_viz/include/flatland_viz/pause_sim_tool.h

@@ -0,0 +1,46 @@
+#ifndef PAUSE_SIM_TOOL_H
+#define PAUSE_SIM_TOOL_H
+
+#include <ros/ros.h>
+#include <rviz/tool.h>
+#include <std_srvs/Empty.h>
+
+namespace flatland_viz {
+
+/**
+ * @name                PauseSimTool
+ * @brief               Rviz tool to support pausing and unpausing the
+ * simulation.
+ */
+class PauseSimTool : public rviz::Tool {
+ public:
+  PauseSimTool();
+  ~PauseSimTool();
+
+ private:
+  /**
+   * @name                onInitialize
+   * @brief               Initializes tools currently loaded when rviz starts
+   */
+  virtual void onInitialize();
+
+  /**
+  virtual void activate();
+   * @name                activate
+   * @brief               Call the pause toggle service
+   */
+  virtual void activate();
+
+  /**
+   * @name                deactivate
+   * @brief               Cleanup when tool is removed
+   */
+  virtual void deactivate();
+
+  ros::NodeHandle nh_;  ///< NodeHandle to call the pause toggle service
+  ros::ServiceClient
+      pause_service_;  ///< ServiceClient that calls the pause toggle service
+};
+}
+
+#endif  // PAUSE_SIM_TOOL_H

flatland_viz/plugin_description.xml

@@ -6,4 +6,11 @@
       Tool for spwaning models on the ground plane in flatland.
     </description>
   </class>
+  <class name="flatland_viz/PauseSim"
+         type="flatland_viz::PauseSimTool"
+         base_class_type="rviz::Tool">
+    <description>
+      Tool for pausing and unpausing flatland simulation.
+    </description>
+  </class>
 </library>