Ask Your Question
0

How to constrain the pose of an interactive marker in RVIZ?

asked 2016-01-17 11:19:23 -0500

masihec gravatar image

updated 2016-01-17 11:19:48 -0500

Hi,

Do you have any idea about how to constrain the pose of an interactive marker in RVIZ?

I want to stop the shape when it reaches a specified position.

Thank you!

edit retag flag offensive close merge delete

3 Answers

Sort by » oldest newest most voted
0

answered 2016-01-18 12:23:07 -0500

This is possible using the feedback callback described in this tutorial.

Python example:

class myInteractivemarker:
    def __init__(self):
         # Define your marker....
         my_marker = InteractiveMarker()
         ......

          # Set feedback callback
          self.server.insert(my_marker, self.processFeedback)

    def processFeedback(self, feedback):
         # Trim the position in X of the marker
         if feedback.pose.position.x < 0:
             feedback.pose.position.x = 0
         if feedback.pose.position.x > 10:
             feedback.pose.position.x = 10
        # Apply changes to the marker.
         self.server.applyChanges()
edit flag offensive delete link more

Comments

Thank you Javier for your answer!

Actually, i tried as you specified but it doesn't work (no changing is done). I didn't find what is wrong exactly, i'm new in ROS so may be i'm making a mistake somewhere!

I will be grateful if you can help me in this.

Here is the code that i'm using right now.

masihec gravatar imagemasihec ( 2016-01-18 21:51:19 -0500 )edit
0

answered 2016-01-18 21:53:25 -0500

masihec gravatar image

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)
edit flag offensive delete link more
0

answered 2018-10-23 01:17:31 -0500

Sai Krishna gravatar image

updated 2018-10-23 01:19:39 -0500

Hi Masihec, You have to use server.setPose( feedback.marker_name, pose ) to change/constrain the position of Interactive marker. A sample code is given below.

def processFeedback(feedback):
     # Trim the position in X of the marker
     if feedback.pose.position.x < 0:
         feedback.pose.position.x = 0
     if feedback.pose.position.x > 10:
         feedback.pose.position.x = 10
    # Apply changes to the marker.
     server.setPose( feedback.marker_name, feedback.pose)
     server.applyChanges()
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-01-17 11:19:23 -0500

Seen: 483 times

Last updated: Oct 23 '18