<commit_before>#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 object to allow both translation and
  // rotation movement
  visualization_msgs::InteractiveMarkerControl control;
  control.always_visible = true;
  control.orientation.w = 1.0;
  control.orientation.y = 1.0;
  control.name = "move_xy_yaw";
  control.interaction_mode =
      visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;

  // Add a cube marker a little forward from the origin of the interactive
  // marker. This guarantees an easy to manipulate target regardless of the body
  // geometry
  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.25;
  control.markers.push_back(easy_to_click_cube);

  // Also add body markers 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
    control.markers.push_back(transformed_body_marker);
  }

  // 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);

  // 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(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);<commit_msg>Split translation and rotation control between two interactive controls.<commit_after>#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);
{'fraction_non_alphanumeric': 0.09845248267350233, 'fraction_numerical': 0.011107946454001708, 'mean_word_length': 4.360814249363868, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 13, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4037050', 'n_tokens_mistral': 3486, 'n_tokens_neox': 3353, 'n_words': 583}