def __init__(self,
js_topic,
obs_topic,
integration_factor=0.05,
iterations=30,
visualize=False,
use_timer=True):
self.km_client = GeometryModel() # ModelClient(GeometryModel)
self._js_msg = ValueMapMsg()
self._integration_factor = integration_factor
self._iterations = iterations
self._use_timer = use_timer
self._unintialized_poses = set()
self.aliases = {}
self.tracked_poses = {}
self.integrator = None
self.lock = RLock()
self.soft_constraints = {}
self.joints = set()
self.joint_aliases = {}
self.visualizer = ROSVisualizer('/tracker_vis',
'world') if visualize else None
self.pub_js = rospy.Publisher(js_topic, ValueMapMsg, queue_size=1)
self.sub_obs = rospy.Subscriber(obs_topic,
PSAMsg,
self.cb_process_obs,
queue_size=5)
def __init__(self,
tracker: Kineverse6DEKFTracker,
model=None,
model_path=None,
urdf_param='/ekf_description_check'):
self.tracker = tracker
self.last_observation = {}
self.last_update = None
self.last_control = {s: 0.0 for s in self.tracker.controls}
self.str_controls = {str(s) for s in self.tracker.controls}
self.vis = ROSVisualizer('~vis', 'world')
if model is not None:
self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
model, Path('ekf'))
else:
self.broadcaster = None
self.pub_state = rospy.Publisher('~state_estimate',
ValueMapMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_observations = rospy.Subscriber('~observations',
TransformStampedArrayMsg,
callback=self.cb_obs,
queue_size=1)
self.sub_controls = rospy.Subscriber('~controls',
ValueMapMsg,
callback=self.cb_controls,
queue_size=1)
def __init__(self, multibody, camera_link, fov, near, far, noise_exp, topic_prefix='', frequency=30, debug=False):
super(PoseObservationPublisher, self).__init__('PoseObservationPublisher')
self.topic_prefix = topic_prefix
self.publisher = rospy.Publisher('{}/pose_obs'.format(topic_prefix), PSAMsg, queue_size=1, tcp_nodelay=True)
self.message_templates = {}
self.multibody = multibody
self.camera_link = camera_link
self.fov = fov
self.near = near
self.far = far
self.noise_exp = noise_exp
self._enabled = True
self.visualizer = ROSVisualizer('pose_obs_viz', 'world') if debug else None
self._last_update = 1000
self._update_wait = 1.0 / frequency
def __init__(self,
tracker: Kineverse6DQPTracker,
model=None,
model_path=None,
reference_frame='world',
urdf_param='/qp_description_check',
update_freq=30,
observation_alias=None):
self.tracker = tracker
self.last_observation = 0
self.last_update = 0
self.reference_frame = reference_frame
self.observation_aliases = {o: o for o in tracker.observation_names}
if observation_alias is not None:
for path, alias in observation_alias.items():
if path in self.observation_aliases:
self.observation_aliases[alias] = path
self.str_controls = {str(s) for s in self.tracker.get_controls()}
self.vis = ROSVisualizer('~vis', reference_frame)
if model is not None:
self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
model, Path('ekf'))
else:
self.broadcaster = None
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
self.pub_state = rospy.Publisher('~state_estimate',
ValueMapMsg,
queue_size=1,
tcp_nodelay=True)
self.sub_observations = rospy.Subscriber('~observations',
TransformStampedArrayMsg,
callback=self.cb_obs,
queue_size=1)
self.sub_controls = rospy.Subscriber('~controls',
ValueMapMsg,
callback=self.cb_controls,
queue_size=1)
self.timer = rospy.Timer(rospy.Duration(1 / update_freq),
self.cb_update)
for x in range(100):
temp = state.copy()
temp.update(cmd)
state.update({s: subs(e, temp) for s, e in int_rules.items()})
diff_points.append((state[x1], state[y1], state[a1]))
# md_points.append((state[x2], state[y2], state[a2]))
# if qpb.equilibrium_reached():
# print('optimization ende early')
# break
full_diff_points.append(diff_points)
vis = ROSVisualizer('diff_drive_vis', 'world')
# md_p = [point3(x, y, 0) for x, y, _ in md_points]
# md_d = [vector3(cos(a), sin(a), 0) for _, _, a in md_points]
vis.begin_draw_cycle('paths')
# vis.draw_sphere('paths', goal, 0.02, r=0, b=1)
for n, diff_points in enumerate(full_diff_points):
diff_p = [point3(x, y, 0) + vector3((n / 3) * 0.5, (n % 3)* -0.5, 0) for x, y, _ in diff_points]
diff_d = [vector3(cos(a), sin(a), 0) for _, _, a in diff_points]
vis.draw_strip('paths', cm.eye(4), 0.02, diff_p)
#vis.draw_strip('paths', spw.eye(4), 0.02, md_p, r=0, g=1)
vis.render('paths')
rospy.sleep(0.3)
from kineverse.utils import real_quat_from_matrix
def get_rot_vector(frame):
qx, qy, qz, _ = real_quat_from_matrix(frame)
axis = vector3(qx, qy, qz)
sin_a2 = norm(axis)
a = asin(sin_a2) * 2
return axis * (a / sin_a2)
if __name__ == '__main__':
rospy.init_node('axis_angle_vis')
vis = ROSVisualizer('axis_vis', 'world')
az, ay = [Position(x) for x in 'ax az'.split(' ')]
frame_rpy = frame3_rpy(0, ay, az, point3(0,0,0))
state = {ay: 0, az: 0}
points = [point3(0,0,0) + get_rot_vector(frame_rpy.subs({ay: sin(v), az: cos(v)})) for v in [(3.14512 / 25) * x for x in range(51)]]
vis.begin_draw_cycle('points')
vis.draw_strip('points', se.eye(4), 0.03, points)
vis.render('points')
rospy.sleep(1)
timer = Time()