Javier here is my code! The code that i'm using for controlling the pose is between #####################################################################
Controlling the pose of the interactive marker
and
#####################################################################
End of the control
#!/usr/bin/env python
import rospy
import copy
from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from visualization_msgs.msg import *
from geometry_msgs.msg import Point
from tf.broadcaster import TransformBroadcaster
from random import random
from math import sin
server = None
menu_handler = MenuHandler()
br = None
counter = 0
rayon = 0.05
tang_alfa = 0.05/0.1222
z_cone = 0.036
z_guide = 0.1
Fa= 12
Fr = 0
K= 10000
def frameCallback( msg ):
global counter, br
time = rospy.Time.now()
br.sendTransform( (0, 0, sin(counter/140.0)*2.0), (0, 0, 0, 1.0), time, "base_link", "moving_frame" )
counter += 1
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 + "." )
#####################################################################
Controlling the pose of the interactive marker
Fr = -K*feedback.pose.position.x
if (feedback.pose.position.x<=2*rayon and (feedback.pose.position.x)*(feedback.pose.position.x)+(feedback.pose.position.y)*(feedback.pose.position.y)> rayon*rayon):
feedback.pose.position.x -= feedback.pose.position.x
feedback.pose.position.y -= feedback.pose.position.y
feedback.pose.position.z -= feedback.pose.position.z
if (feedback.pose.position.x>=2*rayon):
Fr = Fa
feedback.pose.position.x -= feedback.pose.position.x
feedback.pose.position.y -= feedback.pose.position.y
feedback.pose.position.z -= feedback.pose.position.z
if ((z>z_cone) and (z<z_cone+z_guide) and (feedback.pose.position.x)*(feedback.pose.position.x)+(feedback.pose.position.y)*(feedback.pose.position.y)-(feedback.pose.position.z)*(feedback.pose.position.z)*tang_alfa>=0):
feedback.pose.position.x -= feedback.pose.position.x
feedback.pose.position.y -= feedback.pose.position.y
feedback.pose.position.z -= feedback.pose.position.z
server.applyChanges()
#####################################################################
End of the control
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()
def rand( min_, max_ ):
return min_ + random()*(max_-min_)
def makeBox( msg ):
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = msg.scale * 0.01
marker.scale.y = msg.scale * 0.01
marker.scale.z = msg.scale * 0.01
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b ...
(more)