A continuación se tratará de explicar el código en el que se basa este wiki.

Para empezar es necesario la línea que nunca debería faltar en cualquier programa en lenguaje Python:

#!/usr/bin/env python

Luego es necesario es escribir algunas líneas acerca de recursos que se van a necesitar:

import roslib; roslib.load_manifest("interactive_markers")
import rospy
import copy

De especial importancia es la parte roslib.load_manifest(“interactive_markers”), en ésta se invoca un archivo llamado manifest.xml, donde están escritas las diferentes dependencias que requerirá el programa.

Por otra parte se invocan los recursos que permitirán mostrar las figuras en la pantalla y las transformaciones de coordenadas, en otras funciones:

from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from tf.broadcaster import TransformBroadcaster
from random import random
from math import sin

Se inicializan algunas variables que serán necesarias para el desarrollo del programa:

server = None
marker_pos = 0
menu_handler = MenuHandler()
br = None
counter = 0

Ahora se crea una función que muestra la posición del marker en la consola:

 def processFeedback( feedback ):
   s = "Feedback from marker '" + feedback.marker_name
   s += "' / control '" + feedback.control_name + "'"

   mp = ""
   if feedback.mouse_point_valid:
       mp = " at " + str(feedback.mouse_point.x)
       mp += ", " + str(feedback.mouse_point.y)
       mp += ", " + str(feedback.mouse_point.z)
       mp += " in frame " + feedback.header.frame_id

   if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
      rospy.loginfo( s + ": button click" + mp + "." )
   elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
      rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." )
   elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
      rospy.loginfo( s + ": pose changed")
   elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
      rospy.loginfo( s + ": mouse down" + mp + "." )
   elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
      rospy.loginfo( s + ": mouse up" + mp + "." )
   server.applyChanges()

Aquí se define una función que sirve para alinear la pieza de ajedrez:

def alignMarker( feedback ):
  pose = feedback.pose

  pose.position.x = round(pose.position.x-0.5)+0.5
  pose.position.y = round(pose.position.y-0.5)+0.5

  rospy.loginfo( feedback.marker_name + ": aligning position = " + str(feedback.pose.position.x) + "," + str(feedback.pose.position.y) + "," +                       str(feedback.pose.position.z) + " to " +
                                                                   str(pose.position.x) + "," + str(pose.position.y) + "," + str(pose.position.z) )

  server.setPose( feedback.marker_name, pose )
  server.applyChanges()

Luego se empiezan a definir los marcadores interactivos. El primero a definir es el marcador de seis grados de libertad:

def seisGradosdeLibertad( fixed ):
  global marker_pos
  int_marker = InteractiveMarker()
  int_marker.header.frame_id = "/base_link"
  int_marker.pose.position.y = -3.0 * marker_pos
  marker_pos += 1
  int_marker.scale = 1

  int_marker.name = "simple_6dof"
  int_marker.description = "6 Grados Libertad"

Se inserta una caja en el control interactivo:

  makeBoxControl(int_marker)

Se decide si va a estar fija la orientación del marcador:

  if fixed:
      int_marker.name += "_fixed"
      int_marker.description += "\n(fixed orientation)"
      

Ahora de definen los ejes por los cuales el marcador va a trasladarse y rotar, que van a ser los tres ejes básicos:

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 1
  control.orientation.y = 0
  control.orientation.z = 0
  control.name = "rotate_x"
  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 1
  control.orientation.y = 0
  control.orientation.z = 0
  control.name = "move_x"
  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 1
  control.orientation.z = 0
  control.name = "rotate_z"
  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 1
  control.orientation.z = 0
  control.name = "move_z"
  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 0
  control.orientation.z = 1
  control.name = "rotate_y"
  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 0
  control.orientation.z = 1
  control.name = "move_y"
  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
  if fixed:
      control.orientation_mode = InteractiveMarkerControl.FIXED
  int_marker.controls.append(control)

  server.insert(int_marker, processFeedback)

Ahora se procede con el marcador de seis grados de libertad con ejes aleatorios: def seisGradosdeLibertadRandom():

  global marker_pos
  int_marker = InteractiveMarker()
  int_marker.header.frame_id = "/base_link"
  int_marker.pose.position.y = -3.0 * marker_pos
  marker_pos += 1
  int_marker.scale = 1

  int_marker.name = "6dof_random_axes"
  int_marker.description = "6-GradosLibertad\n(Ejes Arbitrarios)"

  makeBoxControl(int_marker)

  control = InteractiveMarkerControl()

  for i in range(3):
      control.orientation.w = rand(-1,1)
      control.orientation.x = rand(-1,1)
      control.orientation.y = rand(-1,1)
      control.orientation.z = rand(-1,1)
      control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
      int_marker.controls.append(copy.deepcopy(control))
      control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
      int_marker.controls.append(control)

  server.insert(int_marker, processFeedback)

El siguiente es el marcador cuyos grados de libertad emulan a los de un cuadrocóptero:

def Cuadrocoptero():

  global marker_pos
  int_marker = InteractiveMarker()
  int_marker.header.frame_id = "/base_link"
  int_marker.pose.position.y = -3.0 * marker_pos
  marker_pos += 1
  int_marker.scale = 1

  int_marker.name = "quadrocopter"
  int_marker.description = "Quadrocopter"

  makeBoxControl(int_marker)

Aquí se usa una copia para no tener que hacer otro control

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 1
  control.orientation.z = 0
  control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
  int_marker.controls.append(copy.deepcopy(control))
  control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
  int_marker.controls.append(control)

  server.insert(int_marker, processFeedback)

Bueno continuando, sigue el marcador de que se mueve en el eje en el que se visualiza el marcador:

def viewFacing():
   global marker_pos
   int_marker = InteractiveMarker()
   int_marker.header.frame_id = "/base_link"
   int_marker.pose.position.y = -3.0 * marker_pos
   marker_pos += 1
   int_marker.scale = 1

   int_marker.name = "view_facing"
   int_marker.description = "View Facing 6-DOF"

Se crea el control que rota en el eje en el que se ve el marcador:

  control = InteractiveMarkerControl()
  control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
  control.orientation.w = 1
  control.name = "rotate"
  int_marker.controls.append(control)

  control = InteractiveMarkerControl()
  control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
  control.independent_marker_orientation = True
  control.name = "move"
  control.markers.append( makeBox(int_marker) )
  control.always_visible = True
  int_marker.controls.append(control)

  server.insert(int_marker, processFeedback)

El siguiente marcador está adherido a un plano que se mueve:

def marcadorPlanoMovil():
  global marker_pos
  int_marker = InteractiveMarker()
  int_marker.header.frame_id = "/moving_frame"
  int_marker.pose.position.y = -3.0 * marker_pos
  marker_pos += 1
  int_marker.scale = 1

  int_marker.name = "moving"
  int_marker.description = "Marcador en un \nMarco Movil"

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 1
  control.orientation.y = 0
  control.orientation.z = 0
  control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
  int_marker.controls.append(copy.deepcopy(control))

  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
  control.always_visible = True
  control.markers.append( makeBox(int_marker) )
  int_marker.controls.append(control)

  server.insert(int_marker, processFeedback)
  

Por último falta definir a la pieza de ajedrez:

def piezadeAjedrez():

  global marker_pos
  int_marker = InteractiveMarker()
  int_marker.header.frame_id = "/base_link"
  int_marker.pose.position.y = -3.0 * marker_pos
  marker_pos += 1
  int_marker.scale = 1

  int_marker.name = "chess_piece"
  int_marker.description = "Chess Piece\n(2D Move + Alignment)"

  control = InteractiveMarkerControl()
  control.orientation.w = 1
  control.orientation.x = 0
  control.orientation.y = 1
  control.orientation.z = 0
  control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
  int_marker.controls.append(copy.deepcopy(control))

  # make a box which also moves in the plane
  control.markers.append( makeBox(int_marker) )
  control.always_visible = True
  int_marker.controls.append(control)

  # we want to use our special callback function
  server.insert(int_marker, processFeedback)

  # set different callback for POSE_UPDATE feedback
  server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE )

Ahora sigue la parte del main donde se muestran las instrucciones que va correr el programa (nodo):

if __name__=="__main__":

La línea anterior es para correr el main si este es el programa ejecutado de manera principalmente y no si es importado:

Se inicializa el nombre del nodo:

   rospy.init_node("basic_controls")
   

Se crea un contador que actualiza las publicaciones:

   rospy.Timer(rospy.Duration(0.01), frameCallback) 
   

Se inicializa el servidor de marcadores interactivos:

server = InteractiveMarkerServer("basic_controls")

Ahora se invocan las funciones ya antes creadas:

  seisGradosdeLibertad( False )
  seisGradosdeLibertad( True )
  seisGradosdeLibertadRandom( )
  viewFacing( )
  Cuadrocoptero( )
  marcadorPlanoMovil()

Ahora se guardan los cambios en la posición de los marcadores:

  server.applyChanges()

En la última línea se cierran los procesos que se estén realizando en el nodo:

  rospy.spin()