jsk_interactive_test: publish_goal

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 import roslib; roslib.load_manifest('jsk_interactive_test')
00005 import rospy
00006 import copy
00007 import httplib, urllib2, urllib, json
00008 from socket import gethostname
00009 from getpass import getuser
00010 
00011 
00012 from std_msgs.msg import ColorRGBA
00013 from geometry_msgs.msg import Vector3, Pose, Point, Quaternion, PoseStamped
00014 from visualization_msgs.msg import MarkerArray, Marker
00015 from interactive_markers.interactive_marker_server import *
00016 from interactive_markers.menu_handler import *
00017 from tf import transformations
00018 import numpy, math
00019 
00020 class InteractiveMarkerTest:
00021     poses = []
00022     colors = []
00023     status = []
00024     size = 2
00025     start_time = False
00026     done_time = False
00027 
00028     def recordResult(self, ui_type, time):
00029         #url = 'http://localhost:3000/interactive_marker_test'
00030         url = 'http://jsk-db.herokuapp.com/interactive_marker_test'
00031         params = {'ui_type': ui_type,
00032                   'user': '%s@%s' % (getuser(), gethostname()),
00033                   'time': time}
00034         data = urllib.urlencode(params)
00035         req = urllib2.Request(url, data)
00036         try:
00037             response = urllib2.urlopen(req)
00038         except:
00039             rospy.logfatal('failed to record result')
00040 
00041     def displayResult(self):
00042         url = 'http://jsk-db.herokuapp.com/interactive_marker_test/json'
00043         req = urllib2.Request(url)
00044         try:
00045             response = urllib2.urlopen(req)
00046             data = json.loads(response.read())
00047             rospy.loginfo("======================================================")
00048             rospy.loginfo("   http://jsk-db.herokuapp.com/interactive_marker_test
00049             rospy.loginfo("                user /  ui_type /    time")
00050             for d in data:
00051                 rospy.loginfo("%12s / %8s / %s sec", d['user'], d['ui_type'], d['time'])
00052             rospy.loginfo("======================================================")
00053         except:
00054             rospy.logfatal('failed to record result')
00055 
00056     def processFeedback(self, feedback):
00057         if not self.start_time:
00058             self.start_time = rospy.get_rostime()
00059         self.feedback = feedback
00060         size = self.size
00061         i = min(range(size*size*size),key=lambda i:numpy.linalg.norm(numpy.array([self.poses[i].position.x,self.poses[i].position.y,self.poses[i].position.z])-
00062                                                                      numpy.array([feedback.pose.position.x,feedback.pose.position.y,feedback.pose.position.z])))
00063         p = self.poses[i]
00064         pos = numpy.linalg.norm(numpy.array([p.position.x,p.position.y,p.position.z])-
00065                                 numpy.array([feedback.pose.position.x,feedback.pose.position.y,feedback.pose.position.z]))
00066         #print transformations.quaternion_matrix([p.orientation.x,p.orientation.y,p.orientation.z,p.orientation.w])
00067         #print transformations.quaternion_matrix([feedback.pose.orientation.x,feedback.pose.orientation.y,feedback.pose.orientation.z,feedback.pose.orientation.w])
00068         #print transformations.quaternion_matrix([p.orientation.x,p.orientation.y,p.orientation.z,p.orientation.w])[0:3,2]
00069         #print transformations.quaternion_matrix([feedback.pose.orientation.x,feedback.pose.orientation.y,feedback.pose.orientation.z,feedback.pose.orientation.w])[0:3,2]
00070         rot = math.acos(numpy.dot(transformations.quaternion_matrix([p.orientation.x,p.orientation.y,p.orientation.z,p.orientation.w])[0:3,2],
00071                                   transformations.quaternion_matrix([feedback.pose.orientation.x,feedback.pose.orientation.y,feedback.pose.orientation.z,feedback.pose.orientation.w])[0:3,2]))
00072         #rot = numpy.linalg.norm(rvec)
00073         rospy.loginfo('error %6.3f / %6.3f'%(pos, numpy.rad2deg(rot)))
00074         if pos < self.position_threshold and rot < numpy.deg2rad(self.rotation_threshold):
00075             self.colors[i] = ColorRGBA(1.0,0.2,0.2,0.8)
00076             self.status[i] = rospy.get_rostime() - self.start_time
00077 
00078     def poseCB(self, msg):
00079         self.server.setPose('control', msg.pose, msg.header)
00080         self.server.applyChanges()
00081         self.processFeedback(msg)
00082 
00083     def __init__(self):
00084         pub_goal = rospy.Publisher('interactive_goal_marker', MarkerArray)
00085         rospy.init_node('publish_interactive_goal_marker')
00086         self.ui_type = rospy.get_param('~ui_type')
00087         #
00088         self.displayResult()
00089         # setup
00090         size = self.size
00091         space = 0.5
00092         hand_mesh = 'file://'+roslib.packages.get_pkg_dir('jsk_interactive_test')+'/scripts/RobotHand.dae'
00093         hand_mesh_scale = Vector3(0.2, 0.2, 0.2)
00094         goal_mesh = 'file://'+roslib.packages.get_pkg_dir('jsk_interactive_test')+'/scripts/Bottle.dae'
00095         goal_mesh_scale = Vector3(1.0, 1.0, 1.0)
00096 
00097         self.position_threshold = rospy.get_param('~position_threshold', 0.01) # 1 [cm]
00098         self.rotation_threshold = rospy.get_param('~rotation_threshold', 2)    # 2 [deg]
00099 
00100         rospy.Subscriber('/goal_marker/move_marker', PoseStamped, self.poseCB)
00101         #
00102         for i in range(size*size*size):
00103             x = space * ((i%(size) - size/2) + numpy.random.normal(0,0.1))
00104             y = space * (((i/(size))%size - size/2) + numpy.random.normal(0,0.1))
00105             z = space * ((i/(size*size) - size/2) + numpy.random.normal(0,0.1))
00106             q = transformations.quaternion_from_euler(*numpy.random.normal(0,0.7,3))
00107             self.poses.append(Pose(Point(x,y,z), Quaternion(*q)))
00108             self.colors.append(ColorRGBA(0.8,0.8,0.8,0.8))
00109             self.status.append(False)
00110 
00111         # interactive marker
00112         server = InteractiveMarkerServer("goal_marker")
00113         self.server = server
00114         int_marker = InteractiveMarker()
00115         int_marker.header.frame_id = "/map"
00116         int_marker.name = "control"
00117         int_marker.scale = 0.3
00118 
00119         mesh_marker = Marker()
00120         mesh_marker.type = Marker.MESH_RESOURCE
00121         mesh_marker.scale = hand_mesh_scale
00122         mesh_marker.color = ColorRGBA(0.2,0.2,1.0,0.8)
00123         mesh_marker.mesh_resource = hand_mesh
00124 
00125         target_marker = Marker()
00126         target_marker.type = Marker.CYLINDER
00127         target_marker.scale = Vector3(0.07,0.07,0.1)
00128         target_marker.color = ColorRGBA(0.4,0.4,1.0,0.6)
00129 
00130         mesh_control = InteractiveMarkerControl()
00131         mesh_control.always_visible = True
00132         mesh_control.markers.append(mesh_marker)
00133         mesh_control.markers.append(target_marker)
00134 
00135         int_marker.controls.append(mesh_control)
00136 
00137         if rospy.has_param(rospy.search_param('make_interactive_marker_arrow')) and rospy.get_param(rospy.search_param('make_interactive_marker_arrow')):
00138             control = InteractiveMarkerControl()
00139             control.orientation.w = 1
00140             control.orientation.x = 1
00141             control.orientation.y = 0
00142             control.orientation.z = 0
00143             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00144             int_marker.controls.append(copy.deepcopy(control))
00145             control.orientation.w = 1
00146             control.orientation.x = 0
00147             control.orientation.y = 1
00148             control.orientation.z = 0
00149             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00150             int_marker.controls.append(copy.deepcopy(control))
00151             control.orientation.w = 1
00152             control.orientation.x = 0
00153             control.orientation.y = 0
00154             control.orientation.z = 1
00155             control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00156             int_marker.controls.append(copy.deepcopy(control))
00157             control.orientation.w = 1
00158             control.orientation.x = 1
00159             control.orientation.y = 0
00160             control.orientation.z = 0
00161             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00162             int_marker.controls.append(copy.deepcopy(control))
00163             control.orientation.w = 1
00164             control.orientation.x = 0
00165             control.orientation.y = 1
00166             control.orientation.z = 0
00167             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00168             int_marker.controls.append(copy.deepcopy(control))
00169             control.orientation.w = 1
00170             control.orientation.x = 0
00171             control.orientation.y = 0
00172             control.orientation.z = 1
00173             control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00174             int_marker.controls.append(copy.deepcopy(control))
00175 
00176         server.insert(int_marker, self.processFeedback)
00177         server.applyChanges()
00178 
00179         # start rosmain
00180         r = rospy.Rate(5.0)
00181         while not rospy.is_shutdown():
00182             array = MarkerArray()
00183             for i in range(size*size*size):
00184                 msg = Marker()
00185                 msg.header.stamp = rospy.get_rostime()
00186                 msg.header.frame_id = '/map'
00187                 msg.mesh_use_embedded_materials = False
00188                 msg.mesh_resource = goal_mesh
00189                 msg.type = Marker.MESH_RESOURCE
00190                 msg.scale = goal_mesh_scale
00191                 msg.color = self.colors[i]
00192                 msg.pose = self.poses[i]
00193                 msg.lifetime = rospy.Time(10)
00194                 msg.id = i
00195                 array.markers.append(msg)
00196             if self.done_time:
00197                 msg = Marker()
00198                 msg.header.stamp = rospy.get_rostime()
00199                 msg.header.frame_id = '/map'
00200                 msg.type = Marker.TEXT_VIEW_FACING
00201                 msg.scale.z = 0.14
00202                 msg.color.r = 1.0
00203                 msg.color.g = 1.0
00204                 msg.color.b = 1.0
00205                 msg.color.a = 1.0
00206                 msg.text = 'Done, time = %5.2f'%(self.done_time)
00207                 msg.lifetime = rospy.Time(10)
00208                 msg.id = size*size*size
00209                 array.markers.append(msg)
00210             pub_goal.publish(array)
00211             r.sleep()
00212             if self.start_time:
00213                 rospy.loginfo("%2d/%d %5.2f"%(len(filter(lambda x: x != False, self.status)),len(self.status),float((rospy.get_rostime()-self.start_time).to_sec())))
00214             if all(self.status):
00215                 if self.done_time:
00216                     rospy.loginfo("Done.. %5.2f"%self.done_time)
00217                 else:
00218                     self.done_time = (rospy.get_rostime()-self.start_time).to_sec()
00219                     self.recordResult(self.ui_type, self.done_time)
00220                 for s in self.status:
00221                     rospy.loginfo("%5.2f"%s.to_sec())
00222 
00223 if __name__ == '__main__':
00224     try:
00225         i = InteractiveMarkerTest()
00226     except rospy.ROSInterruptException:
00227         pass
00228