Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('reactive_grasping_pr2')
00004 import rospy
00005 from interactive_markers.interactive_marker_server import *
00006 import copy
00007
00008 from interactive_markers.menu_handler import *
00009
00010 from geometry_msgs.msg import Pose
00011 from motld.msg import TrackedObjects
00012
00013 class SimulatedGoals:
00014 def __init__(self):
00015
00016 self.obj_pub = rospy.Publisher("objects", TrackedObjects)
00017
00018 self.last_green = None
00019 self.last_red = None
00020 self.server = InteractiveMarkerServer("simulated_goals")
00021 self.makeMarker( 'red', [1,0,0] )
00022 self.makeMarker( 'green', [0,1,0] )
00023 self.server.applyChanges()
00024
00025
00026 def processFeedback( self, feedback ):
00027 obj = TrackedObjects()
00028 rospy.loginfo("feedback")
00029 obj.name = [1, 2]
00030 obj.width = [0, 0]
00031 obj.height = [0, 0]
00032 obj.pose.header = feedback.header
00033 obj.pose.header.stamp = rospy.Time.now()
00034 obj.pose.poses = []
00035
00036 ps = Pose()
00037 ps.position.x = feedback.pose.position.x
00038 ps.position.y = feedback.pose.position.y
00039 ps.position.z = feedback.pose.position.z
00040 ps.orientation.w = 1.;
00041
00042 if feedback.marker_name == "red":
00043 rospy.loginfo("publishing")
00044 if self.last_green:
00045 obj.pose.poses.append(self.last_green)
00046 else:
00047 obj.pose.poses.append(Pose())
00048 obj.pose.poses.append(ps)
00049 self.obj_pub.publish(obj)
00050 self.last_red = ps
00051 elif feedback.marker_name == "green":
00052 rospy.loginfo("publishing")
00053 obj.pose.poses.append(ps)
00054 if self.last_red:
00055 obj.pose.poses.append(self.last_red)
00056 else:
00057 obj.pose.poses.append(Pose())
00058 self.obj_pub.publish(obj)
00059 self.last_green = ps
00060 else:
00061 rospy.loginfo("invalid marker name %s" % (feedback.marker_name))
00062
00063
00064 self.server.applyChanges()
00065
00066 def makeMarker( self, name, color ):
00067 int_marker = InteractiveMarker()
00068 int_marker.header.frame_id = "/head_mount_kinect_rgb_optical_frame"
00069 int_marker.scale = 1
00070 int_marker.name = name
00071 int_marker.description = name
00072
00073 marker = Marker()
00074
00075 marker.type = Marker.SPHERE
00076 marker.scale.x = 0.05
00077 marker.scale.y = 0.05
00078 marker.scale.z = 0.05
00079 marker.color.r = color[0]
00080 marker.color.g = color[1]
00081 marker.color.b = color[2]
00082 marker.color.a = 1.0
00083
00084 control = InteractiveMarkerControl()
00085 control.always_visible = True
00086 control.markers.append( marker )
00087
00088 int_marker.controls.append( control )
00089
00090 control = InteractiveMarkerControl()
00091 control.orientation.w = 1
00092 control.orientation.x = 1
00093 control.orientation.y = 0
00094 control.orientation.z = 0
00095 control.name = "move_x"
00096 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00097 int_marker.controls.append(control)
00098
00099 control = InteractiveMarkerControl()
00100 control.orientation.w = 1
00101 control.orientation.x = 0
00102 control.orientation.y = 1
00103 control.orientation.z = 0
00104 control.name = "move_z"
00105 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00106 int_marker.controls.append(control)
00107
00108 control = InteractiveMarkerControl()
00109 control.orientation.w = 1
00110 control.orientation.x = 0
00111 control.orientation.y = 0
00112 control.orientation.z = 1
00113 control.name = "move_y"
00114 control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00115 int_marker.controls.append(control)
00116
00117 self.server.insert(int_marker, self.processFeedback)
00118
00119
00120
00121 if __name__=="__main__":
00122 rospy.init_node("simulated_goals")
00123 ball_server = SimulatedGoals()
00124
00125 rospy.spin()
00126
