footstep_visualizer.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 # hrpsys robot only
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 #def cop_callback(lleg_cop, rleg_cop):
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             # act_contact_states_msg = None
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             # lleg_cop_msg = None
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             # rleg_cop_msg = None
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             # zmp_msg = None
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             # ref_cp_msg = None
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             # act_cp_msg = None
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      # except Exception, e:
00228      #     print e
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     # lleg_cop_sub = Subscriber("/lfsensor_cop", PointStamped)
00316     # rleg_cop_sub = Subscriber("/rfsensor_cop", PointStamped)
00317     # ts = TimeSynchronizer([lleg_cop_sub, rleg_cop_sub], 10)
00318     # ts.registerCallback(cop_callback)
00319     rospy.Timer(rospy.Duration(0.1), periodicCallback)
00320     rospy.spin()
00321     


jsk_footstep_controller
Author(s):
autogenerated on Fri Apr 19 2019 03:45:37