00001
00002
00003 import numpy as np
00004 import roslib
00005 roslib.load_manifest('hrl_software_simulation_darpa_m3')
00006 import rospy
00007
00008 import hrl_common_code_darpa_m3.visualization.draw_scene as ds
00009
00010 from hrl_haptic_manipulation_in_clutter_msgs.msg import BodyDraw
00011 from hrl_haptic_manipulation_in_clutter_msgs.msg import JtsDraw
00012 from hrl_msgs.msg import FloatArrayBare
00013 from hrl_lib.transforms import *
00014 import hrl_lib.viz as hv
00015 import threading
00016
00017 from visualization_msgs.msg import Marker
00018
00019
00020 class DrawAll:
00021 def __init__(self):
00022 self.lock = threading.RLock()
00023 rospy.init_node('draw_all_bodies')
00024
00025
00026
00027 while not rospy.is_shutdown():
00028 try:
00029 self.moveable_dimen = rospy.get_param('m3/software_testbed/movable_dimen')
00030 break
00031 except KeyError:
00032 rospy.sleep(0.1)
00033 continue
00034
00035 self.fixed_dimen = rospy.get_param('m3/software_testbed/fixed_dimen')
00036 self.compliant_dimen = rospy.get_param('m3/software_testbed/compliant_dimen')
00037 self.num_fixed = rospy.get_param('m3/software_testbed/num_fixed')
00038 self.num_moveable = rospy.get_param('m3/software_testbed/num_movable')
00039 self.num_compliant = rospy.get_param('m3/software_testbed/num_compliant')
00040 self.num_tot = rospy.get_param('m3/software_testbed/num_total')
00041
00042 self.fixed_ctype = rospy.get_param('m3/software_testbed/fixed_ctype')
00043 self.movable_ctype = rospy.get_param('m3/software_testbed/movable_ctype')
00044
00045 self.goal = np.matrix(rospy.get_param('m3/software_testbed/goal')).T
00046
00047 try:
00048 self.movable_stiffness = rospy.get_param('m3/software_testbed/compliant_stiffness')
00049 except KeyError:
00050 self.movable_stiffness = [-1]*int(self.num_compliant)
00051
00052
00053
00054
00055 while not rospy.is_shutdown():
00056 try:
00057 self.num_links = rospy.get_param('/m3/software_testbed/linkage/num_links')
00058 break
00059 except KeyError:
00060 rospy.sleep(0.1)
00061 continue
00062
00063 self.links_dim = rospy.get_param('/m3/software_testbed/linkage/dimensions')
00064 self.links_shape = rospy.get_param('/m3/software_testbed/linkage/shapes')
00065
00066 self.joints_pos = []
00067 self.active_jts = None
00068 self.constr_jts = None
00069 self.links_pos = []
00070 self.links_rot = []
00071 self.obst_pos = []
00072 self.obst_rot = []
00073 self.draw_obstacles = ds.SceneDraw("sim/viz/obstacles", "/torso_lift_link")
00074 self.draw_links = ds.SceneDraw("sim/viz/linkage", "/torso_lift_link")
00075 self.color_active = [0, 1, 0, 1]
00076 self.color_limit = [1, 0, 0, 1]
00077 self.color_passive = [0, 0, 1, 1]
00078 self.color_links =rospy.get_param('m3/software_testbed/linkage/colors')
00079 rospy.Subscriber("/sim_arm/bodies_visualization", BodyDraw, self.bodies_callback)
00080
00081 self.goal_marker_pub = rospy.Publisher('/epc_skin/viz/goal', Marker)
00082
00083
00084 def bodies_callback(self, msg):
00085 with self.lock:
00086 self.links_pos = msg.link_loc
00087 self.links_rot = msg.link_rot
00088 self.obst_pos = msg.obst_loc
00089 self.obst_rot = msg.obst_rot
00090
00091 def draw_bodies(self):
00092 obst_counter = 0
00093 link_counter = 0
00094 color_active = self.color_active
00095 color_limit = self.color_limit
00096 color_passive = self.color_passive
00097 color_links = self.color_links
00098 jt_size = 0.05
00099
00100 self.lock.acquire()
00101 for i in xrange(len(self.links_pos)):
00102 link_rot_mat = self.draw_links.get_rot_mat(self.links_rot[i].data).T
00103 if self.links_shape[i] == "cube":
00104 shape = self.draw_links.Marker.CUBE
00105 dims = self.links_dim[i]
00106 elif self.links_shape[i] == "capsule":
00107 shape = self.draw_links.Marker.CYLINDER
00108 dims = self.links_dim[i]
00109
00110 dim_jt = [self.links_dim[i][0], self.links_dim[i][0], self.links_dim[i][0]]
00111 jt_pos_loc = link_rot_mat*np.array([0, 0, self.links_dim[i][2]/2.0]).reshape(3,1)
00112 jt_pos_plus = np.array(self.links_pos[i].data).reshape(3,1) + jt_pos_loc
00113 jt_pos_neg = np.array(self.links_pos[i].data).reshape(3,1) - jt_pos_loc
00114 self.draw_links.pub_body(jt_pos_plus, matrix_to_quaternion(link_rot_mat), dim_jt, color_links[i], link_counter, self.draw_links.Marker.SPHERE)
00115 self.draw_links.pub_body(jt_pos_neg, matrix_to_quaternion(link_rot_mat), dim_jt, color_links[i], link_counter+1, self.draw_links.Marker.SPHERE)
00116 link_counter=link_counter+2
00117
00118 self.draw_links.pub_body(self.links_pos[i].data, matrix_to_quaternion(self.draw_links.get_rot_mat(self.links_rot[i].data).T),
00119 dims, color_links[i], link_counter, shape)
00120 link_counter = link_counter + 1
00121
00122 if self.obst_pos != []:
00123 for i in xrange(self.num_moveable):
00124 color = [0.6,0.6,0,0.7]
00125 self.draw_obstacles.pub_body(self.obst_pos[i].data,
00126 matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[i].data).T),
00127 [self.moveable_dimen[i][0]*2, self.moveable_dimen[i][1]*2, self.moveable_dimen[i][2]],
00128 color,
00129 obst_counter,
00130 self.draw_obstacles.Marker.CYLINDER)
00131 obst_counter = obst_counter + 1
00132
00133 for i in xrange(self.num_compliant):
00134 color = [0, 100/255.0, 0, 0.9]
00135 self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+i].data,
00136 matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+i].data).T),
00137 [self.compliant_dimen[i][0]*2, self.compliant_dimen[i][1]*2, self.compliant_dimen[i][2]],
00138 color,
00139 obst_counter,
00140 self.draw_obstacles.Marker.CYLINDER)
00141 obst_counter = obst_counter + 1
00142
00143
00144 for i in xrange(self.num_fixed):
00145 color2 = [0.3, 0.,0.,0.7]
00146
00147 if self.fixed_ctype[i] == 'wall':
00148
00149 obst_rot_mat = self.draw_links.get_rot_mat(self.obst_rot[i].data).T
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167 self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+self.num_compliant+i].data,
00168 matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+self.num_compliant+i].data).T),
00169 [self.fixed_dimen[i][0], self.fixed_dimen[i][1], self.fixed_dimen[i][2]],
00170 color2,
00171 obst_counter,
00172 self.draw_obstacles.Marker.CUBE)
00173 else:
00174 self.draw_obstacles.pub_body(self.obst_pos[self.num_moveable+self.num_compliant+i].data,
00175 matrix_to_quaternion(self.draw_obstacles.get_rot_mat(self.obst_rot[self.num_moveable+self.num_compliant+i].data).T),
00176 [self.fixed_dimen[i][0]*2, self.fixed_dimen[i][1]*2, self.fixed_dimen[i][2]],
00177 color2,
00178 obst_counter,
00179 self.draw_obstacles.Marker.CYLINDER)
00180 obst_counter = obst_counter + 1
00181
00182
00183 self.lock.release()
00184
00185 def publish_goal_marker(self, goal_pos):
00186 o = np.matrix([0.,0.,0.,1.]).T
00187 g_m = hv.single_marker(goal_pos, o, 'sphere',
00188 '/world', color=(0., 1., 1., 1.),
00189 scale = (0.02, 0.02, 0.02),
00190 m_id = 23, duration=0)
00191 g_m.header.stamp = rospy.Time.now()
00192 self.goal_marker_pub.publish(g_m)
00193
00194
00195
00196
00197 if __name__ == '__main__':
00198 draw_stuff = DrawAll()
00199 rospy.loginfo('Started visualizing bodies!')
00200 while not rospy.is_shutdown():
00201 draw_stuff.draw_bodies()
00202 draw_stuff.publish_goal_marker(draw_stuff.goal)
00203 rospy.sleep(0.1)
00204
00205
00206
00207