コード例 #1
0
ファイル: navgraph.cpp プロジェクト: jmlich/fawkes
/** Set default properties on node for which no local value exists.
 * This sets all default properties on the node, for which no
 * property of the same name has been set on the node.
 * @param node node to apply default properties to
 */
void
NavGraph::apply_default_properties(NavGraphNode &node)
{
  for (const auto &p : default_properties_) {
    if (! node.has_property(p.first)) {
      node.set_property(p.first, p.second);
    }
  }
}
コード例 #2
0
void
NavGraphInteractiveThread::process_node_feedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
  switch ( feedback->event_type )
  {
    case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
      logger->log_debug(name(), "%s/%s: menu item %u clicked",
			feedback->marker_name.c_str(), feedback->control_name.c_str(),
			feedback->menu_entry_id);
      {
	visualization_msgs::InteractiveMarker int_marker;
	if (server_->get(feedback->marker_name, int_marker)) {
	  if (node_menus_.find(int_marker.name) != node_menus_.end()) {
	    NodeMenu &menu = node_menus_[int_marker.name];
	    if (feedback->menu_entry_id == menu.ori_handle) {
	      process_node_ori_feedback(feedback, menu, int_marker);
	    } else if (feedback->menu_entry_id == menu.goto_handle) {
	      if (nav_if_->has_writer()) {
		NavigatorInterface::PlaceGotoMessage *gotomsg =
		  new NavigatorInterface::PlaceGotoMessage(int_marker.name.c_str());
		nav_if_->msgq_enqueue(gotomsg);
	      } else {
		logger->log_error(name(), "Cannot goto %s, no writer for interface",
				  int_marker.name.c_str());
	      }
	    } else if (feedback->menu_entry_id == menu.remove_handle) {
	      navgraph.lock();
	      navgraph->remove_node(feedback->marker_name);
	      navgraph->calc_reachability(true);
	      navgraph.unlock();
	      server_->erase(feedback->marker_name);
	      server_->applyChanges();
	    } else if (menu.undir_connect_nodes.find(feedback->menu_entry_id) != menu.undir_connect_nodes.end()) {
	      std::string to_node = menu.undir_connect_nodes[feedback->menu_entry_id];
	      NavGraphEdge edge(int_marker.name, to_node);
	      navgraph.lock();
	      try {
		navgraph->add_edge(edge);
		navgraph->calc_reachability(true);
	      } catch (Exception &e) {
		navgraph.unlock();
		logger->log_error(name(), "Failed to insert edge %s--%s as requested, exception follows",
				  int_marker.name.c_str(), to_node.c_str());
		logger->log_error(name(), e);
	      }
	      server_->erase(int_marker.name);
	      add_node(navgraph->node(int_marker.name), *navgraph);
	      server_->erase(to_node);
	      add_node(navgraph->node(to_node), *navgraph);
	      navgraph.unlock();
	      server_->applyChanges();

	    } else if (menu.dir_connect_nodes.find(feedback->menu_entry_id) != menu.dir_connect_nodes.end()) {
	      NavGraphEdge edge(int_marker.name,
				menu.dir_connect_nodes[feedback->menu_entry_id]);
	      edge.set_directed(true);
	      navgraph.lock();
	      try {
		navgraph->add_edge(edge);
		navgraph->calc_reachability(true);
	      } catch (Exception &e) {
		navgraph.unlock();
		logger->log_error(name(), "Failed to insert edge %s->%s as requested, "
				  "exception follows", int_marker.name.c_str(),
				  menu.dir_connect_nodes[feedback->menu_entry_id].c_str());
		logger->log_error(name(), e);
	      }
	      server_->erase(int_marker.name);
	      add_node(navgraph->node(int_marker.name), *navgraph);
	      navgraph.unlock();
	      server_->applyChanges();

	    } else if (menu.disconnect_nodes.find(feedback->menu_entry_id) != menu.disconnect_nodes.end()) {
	      navgraph.lock();
	      std::string to_node = menu.disconnect_nodes[feedback->menu_entry_id];
	      NavGraphEdge edge = navgraph->edge(feedback->marker_name, to_node);
	      if (! edge) {
		logger->log_error(name(), "Failed to find edge %s--%s",
				  feedback->marker_name.c_str(), to_node.c_str());
	      }
	      navgraph->remove_edge(edge);
	      navgraph->calc_reachability(true);
	      server_->erase(feedback->marker_name);
	      add_node(navgraph->node(feedback->marker_name), *navgraph);
	      if (! edge.is_directed()) {
		server_->erase(to_node);
		add_node(navgraph->node(to_node), *navgraph);
	      }
	      navgraph.unlock();
	      server_->applyChanges();
	    } else {
	      logger->log_warn(name(), "Got menu feedback for %s/%s, but marker not known",
			       feedback->marker_name.c_str(), feedback->control_name.c_str());
	    }
	    
	  } else {
	    logger->log_warn(name(), "Got feedback for %s/%s, but marker not known",
			     feedback->marker_name.c_str(), feedback->control_name.c_str());
	  }
	}
      }
      break;

    case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
      if (feedback->header.frame_id == cfg_global_frame_) {
	navgraph.lock();
	NavGraphNode node = navgraph->node(feedback->marker_name);
	if (node) {

	  if (feedback->control_name == "rot_z") {
	    // orientation update
	    tf::Quaternion q(feedback->pose.orientation.x, feedback->pose.orientation.y,
			     feedback->pose.orientation.z, feedback->pose.orientation.w);
	    node.set_property(navgraph::PROP_ORIENTATION, (float)tf::get_yaw(q));
	  } else {
	    // position update
	    node.set_x(feedback->pose.position.x);
	    node.set_y(feedback->pose.position.y);
	  }
	  navgraph->update_node(node);
	} else {
	  logger->log_warn(name(), "Position update for %s, but not unknown",
			   feedback->marker_name.c_str());
	}
	navgraph.unlock();
	navgraph->notify_of_change();
      } else {
	logger->log_warn(name(), "Interactive marker feedback in non-global frame %s, ignoring",
			 feedback->header.frame_id.c_str());
      }
      break;

    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
    case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
      break;
  }

  server_->applyChanges();
}