[Feature] Move and delete models using interactive markers

flatland_server/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_geometry_msgs
   geometry_msgs
   visualization_msgs
+  interactive_markers
   flatland_msgs
 )
 
@@ -90,6 +91,7 @@ add_library(flatland_lib
   src/collision_filter_registry.cpp
   src/model_plugin.cpp
   src/plugin_manager.cpp
+  src/interactive_marker_manager.cpp
   src/timekeeper.cpp
   src/service_manager.cpp
   src/yaml_reader.cpp

flatland_server/include/flatland_server/interactive_marker_manager.h

@@ -0,0 +1,83 @@
+#ifndef INTERACTIVEMARKERMANAGER_H
+#define INTERACTIVEMARKERMANAGER_H
+
+#include <flatland_server/geometry.h>
+#include <flatland_server/model.h>
+#include <flatland_server/plugin_manager.h>
+#include <flatland_server/types.h>
+#include <interactive_markers/interactive_marker_server.h>
+#include <interactive_markers/menu_handler.h>
+#include <visualization_msgs/MarkerArray.h>
+
+namespace flatland_server {
+
+class InteractiveMarkerManager {
+ public:
+  /**
+   * @brief Constructor for the interactive marker manager class
+   * @param[in] model_list_ptr Pointer to the list of models in the World class
+   * @param[in] plugin_manager_ptr Pointer to the plugin manager in the World
+   * class
+   */
+  InteractiveMarkerManager(std::vector<Model*>* model_list_ptr,
+                           PluginManager* plugin_manager_ptr);
+
+  /**
+   * @brief Destructor for the interactive marker manager class
+   */
+  ~InteractiveMarkerManager();
+
+  /**
+   * @brief Add a new interactive marker when spawning a model
+   * @param[in] model_name Name of the model being spawned
+   * @param[in] pose Initial pose of the spawning model
+   * @param[in] body_markers Marker array corresponding to new model bodies
+   */
+  void createInteractiveMarker(
+      const std::string& model_name, const Pose& pose,
+      const visualization_msgs::MarkerArray& body_markers);
+
+  /**
+   * @brief Remove interactive marker corresponding to a given model when
+   * deleting it from the simulation
+   * @param[in] model_name Name of the model being deleted
+   */
+  void deleteInteractiveMarker(const std::string& model_name);
+
+  /**
+   * @brief Update the interactive marker poses after running
+   * physics update to synchronize the markers with the models
+   */
+  void update();
+
+ private:
+  interactive_markers::MenuHandler
+      menu_handler_;  ///< Handler for the interactive marker context menus
+  boost::shared_ptr<interactive_markers::InteractiveMarkerServer>
+      interactive_marker_server_;  ///< Interactive marker server
+  std::vector<Model*>*
+      models_;  ///< Pointer to the model list in the World class
+  PluginManager*
+      plugin_manager_;  ///< Pointer to the plugin manager in the World class
+
+  /**
+  * @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(
+      const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
+
+  /**
+  * @brief Process feedback from the context menu of the interactive marker to
+  * delete the appropriate model
+  * @param[in] feedback The feedback structure containing the name of the model
+  * to be deleted
+  */
+  void deleteModelMenuCallback(
+      const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
+};
+}
+
+#endif  // INTERACTIVEMARKERMANAGER_H

flatland_server/include/flatland_server/world.h

@@ -49,6 +49,7 @@
 
 #include <Box2D/Box2D.h>
 #include <flatland_server/collision_filter_registry.h>
+#include <flatland_server/interactive_marker_manager.h>
 #include <flatland_server/layer.h>
 #include <flatland_server/model.h>
 #include <flatland_server/plugin_manager.h>
@@ -72,6 +73,8 @@ class World : public b2ContactListener {
   std::vector<Model *> models_;   ///< list of models
   CollisionFilterRegistry cfr_;   ///< collision registry for layers and models
   PluginManager plugin_manager_;  ///< for loading and updating plugins
+  InteractiveMarkerManager
+      int_marker_manager_;  ///< for dynamically moving models from Rviz
   int physics_position_iterations_;  ///< Box2D solver param
   int physics_velocity_iterations_;  ///< Box2D solver param
 

flatland_server/package.xml

@@ -24,6 +24,7 @@
   <depend>geometry_msgs</depend>
   <depend>yaml-cpp</depend>
   <depend>visualization_msgs</depend>
+  <depend>interactive_markers</depend>
   <depend>flatland_msgs</depend>
 
   <export>

flatland_server/src/interactive_marker_manager.cpp

@@ -0,0 +1,204 @@
+#include <flatland_server/interactive_marker_manager.h>
+
+namespace flatland_server {
+
+InteractiveMarkerManager::InteractiveMarkerManager(
+    std::vector<Model *> *model_list_ptr, PluginManager *plugin_manager_ptr) {
+  models_ = model_list_ptr;
+  plugin_manager_ = plugin_manager_ptr;
+
+  // Initialize interactive marker server
+  interactive_marker_server_.reset(
+      new interactive_markers::InteractiveMarkerServer(
+          "interactive_model_markers"));
+
+  // Add "Delete Model" context menu option to menu handler and bind callback
+  menu_handler_.setCheckState(
+      menu_handler_.insert(
+          "Delete Model",
+          boost::bind(&InteractiveMarkerManager::deleteModelMenuCallback, this,
+                      _1)),
+      interactive_markers::MenuHandler::NO_CHECKBOX);
+  interactive_marker_server_->applyChanges();
+}
+
+void InteractiveMarkerManager::createInteractiveMarker(
+    const std::string &model_name, const Pose &pose,
+    const visualization_msgs::MarkerArray &body_markers) {
+  // Set up interactive marker control objects to allow both translation and
+  // rotation movement
+  visualization_msgs::InteractiveMarkerControl plane_control;
+  plane_control.always_visible = true;
+  plane_control.orientation.w = 0.707;
+  plane_control.orientation.y = 0.707;
+  plane_control.name = "move_xy";
+  plane_control.interaction_mode =
+      visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
+  visualization_msgs::InteractiveMarkerControl rotate_control;
+  rotate_control.always_visible = true;
+  rotate_control.orientation.w = 0.707;
+  rotate_control.orientation.y = 0.707;
+  rotate_control.name = "rotate_z";
+  rotate_control.interaction_mode =
+      visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
+
+  // Add a non-interactive text marker with the model name
+  visualization_msgs::InteractiveMarkerControl no_control;
+  no_control.always_visible = true;
+  no_control.name = "no_control";
+  no_control.interaction_mode =
+      visualization_msgs::InteractiveMarkerControl::NONE;
+  visualization_msgs::Marker text_marker;
+  text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+  text_marker.color.r = 1.0;
+  text_marker.color.g = 1.0;
+  text_marker.color.b = 1.0;
+  text_marker.color.a = 1.0;
+  text_marker.pose.position.x = 0.5;
+  text_marker.pose.position.y = 0.0;
+  text_marker.scale.z = 0.5;
+  text_marker.text = model_name;
+  no_control.markers.push_back(text_marker);
+
+  // Add a cube marker to be an easy-to-manipulate target in Rviz
+  visualization_msgs::Marker easy_to_click_cube;
+  easy_to_click_cube.type = visualization_msgs::Marker::CUBE;
+  easy_to_click_cube.color.r = 0.0;
+  easy_to_click_cube.color.g = 1.0;
+  easy_to_click_cube.color.b = 0.0;
+  easy_to_click_cube.color.a = 0.5;
+  easy_to_click_cube.scale.x = 0.5;
+  easy_to_click_cube.scale.y = 0.5;
+  easy_to_click_cube.scale.z = 0.05;
+  easy_to_click_cube.pose.position.x = 0.0;
+  plane_control.markers.push_back(easy_to_click_cube);
+
+  // Also add body markers to the no_control object to visualize model pose
+  // while moving its interactive marker
+  for (size_t i = 0; i < body_markers.markers.size(); i++) {
+    visualization_msgs::Marker transformed_body_marker =
+        body_markers.markers[i];
+
+    // Transform original body frame marker from global to local frame
+    RotateTranslate rt = Geometry::CreateTransform(pose.x, pose.y, pose.theta);
+    transformed_body_marker.header.frame_id = "";
+    transformed_body_marker.pose.position.x =
+        (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos +
+        (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin;
+    transformed_body_marker.pose.position.y =
+        -(body_markers.markers[i].pose.position.x - rt.dx) * rt.sin +
+        (body_markers.markers[i].pose.position.y - rt.dy) * rt.cos;
+    transformed_body_marker.pose.orientation.w = 1.0;
+    transformed_body_marker.pose.orientation.x = 0.0;
+    transformed_body_marker.pose.orientation.y = 0.0;
+    transformed_body_marker.pose.orientation.z = 0.0;
+
+    // Make line strips thicker than the original
+    if (transformed_body_marker.type ==
+            visualization_msgs::Marker::LINE_STRIP ||
+        transformed_body_marker.type == visualization_msgs::Marker::LINE_LIST) {
+      transformed_body_marker.scale.x = 0.1;
+    }
+
+    // Add transformed body marker to interactive marker control object
+    no_control.markers.push_back(transformed_body_marker);
+  }
+
+  // Send new interactive marker to server
+  visualization_msgs::InteractiveMarker new_interactive_marker;
+  new_interactive_marker.header.frame_id = "map";
+  new_interactive_marker.name = model_name;
+  new_interactive_marker.pose.position.x = pose.x;
+  new_interactive_marker.pose.position.y = pose.y;
+  new_interactive_marker.pose.orientation.w = cos(0.5 * pose.theta);
+  new_interactive_marker.pose.orientation.z = sin(0.5 * pose.theta);
+  new_interactive_marker.controls.push_back(plane_control);
+  new_interactive_marker.controls.push_back(rotate_control);
+  new_interactive_marker.controls.push_back(no_control);
+  interactive_marker_server_->insert(new_interactive_marker);
+
+  // Bind feedback callback for the new interactive marker
+  interactive_marker_server_->setCallback(
+      model_name,
+      boost::bind(&InteractiveMarkerManager::processInteractiveFeedback, this,
+                  _1),
+      visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP);
+
+  // Add context menu to the new interactive marker
+  menu_handler_.apply(*interactive_marker_server_, model_name);
+
+  // Apply changes to server
+  interactive_marker_server_->applyChanges();
+}
+
+void InteractiveMarkerManager::deleteModelMenuCallback(
+    const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
+  // Delete the model just as when the DeleteModel service is called
+  for (int i = 0; i < (*models_).size(); i++) {
+    if ((*models_)[i]->GetName() == feedback->marker_name) {
+      // delete the plugins associated with the model
+      plugin_manager_->DeleteModelPlugin((*models_)[i]);
+      delete (*models_)[i];
+      (*models_).erase((*models_).begin() + i);
+
+      // Also remove corresponding interactive marker
+      deleteInteractiveMarker(feedback->marker_name);
+      break;
+    }
+  }
+
+  // Update menu handler and server
+  menu_handler_.apply(*interactive_marker_server_, feedback->marker_name);
+  interactive_marker_server_->applyChanges();
+}
+
+void InteractiveMarkerManager::deleteInteractiveMarker(
+    const std::string &model_name) {
+  // Remove target interactive marker by name and
+  // update the server
+  interactive_marker_server_->erase(model_name);
+  interactive_marker_server_->applyChanges();
+}
+
+void InteractiveMarkerManager::processInteractiveFeedback(
+    const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
+  // Update model that was manipulated the same way
+  // as when the MoveModel service is called
+  for (int i = 0; i < models_->size(); i++) {
+    if ((*models_)[i]->GetName() == feedback->marker_name) {
+      Pose new_pose;
+      new_pose.x = feedback->pose.position.x;
+      new_pose.y = feedback->pose.position.y;
+      new_pose.theta = atan2(
+          2.0 * feedback->pose.orientation.w * feedback->pose.orientation.z,
+          1.0 -
+              2.0 * feedback->pose.orientation.z *
+                  feedback->pose.orientation.z);
+      (*models_)[i]->SetPose(new_pose);
+      break;
+    }
+  }
+  interactive_marker_server_->applyChanges();
+}
+
+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();
+  }
+}
+
+InteractiveMarkerManager::~InteractiveMarkerManager() {
+  interactive_marker_server_.reset();
+}
+}

flatland_server/src/world.cpp

@@ -58,7 +58,8 @@
 
 namespace flatland_server {
 
-World::World() : gravity_(0, 0) {
+World::World()
+    : gravity_(0, 0), int_marker_manager_(&models_, &plugin_manager_) {
   physics_world_ = new b2World(gravity_);
   physics_world_->SetContactListener(this);
 }
@@ -101,6 +102,8 @@ void World::Update(Timekeeper &timekeeper) {
                        physics_position_iterations_);
   timekeeper.StepTime();
   plugin_manager_.AfterPhysicsStep(timekeeper);
+
+  int_marker_manager_.update();
 }
 
 void World::BeginContact(b2Contact *contact) {
@@ -256,6 +259,13 @@ void World::LoadModel(const std::string &model_yaml_path, const std::string &ns,
 
   models_.push_back(m);
 
+  visualization_msgs::MarkerArray body_markers;
+  for (size_t i = 0; i < m->bodies_.size(); i++) {
+    DebugVisualization::Get().BodyToMarkers(
+        body_markers, m->bodies_[i]->physics_body_, 1.0, 0.0, 0.0, 1.0);
+  }
+  int_marker_manager_.createInteractiveMarker(name, pose, body_markers);
+
   ROS_INFO_NAMED("World", "Model \"%s\" loaded", m->name_.c_str());
   m->DebugOutput();
 }
@@ -270,6 +280,7 @@ void World::DeleteModel(const std::string &name) {
       plugin_manager_.DeleteModelPlugin(models_[i]);
       delete models_[i];
       models_.erase(models_.begin() + i);
+      int_marker_manager_.deleteInteractiveMarker(name);
       found = true;
       break;
     }

flatland_viz/include/flatland_viz/flatland_viz.h

@@ -114,6 +114,7 @@ class FlatlandViz : public QWidget {
   rviz::RenderPanel* render_panel_;
 
   rviz::Display* grid_;
+  rviz::Display* interactive_markers_;
   std::map<std::string, rviz::Display*> debug_displays_;
   ros::Subscriber debug_topic_subscriber_;
   rviz::PropertyTreeWidget* tree_widget_;