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()