footstep_visualizer.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from hrpsys_ros_bridge.srv import OpenHRP_StabilizerService_getParameter as getParameter
4 import rospy
5 from tf2_py import ExtrapolationException
6 import tf2_ros
7 from geometry_msgs.msg import PointStamped
8 from sensor_msgs.msg import Image
9 
10 from cv_bridge import CvBridge
11 from tf.transformations import *
12 import cv2 as cv
13 import numpy as np
14 from threading import Lock
15 from math import pi, isnan
16 # hrpsys robot only
17 from hrpsys_ros_bridge.msg import ContactStatesStamped, ContactStateStamped, ContactState
18 from jsk_footstep_controller.cfg import FootstepVisualizerConfig
19 from dynamic_reconfigure.server import Server
20 
21 msg_lock = Lock()
22 def matrixFromTranslationQuaternion(trans, q):
23  return concatenate_matrices(translation_matrix(trans),
24  quaternion_matrix(q))
25 
26 def verticesPoints(original_vertices, origin_pose, scale, resolution_size):
27  original_vertices_3d_local = [[v[0], v[1], 0.0] for v in original_vertices]
28  vertices_3d_pose = [concatenate_matrices(origin_pose, translation_matrix(v)) for v in original_vertices_3d_local]
29  vertices_3d = [translation_from_matrix(v) for v in vertices_3d_pose]
30 
31  return [(int(v[0] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0),
32  int(v[1] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0))
33  for v in vertices_3d]
34 
35 def verticesPoints3D(original_vertices, origin_pose, scale, resolution_size):
36  for v in original_vertices:
37  if isnan(v[0]) or isnan(v[1]) or isnan(v[2]):
38  return [False]
39  original_vertices_3d_local = original_vertices
40  vertices_3d_pose = [concatenate_matrices(origin_pose, translation_matrix(v)) for v in original_vertices_3d_local]
41  vertices_3d = [translation_from_matrix(v) for v in vertices_3d_pose]
42 
43 
44  return [(int(v[0] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0),
45  int(v[1] / (scale / 2.0) * resolution_size / 2.0 + resolution_size / 2.0))
46  for v in vertices_3d]
47 
48 def transformToMatrix(transform):
49  return matrixFromTranslationQuaternion([transform.translation.x,
50  transform.translation.y,
51  transform.translation.z],
52  [transform.rotation.x,
53  transform.rotation.y,
54  transform.rotation.z,
55  transform.rotation.w])
56 
57 ACT_CP_COLOR=(192, 202, 164)
58 REF_CP_COLOR=(167, 60, 151)
59 
60 def drawPoint(image, point, size, color, text):
61  cv.circle(image, point, 7, color + (255,), -1)
62  cv.putText(image, text, (point[0] + 5, point[1] + 5),
63  cv.FONT_HERSHEY_PLAIN, 1.0, color + (255,))
64 
65 
66 #def cop_callback(lleg_cop, rleg_cop):
67 def periodicCallback(event):
68  global tf_buffer, lleg_cop_msg, rleg_cop_msg, zmp_msg, act_cp_msg, ref_cp_msg, act_contact_states_msg
69  print("run")
70  with msg_lock:
71  try:
72  _lleg_pose = tf_buffer.lookup_transform(root_link, lleg_end_coords, rospy.Time())
73  _rleg_pose = tf_buffer.lookup_transform(root_link, rleg_end_coords, rospy.Time())
74  if lleg_cop_msg:
75  _lleg_cop_origin = tf_buffer.lookup_transform(root_link, lleg_cop_msg.header.frame_id, rospy.Time())
76  lleg_cop_origin = transformToMatrix(_lleg_cop_origin.transform)
77  lleg_cop_point = np.array([lleg_cop_msg.point.x,
78  lleg_cop_msg.point.y,
79  lleg_cop_msg.point.z])
80  if rleg_cop_msg:
81  _rleg_cop_origin = tf_buffer.lookup_transform(root_link, rleg_cop_msg.header.frame_id, rospy.Time())
82  rleg_cop_origin = transformToMatrix(_rleg_cop_origin.transform)
83  rleg_cop_point = np.array([rleg_cop_msg.point.x,
84  rleg_cop_msg.point.y,
85  rleg_cop_msg.point.z])
86  if zmp_msg:
87  _zmp_origin = tf_buffer.lookup_transform(root_link, zmp_msg.header.frame_id, rospy.Time())
88  zmp_origin = transformToMatrix(_zmp_origin.transform)
89  zmp_point = np.array([zmp_msg.point.x,
90  zmp_msg.point.y,
91  zmp_msg.point.z])
92  if act_cp_msg:
93  _act_cp_origin = tf_buffer.lookup_transform(root_link, act_cp_msg.header.frame_id, rospy.Time())
94  act_cp_origin = transformToMatrix(_act_cp_origin.transform)
95  act_cp_point = np.array([act_cp_msg.point.x,
96  act_cp_msg.point.y,
97  act_cp_msg.point.z])
98  if ref_cp_msg:
99  _ref_cp_origin = tf_buffer.lookup_transform(root_link, ref_cp_msg.header.frame_id, rospy.Time())
100  ref_cp_origin = transformToMatrix(_ref_cp_origin.transform)
101  ref_cp_point = np.array([ref_cp_msg.point.x,
102  ref_cp_msg.point.y,
103  ref_cp_msg.point.z])
104  lleg_pos = np.array([_lleg_pose.transform.translation.x,
105  _lleg_pose.transform.translation.y,
106  _lleg_pose.transform.translation.z])
107  lleg_rot = np.array([_lleg_pose.transform.rotation.x,
108  _lleg_pose.transform.rotation.y,
109  _lleg_pose.transform.rotation.z,
110  _lleg_pose.transform.rotation.w])
111  rleg_pos = np.array([_rleg_pose.transform.translation.x,
112  _rleg_pose.transform.translation.y,
113  _rleg_pose.transform.translation.z])
114  rleg_rot = np.array([_rleg_pose.transform.rotation.x,
115  _rleg_pose.transform.rotation.y,
116  _rleg_pose.transform.rotation.z,
117  _rleg_pose.transform.rotation.w])
118 
119  lleg_pose = matrixFromTranslationQuaternion(lleg_pos, lleg_rot)
120  rleg_pose = matrixFromTranslationQuaternion(rleg_pos, rleg_rot)
121  mid_coords = concatenate_matrices(matrixFromTranslationQuaternion((lleg_pos + rleg_pos) / 2.0,
122  quaternion_slerp(lleg_rot, rleg_rot, 0.5)),
123  euler_matrix(pi, 0, 0))
124  lleg_from_mid = concatenate_matrices(inverse_matrix(mid_coords),
125  lleg_pose)
126  rleg_from_mid = concatenate_matrices(inverse_matrix(mid_coords),
127  rleg_pose)
128  lleg_points = verticesPoints(lleg_vertices, lleg_from_mid, scale, image_size)
129  rleg_points = verticesPoints(rleg_vertices, rleg_from_mid, scale, image_size)
130  lleg_margined_points = verticesPoints(lleg_margined_vertices, lleg_from_mid, scale, image_size)
131  rleg_margined_points = verticesPoints(rleg_margined_vertices, rleg_from_mid, scale, image_size)
132  image = np.zeros((image_size, image_size, 4), dtype=np.uint8)
133  lleg_contact = False
134  rleg_contact = False
135  lleg_color = (0, 255, 0, 255)
136  rleg_color = (0, 0, 255, 255)
137  lleg_width = 2
138  rleg_width = 2
139  if act_contact_states_msg:
140  for state in act_contact_states_msg.states:
141  if state.header.frame_id == "lfsensor":
142  if state.state.state == ContactState.ON:
143  lleg_color = (0, 255, 0, 255)
144  lleg_width = 3
145  lleg_contact = True
146  else:
147  lleg_color = (150, 255, 150, 255)
148  lleg_width = 1
149  if state.header.frame_id == "rfsensor":
150  if state.state.state == ContactState.ON:
151  rleg_color = (0, 0, 255, 255)
152  rleg_width = 3
153  rleg_contact = True
154  else:
155  rleg_color = (150, 150, 255, 255)
156  rleg_width = 1
157  # act_contact_states_msg = None
158  cv.line(image, lleg_points[0], lleg_points[1], lleg_color, lleg_width)
159  cv.line(image, lleg_points[1], lleg_points[2], lleg_color, lleg_width)
160  cv.line(image, lleg_points[2], lleg_points[3], lleg_color, lleg_width)
161  cv.line(image, lleg_points[3], lleg_points[0], lleg_color, lleg_width)
162  cv.line(image, rleg_points[0], rleg_points[1], rleg_color, rleg_width)
163  cv.line(image, rleg_points[1], rleg_points[2], rleg_color, rleg_width)
164  cv.line(image, rleg_points[2], rleg_points[3], rleg_color, rleg_width)
165  cv.line(image, rleg_points[3], rleg_points[0], rleg_color, rleg_width)
166  if lleg_contact and rleg_contact:
167  hull = cv.convexHull(np.array(lleg_points + rleg_points))
168  cv.drawContours(image, [hull], -1, (155, 155, 155, 255), 2)
169  margined_hull = cv.convexHull(np.array(lleg_margined_points + rleg_margined_points))
170  elif lleg_contact:
171  margined_hull = cv.convexHull(np.array(lleg_margined_points))
172  elif rleg_contact:
173  margined_hull = cv.convexHull(np.array(rleg_margined_points))
174  if (lleg_contact or rleg_contact) and g_config.cp_safe_area:
175  cv.drawContours(image, [margined_hull], -1, (255, 255, 255, 255), 2)
176  if lleg_cop_msg and g_config.lcop:
177  lleg_cop_point_2d = verticesPoints3D([lleg_cop_point],
178  concatenate_matrices(inverse_matrix(mid_coords),
179  lleg_cop_origin),
180  scale,
181  image_size)[0]
182  if lleg_cop_point_2d:
183  drawPoint(image, lleg_cop_point_2d, 5, (0, 255, 0), "LCoP")
184  # lleg_cop_msg = None
185  if rleg_cop_msg and g_config.rcop:
186  rleg_cop_point_2d = verticesPoints3D([rleg_cop_point],
187  concatenate_matrices(inverse_matrix(mid_coords),
188  rleg_cop_origin),
189  scale,
190  image_size)[0]
191  if rleg_cop_point_2d:
192  drawPoint(image, rleg_cop_point_2d, 5, (0, 0, 255), "RCoP")
193  # rleg_cop_msg = None
194  if zmp_msg and g_config.zmp:
195  zmp_point_2d = verticesPoints3D([zmp_point],
196  concatenate_matrices(inverse_matrix(mid_coords),
197  zmp_origin),
198  scale,
199  image_size)[0]
200  if zmp_point_2d:
201  drawPoint(image, zmp_point_2d, 5, (0, 255, 255), "ZMP")
202  # zmp_msg = None
203  if ref_cp_msg and g_config.ref_cp:
204  ref_cp_point_2d = verticesPoints3D([ref_cp_point],
205  concatenate_matrices(inverse_matrix(mid_coords),
206  ref_cp_origin),
207  scale,
208  image_size)[0]
209  if ref_cp_point_2d:
210  drawPoint(image, ref_cp_point_2d, 7, REF_CP_COLOR, "RCP")
211  # ref_cp_msg = None
212  if act_cp_msg and g_config.act_cp:
213  act_cp_point_2d = verticesPoints3D([act_cp_point],
214  concatenate_matrices(inverse_matrix(mid_coords),
215  act_cp_origin),
216  scale,
217  image_size)[0]
218  if act_cp_point_2d:
219  drawPoint(image, act_cp_point_2d, 7, ACT_CP_COLOR, "ACP")
220  # act_cp_msg = None
221  bridge = CvBridge()
222  pub.publish(bridge.cv2_to_imgmsg(image, "bgra8"))
223  except ExtrapolationException:
224  rospy.logwarn("tf error")
225  finally:
226  pass
227  # except Exception, e:
228  # print e
229 
230 lleg_cop_msg = None
231 rleg_cop_msg = None
232 zmp_msg = None
233 act_cp_msg = None
234 ref_cp_msg = None
235 act_contact_states_msg = None
236 def llegCopCallback(msg):
237  global lleg_cop_msg
238  with msg_lock:
239  lleg_cop_msg = msg
240 
241 
242 def rlegCopCallback(msg):
243  global rleg_cop_msg
244  with msg_lock:
245  rleg_cop_msg = msg
246 
247 def actCPCallback(msg):
248  global act_cp_msg
249  with msg_lock:
250  act_cp_msg = msg
251 
252 def refCPCallback(msg):
253  global ref_cp_msg
254  with msg_lock:
255  ref_cp_msg = msg
256 
257 def contactStatesCallback(msg):
258  global act_contact_states_msg
259  with msg_lock:
260  act_contact_states_msg = msg
261 
262 def zmpCallback(msg):
263  global zmp_msg
264  with msg_lock:
265  zmp_msg = msg
266 
267 def config_callback(config, level):
268  global g_config
269  g_config = config
270  return config
271 
272 if __name__ == "__main__":
273  rospy.init_node("footstep_visualizer")
274  pub = rospy.Publisher("~output", Image, queue_size=1)
275  srv = Server(FootstepVisualizerConfig, config_callback)
276  tf_buffer = tf2_ros.Buffer()
277  tf_listener = tf2_ros.TransformListener(tf_buffer)
278  rospy.loginfo("wait for st server")
279  rospy.wait_for_service("/StabilizerServiceROSBridge/getParameter")
280  rospy.loginfo("st server found")
281  g_get_parameter_srv = rospy.ServiceProxy("/StabilizerServiceROSBridge/getParameter", getParameter)
282  lleg_end_coords = rospy.get_param("~lleg_end_coords", "lleg_end_coords")
283  rleg_end_coords = rospy.get_param("~rleg_end_coords", "rleg_end_coords")
284  image_size = rospy.get_param("~image_size", 400)
285  scale = rospy.get_param("~scale", 0.6)
286  i_param = g_get_parameter_srv().i_param
287  leg_front_margin = i_param.eefm_leg_front_margin
288  leg_rear_margin = i_param.eefm_leg_rear_margin
289  leg_inside_margin = i_param.eefm_leg_inside_margin
290  leg_outside_margin = i_param.eefm_leg_outside_margin
291  cp_check_margin = i_param.cp_check_margin
292  lleg_vertices = [[leg_front_margin, leg_outside_margin],
293  [leg_front_margin, -1*leg_inside_margin],
294  [-1*leg_rear_margin, -1*leg_inside_margin],
295  [-1*leg_rear_margin, leg_outside_margin]]
296  rleg_vertices = [[leg_front_margin, leg_inside_margin],
297  [leg_front_margin, -1*leg_outside_margin],
298  [-1*leg_rear_margin, -1*leg_outside_margin],
299  [-1*leg_rear_margin, leg_inside_margin]]
300  lleg_margined_vertices = [[leg_front_margin-cp_check_margin[0], leg_outside_margin-cp_check_margin[3]],
301  [leg_front_margin-cp_check_margin[0], -1*(leg_inside_margin-cp_check_margin[2])],
302  [-1*(leg_rear_margin-cp_check_margin[1]), -1*(leg_inside_margin-cp_check_margin[2])],
303  [-1*(leg_rear_margin-cp_check_margin[1]), leg_outside_margin-cp_check_margin[3]]]
304  rleg_margined_vertices = [[leg_front_margin-cp_check_margin[0], leg_inside_margin-cp_check_margin[2]],
305  [leg_front_margin-cp_check_margin[0], -1*(leg_outside_margin-cp_check_margin[3])],
306  [-1*(leg_rear_margin-cp_check_margin[1]), -1*(leg_outside_margin-cp_check_margin[3])],
307  [-1*(leg_rear_margin-cp_check_margin[1]), leg_inside_margin-cp_check_margin[2]]]
308  root_link = rospy.get_param("~root_link", "BODY")
309  lleg_sub = rospy.Subscriber("/lfsensor_cop", PointStamped, llegCopCallback)
310  rleg_sub = rospy.Subscriber("/rfsensor_cop", PointStamped, rlegCopCallback)
311  zmp_sub = rospy.Subscriber("/zmp", PointStamped, zmpCallback)
312  act_cp_point_sub = rospy.Subscriber("/act_capture_point", PointStamped, actCPCallback)
313  ref_cp_point_sub = rospy.Subscriber("/ref_capture_point", PointStamped, refCPCallback)
314  contact_states_sub = rospy.Subscriber("/act_contact_states", ContactStatesStamped, contactStatesCallback)
315  # lleg_cop_sub = Subscriber("/lfsensor_cop", PointStamped)
316  # rleg_cop_sub = Subscriber("/rfsensor_cop", PointStamped)
317  # ts = TimeSynchronizer([lleg_cop_sub, rleg_cop_sub], 10)
318  # ts.registerCallback(cop_callback)
319  rospy.Timer(rospy.Duration(0.1), periodicCallback)
320  rospy.spin()
321 
footstep_visualizer.matrixFromTranslationQuaternion
def matrixFromTranslationQuaternion(trans, q)
Definition: footstep_visualizer.py:22
footstep_visualizer.config_callback
def config_callback(config, level)
Definition: footstep_visualizer.py:267
footstep_visualizer.actCPCallback
def actCPCallback(msg)
Definition: footstep_visualizer.py:247
tf2_ros::Buffer
footstep_visualizer.periodicCallback
def periodicCallback(event)
Definition: footstep_visualizer.py:67
footstep_visualizer.llegCopCallback
def llegCopCallback(msg)
Definition: footstep_visualizer.py:236
footstep_visualizer.contactStatesCallback
def contactStatesCallback(msg)
Definition: footstep_visualizer.py:257
footstep_visualizer.transformToMatrix
def transformToMatrix(transform)
Definition: footstep_visualizer.py:48
footstep_visualizer.drawPoint
def drawPoint(image, point, size, color, text)
Definition: footstep_visualizer.py:60
footstep_visualizer.refCPCallback
def refCPCallback(msg)
Definition: footstep_visualizer.py:252
footstep_visualizer.verticesPoints
def verticesPoints(original_vertices, origin_pose, scale, resolution_size)
Definition: footstep_visualizer.py:26
footstep_visualizer.g_get_parameter_srv
g_get_parameter_srv
Definition: footstep_visualizer.py:281
footstep_visualizer.zmpCallback
def zmpCallback(msg)
Definition: footstep_visualizer.py:262
tf2_ros::TransformListener
footstep_visualizer.rlegCopCallback
def rlegCopCallback(msg)
Definition: footstep_visualizer.py:242
footstep_visualizer.verticesPoints3D
def verticesPoints3D(original_vertices, origin_pose, scale, resolution_size)
Definition: footstep_visualizer.py:35

jsk_footstep_controller
Author(s):
autogenerated on Fri May 14 2021 02:52:04