00001
00002
00003 from hrpsys_ros_bridge.srv import OpenHRP_StabilizerService_getParameter as getParameter
00004 import rospy
00005 from tf2_py import ExtrapolationException
00006 import tf2_ros
00007 from geometry_msgs.msg import PointStamped
00008 from sensor_msgs.msg import Image
00009
00010 from cv_bridge import CvBridge
00011 from tf.transformations import *
00012 import cv2 as cv
00013 import numpy as np
00014 from threading import Lock
00015 from math import pi, isnan
00016
00017 from hrpsys_ros_bridge.msg import ContactStatesStamped, ContactStateStamped, ContactState
00018 from jsk_footstep_controller.cfg import FootstepVisualizerConfig
00019 from dynamic_reconfigure.server import Server
00020
00021 msg_lock = Lock()
00022 def matrixFromTranslationQuaternion(trans, q):
00023 return concatenate_matrices(translation_matrix(trans),
00024 quaternion_matrix(q))
00025
00026 def verticesPoints(original_vertices, origin_pose, scale, resolution_size):
00027 original_vertices_3d_local = [[v[0], v[1], 0.0] for v in original_vertices]
00028 vertices_3d_pose = [concatenate_matrices(origin_pose, translation_matrix(v)) for v in original_vertices_3d_local]
00029 vertices_3d = [translation_from_matrix(v) for v in vertices_3d_pose]
00030
00031 return [(int(v[0] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0),
00032 int(v[1] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0))
00033 for v in vertices_3d]
00034
00035 def verticesPoints3D(original_vertices, origin_pose, scale, resolution_size):
00036 for v in original_vertices:
00037 if isnan(v[0]) or isnan(v[1]) or isnan(v[2]):
00038 return [False]
00039 original_vertices_3d_local = original_vertices
00040 vertices_3d_pose = [concatenate_matrices(origin_pose, translation_matrix(v)) for v in original_vertices_3d_local]
00041 vertices_3d = [translation_from_matrix(v) for v in vertices_3d_pose]
00042
00043
00044 return [(int(v[0] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0),
00045 int(v[1] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0))
00046 for v in vertices_3d]
00047
00048 def transformToMatrix(transform):
00049 return matrixFromTranslationQuaternion([transform.translation.x,
00050 transform.translation.y,
00051 transform.translation.z],
00052 [transform.rotation.x,
00053 transform.rotation.y,
00054 transform.rotation.z,
00055 transform.rotation.w])
00056
00057 ACT_CP_COLOR=(192, 202, 164)
00058 REF_CP_COLOR=(167, 60, 151)
00059
00060 def drawPoint(image, point, size, color, text):
00061 cv.circle(image, point, 7, color + (255,), -1)
00062 cv.putText(image, text, (point[0] + 5, point[1] + 5),
00063 cv.FONT_HERSHEY_PLAIN, 1.0, color + (255,))
00064
00065
00066
00067 def periodicCallback(event):
00068 global tf_buffer, lleg_cop_msg, rleg_cop_msg, zmp_msg, act_cp_msg, ref_cp_msg, act_contact_states_msg
00069 print "run"
00070 with msg_lock:
00071 try:
00072 _lleg_pose = tf_buffer.lookup_transform(root_link, lleg_end_coords, rospy.Time())
00073 _rleg_pose = tf_buffer.lookup_transform(root_link, rleg_end_coords, rospy.Time())
00074 if lleg_cop_msg:
00075 _lleg_cop_origin = tf_buffer.lookup_transform(root_link, lleg_cop_msg.header.frame_id, rospy.Time())
00076 lleg_cop_origin = transformToMatrix(_lleg_cop_origin.transform)
00077 lleg_cop_point = np.array([lleg_cop_msg.point.x,
00078 lleg_cop_msg.point.y,
00079 lleg_cop_msg.point.z])
00080 if rleg_cop_msg:
00081 _rleg_cop_origin = tf_buffer.lookup_transform(root_link, rleg_cop_msg.header.frame_id, rospy.Time())
00082 rleg_cop_origin = transformToMatrix(_rleg_cop_origin.transform)
00083 rleg_cop_point = np.array([rleg_cop_msg.point.x,
00084 rleg_cop_msg.point.y,
00085 rleg_cop_msg.point.z])
00086 if zmp_msg:
00087 _zmp_origin = tf_buffer.lookup_transform(root_link, zmp_msg.header.frame_id, rospy.Time())
00088 zmp_origin = transformToMatrix(_zmp_origin.transform)
00089 zmp_point = np.array([zmp_msg.point.x,
00090 zmp_msg.point.y,
00091 zmp_msg.point.z])
00092 if act_cp_msg:
00093 _act_cp_origin = tf_buffer.lookup_transform(root_link, act_cp_msg.header.frame_id, rospy.Time())
00094 act_cp_origin = transformToMatrix(_act_cp_origin.transform)
00095 act_cp_point = np.array([act_cp_msg.point.x,
00096 act_cp_msg.point.y,
00097 act_cp_msg.point.z])
00098 if ref_cp_msg:
00099 _ref_cp_origin = tf_buffer.lookup_transform(root_link, ref_cp_msg.header.frame_id, rospy.Time())
00100 ref_cp_origin = transformToMatrix(_ref_cp_origin.transform)
00101 ref_cp_point = np.array([ref_cp_msg.point.x,
00102 ref_cp_msg.point.y,
00103 ref_cp_msg.point.z])
00104 lleg_pos = np.array([_lleg_pose.transform.translation.x,
00105 _lleg_pose.transform.translation.y,
00106 _lleg_pose.transform.translation.z])
00107 lleg_rot = np.array([_lleg_pose.transform.rotation.x,
00108 _lleg_pose.transform.rotation.y,
00109 _lleg_pose.transform.rotation.z,
00110 _lleg_pose.transform.rotation.w])
00111 rleg_pos = np.array([_rleg_pose.transform.translation.x,
00112 _rleg_pose.transform.translation.y,
00113 _rleg_pose.transform.translation.z])
00114 rleg_rot = np.array([_rleg_pose.transform.rotation.x,
00115 _rleg_pose.transform.rotation.y,
00116 _rleg_pose.transform.rotation.z,
00117 _rleg_pose.transform.rotation.w])
00118
00119 lleg_pose = matrixFromTranslationQuaternion(lleg_pos, lleg_rot)
00120 rleg_pose = matrixFromTranslationQuaternion(rleg_pos, rleg_rot)
00121 mid_coords = concatenate_matrices(matrixFromTranslationQuaternion((lleg_pos + rleg_pos) / 2.0,
00122 quaternion_slerp(lleg_rot, rleg_rot, 0.5)),
00123 euler_matrix(pi, 0, 0))
00124 lleg_from_mid = concatenate_matrices(inverse_matrix(mid_coords),
00125 lleg_pose)
00126 rleg_from_mid = concatenate_matrices(inverse_matrix(mid_coords),
00127 rleg_pose)
00128 lleg_points = verticesPoints(lleg_vertices, lleg_from_mid, scale, image_size)
00129 rleg_points = verticesPoints(rleg_vertices, rleg_from_mid, scale, image_size)
00130 lleg_margined_points = verticesPoints(lleg_margined_vertices, lleg_from_mid, scale, image_size)
00131 rleg_margined_points = verticesPoints(rleg_margined_vertices, rleg_from_mid, scale, image_size)
00132 image = np.zeros((image_size, image_size, 4), dtype=np.uint8)
00133 lleg_contact = False
00134 rleg_contact = False
00135 lleg_color = (0, 255, 0, 255)
00136 rleg_color = (0, 0, 255, 255)
00137 lleg_width = 2
00138 rleg_width = 2
00139 if act_contact_states_msg:
00140 for state in act_contact_states_msg.states:
00141 if state.header.frame_id == "lfsensor":
00142 if state.state.state == ContactState.ON:
00143 lleg_color = (0, 255, 0, 255)
00144 lleg_width = 3
00145 lleg_contact = True
00146 else:
00147 lleg_color = (150, 255, 150, 255)
00148 lleg_width = 1
00149 if state.header.frame_id == "rfsensor":
00150 if state.state.state == ContactState.ON:
00151 rleg_color = (0, 0, 255, 255)
00152 rleg_width = 3
00153 rleg_contact = True
00154 else:
00155 rleg_color = (150, 150, 255, 255)
00156 rleg_width = 1
00157
00158 cv.line(image, lleg_points[0], lleg_points[1], lleg_color, lleg_width)
00159 cv.line(image, lleg_points[1], lleg_points[2], lleg_color, lleg_width)
00160 cv.line(image, lleg_points[2], lleg_points[3], lleg_color, lleg_width)
00161 cv.line(image, lleg_points[3], lleg_points[0], lleg_color, lleg_width)
00162 cv.line(image, rleg_points[0], rleg_points[1], rleg_color, rleg_width)
00163 cv.line(image, rleg_points[1], rleg_points[2], rleg_color, rleg_width)
00164 cv.line(image, rleg_points[2], rleg_points[3], rleg_color, rleg_width)
00165 cv.line(image, rleg_points[3], rleg_points[0], rleg_color, rleg_width)
00166 if lleg_contact and rleg_contact:
00167 hull = cv.convexHull(np.array(lleg_points + rleg_points))
00168 cv.drawContours(image, [hull], -1, (155, 155, 155, 255), 2)
00169 margined_hull = cv.convexHull(np.array(lleg_margined_points + rleg_margined_points))
00170 elif lleg_contact:
00171 margined_hull = cv.convexHull(np.array(lleg_margined_points))
00172 elif rleg_contact:
00173 margined_hull = cv.convexHull(np.array(rleg_margined_points))
00174 if (lleg_contact or rleg_contact) and g_config.cp_safe_area:
00175 cv.drawContours(image, [margined_hull], -1, (255, 255, 255, 255), 2)
00176 if lleg_cop_msg and g_config.lcop:
00177 lleg_cop_point_2d = verticesPoints3D([lleg_cop_point],
00178 concatenate_matrices(inverse_matrix(mid_coords),
00179 lleg_cop_origin),
00180 scale,
00181 image_size)[0]
00182 if lleg_cop_point_2d:
00183 drawPoint(image, lleg_cop_point_2d, 5, (0, 255, 0), "LCoP")
00184
00185 if rleg_cop_msg and g_config.rcop:
00186 rleg_cop_point_2d = verticesPoints3D([rleg_cop_point],
00187 concatenate_matrices(inverse_matrix(mid_coords),
00188 rleg_cop_origin),
00189 scale,
00190 image_size)[0]
00191 if rleg_cop_point_2d:
00192 drawPoint(image, rleg_cop_point_2d, 5, (0, 0, 255), "RCoP")
00193
00194 if zmp_msg and g_config.zmp:
00195 zmp_point_2d = verticesPoints3D([zmp_point],
00196 concatenate_matrices(inverse_matrix(mid_coords),
00197 zmp_origin),
00198 scale,
00199 image_size)[0]
00200 if zmp_point_2d:
00201 drawPoint(image, zmp_point_2d, 5, (0, 255, 255), "ZMP")
00202
00203 if ref_cp_msg and g_config.ref_cp:
00204 ref_cp_point_2d = verticesPoints3D([ref_cp_point],
00205 concatenate_matrices(inverse_matrix(mid_coords),
00206 ref_cp_origin),
00207 scale,
00208 image_size)[0]
00209 if ref_cp_point_2d:
00210 drawPoint(image, ref_cp_point_2d, 7, REF_CP_COLOR, "RCP")
00211
00212 if act_cp_msg and g_config.act_cp:
00213 act_cp_point_2d = verticesPoints3D([act_cp_point],
00214 concatenate_matrices(inverse_matrix(mid_coords),
00215 act_cp_origin),
00216 scale,
00217 image_size)[0]
00218 if act_cp_point_2d:
00219 drawPoint(image, act_cp_point_2d, 7, ACT_CP_COLOR, "ACP")
00220
00221 bridge = CvBridge()
00222 pub.publish(bridge.cv2_to_imgmsg(image, "bgra8"))
00223 except ExtrapolationException:
00224 rospy.logwarn("tf error")
00225 finally:
00226 pass
00227
00228
00229
00230 lleg_cop_msg = None
00231 rleg_cop_msg = None
00232 zmp_msg = None
00233 act_cp_msg = None
00234 ref_cp_msg = None
00235 act_contact_states_msg = None
00236 def llegCopCallback(msg):
00237 global lleg_cop_msg
00238 with msg_lock:
00239 lleg_cop_msg = msg
00240
00241
00242 def rlegCopCallback(msg):
00243 global rleg_cop_msg
00244 with msg_lock:
00245 rleg_cop_msg = msg
00246
00247 def actCPCallback(msg):
00248 global act_cp_msg
00249 with msg_lock:
00250 act_cp_msg = msg
00251
00252 def refCPCallback(msg):
00253 global ref_cp_msg
00254 with msg_lock:
00255 ref_cp_msg = msg
00256
00257 def contactStatesCallback(msg):
00258 global act_contact_states_msg
00259 with msg_lock:
00260 act_contact_states_msg = msg
00261
00262 def zmpCallback(msg):
00263 global zmp_msg
00264 with msg_lock:
00265 zmp_msg = msg
00266
00267 def config_callback(config, level):
00268 global g_config
00269 g_config = config
00270 return config
00271
00272 if __name__ == "__main__":
00273 rospy.init_node("footstep_visualizer")
00274 pub = rospy.Publisher("~output", Image)
00275 srv = Server(FootstepVisualizerConfig, config_callback)
00276 tf_buffer = tf2_ros.Buffer()
00277 tf_listener = tf2_ros.TransformListener(tf_buffer)
00278 rospy.loginfo("wait for st server")
00279 rospy.wait_for_service("/StabilizerServiceROSBridge/getParameter")
00280 rospy.loginfo("st server found")
00281 g_get_parameter_srv = rospy.ServiceProxy("/StabilizerServiceROSBridge/getParameter", getParameter)
00282 lleg_end_coords = rospy.get_param("~lleg_end_coords", "lleg_end_coords")
00283 rleg_end_coords = rospy.get_param("~rleg_end_coords", "rleg_end_coords")
00284 image_size = rospy.get_param("~image_size", 400)
00285 scale = rospy.get_param("~scale", 0.6)
00286 i_param = g_get_parameter_srv().i_param
00287 leg_front_margin = i_param.eefm_leg_front_margin
00288 leg_rear_margin = i_param.eefm_leg_rear_margin
00289 leg_inside_margin = i_param.eefm_leg_inside_margin
00290 leg_outside_margin = i_param.eefm_leg_outside_margin
00291 cp_check_margin = i_param.cp_check_margin
00292 lleg_vertices = [[leg_front_margin, leg_outside_margin],
00293 [leg_front_margin, -1*leg_inside_margin],
00294 [-1*leg_rear_margin, -1*leg_inside_margin],
00295 [-1*leg_rear_margin, leg_outside_margin]]
00296 rleg_vertices = [[leg_front_margin, leg_inside_margin],
00297 [leg_front_margin, -1*leg_outside_margin],
00298 [-1*leg_rear_margin, -1*leg_outside_margin],
00299 [-1*leg_rear_margin, leg_inside_margin]]
00300 lleg_margined_vertices = [[leg_front_margin-cp_check_margin[0], leg_outside_margin-cp_check_margin[3]],
00301 [leg_front_margin-cp_check_margin[0], -1*(leg_inside_margin-cp_check_margin[2])],
00302 [-1*(leg_rear_margin-cp_check_margin[1]), -1*(leg_inside_margin-cp_check_margin[2])],
00303 [-1*(leg_rear_margin-cp_check_margin[1]), leg_outside_margin-cp_check_margin[3]]]
00304 rleg_margined_vertices = [[leg_front_margin-cp_check_margin[0], leg_inside_margin-cp_check_margin[2]],
00305 [leg_front_margin-cp_check_margin[0], -1*(leg_outside_margin-cp_check_margin[3])],
00306 [-1*(leg_rear_margin-cp_check_margin[1]), -1*(leg_outside_margin-cp_check_margin[3])],
00307 [-1*(leg_rear_margin-cp_check_margin[1]), leg_inside_margin-cp_check_margin[2]]]
00308 root_link = rospy.get_param("~root_link", "BODY")
00309 lleg_sub = rospy.Subscriber("/lfsensor_cop", PointStamped, llegCopCallback)
00310 rleg_sub = rospy.Subscriber("/rfsensor_cop", PointStamped, rlegCopCallback)
00311 zmp_sub = rospy.Subscriber("/zmp", PointStamped, zmpCallback)
00312 act_cp_point_sub = rospy.Subscriber("/act_capture_point", PointStamped, actCPCallback)
00313 ref_cp_point_sub = rospy.Subscriber("/ref_capture_point", PointStamped, refCPCallback)
00314 contact_states_sub = rospy.Subscriber("/act_contact_states", ContactStatesStamped, contactStatesCallback)
00315
00316
00317
00318
00319 rospy.Timer(rospy.Duration(0.1), periodicCallback)
00320 rospy.spin()
00321