def __init__(self):
rospy.init_node('odom_pose')
self._map_frame = tf_frame_normalize(
rospy.get_param('~map_frame', 'map'))
self._odom_frame = tf_frame_normalize(
rospy.get_param('~odom_frame', 'odom'))
self._frame_prefix = rospy.get_param('~frame_prefix', 'odo')
frame_list = rospy.get_param('~frame_list', '[base_link, laser_link]')
frame_list = frame_list.replace('[', '')
frame_list = frame_list.replace(']', '')
frame_list = frame_list.strip()
frame_list = frame_list.split(',')
frame_list = [tf_frame_normalize(frame) for frame in frame_list]
if self._odom_frame not in frame_list:
frame_list.append(self._odom_frame)
self._frame_list = frame_list
rospy.Subscriber("tf", TFMessage, self._tf_callback, queue_size=1)
self._pub_tf = rospy.Publisher("tf", TFMessage, queue_size=1)
rospy.spin()
def __init__(self):
"""
Constructor
"""
rospy.init_node('gt_mapping')
self._tf_listener = tf.TransformListener()
self._map_frame = tf_frame_normalize(rospy.get_param("~map_frame", "map"))
self._ref_frame = tf_frame_normalize(rospy.get_param("~ref_frame", "map"))
max_scan_buffer_len = rospy.get_param("~max_scan_buffer_len", 1000)
self._occ_threshold = rospy.get_param("~occ_threshold", 0.25)
self._standalone = rospy.get_param("~standalone", False)
map_resolution = rospy.get_param("~resolution", 0.05)
self._sub_map = rospy.Subscriber("/map", OccupancyGrid, self._map_callback)
self._sub_eos = rospy.Subscriber("endOfSim", Bool, self._eos_callback)
self._gen_map = rospy.Subscriber("genMap", Bool, self._gen_map_callback)
self._sub_scan = rospy.Subscriber("/GT/base_scan", LaserScan, self._sensor_callback)
self._sub_doLoc = rospy.Subscriber("doLocOnly", Bool, self._loc_only_callback)
self._pub_map = rospy.Publisher("/GT/map", OccupancyGrid, queue_size=1)
if self._standalone:
self._map_resolution = map_resolution
else:
self._map_resolution = None
self._scan_buffer = deque(maxlen=max_scan_buffer_len)
self._map_hits = defaultdict(int)
self._map_visits = defaultdict(int)
self._width = 0
self._height = 0
self._min_x = None
self._min_y = None
self._max_x = None
self._max_y = None
self._map_origin = np.zeros(2)
self._msg_seq = 0
self._loc_only = False
rospy.spin()
def _add_scan_messages(self,
noisy_measurements,
det_measurements=None,
add_gt=True,
add_odom=True):
"""
Function for adding all scan messages (Noisy pose, GT pose, Odom pose) at once.
:param noisy_measurements: (ndarray) 2D Array of noisy measurements to be published.
Measured ranges must be in measurements[:, 1]
:param det_measurements: (ndarray)[Default: None] 2D Array of deterministic measurements to be published.
Measured ranges must be in measurements[:, 1].
If None, then ground truth won't be published,
even if add_gt is True.
:param add_gt: (bool)[Default: True] Add scan message from ground truth pose frame if True
and det_measurements is not None.
:param add_odom: (bool)[Default: True] Add scan messages from odom pose frame if True.
:return: None
"""
if self._bag is None:
return
topic = str(self._params['scan_topic'])
frame = str(self._params['laser_frame'])
self.__add_scan_msg(topic, frame, noisy_measurements)
if add_gt and det_measurements is not None:
gt_prefix = str(self._params['gt_prefix'])
gt_topic = '/' + tf_frame_normalize(tf_frame_join(
gt_prefix, topic))
gt_frame = tf_frame_normalize(tf_frame_join(gt_prefix, frame))
self.__add_scan_msg(gt_topic, gt_frame, det_measurements)
if add_odom:
odo_prefix = str(self._params['odo_prefix'])
odo_topic = '/' + tf_frame_normalize(
tf_frame_join(odo_prefix, topic))
odo_frame = tf_frame_normalize(tf_frame_join(odo_prefix, frame))
self.__add_scan_msg(odo_topic, odo_frame, noisy_measurements)
def _tf_callback(self, msg):
"""
Function called whenever a TF message is received.
If the message involves a pair of desired tf frames, it will publish a map->new_odom transform,
and republish the desired tf messages with a prefix added to the frames
:param msg: (tf2_msgs.TFMessage) TF Messages
:return: None
"""
transforms = []
seq = None
ts = rospy.Time.now()
for transform in msg.transforms:
if seq is None:
seq = transform.header.seq
elif transform.header.seq < seq:
seq = transform.header.seq
if transform.header.stamp.secs < ts.secs or\
(transform.header.stamp.secs == ts.secs and transform.header.stamp.nsecs < ts.nsecs):
ts = transform.header.stamp
pframe = tf_frame_normalize(transform.header.frame_id)
cframe = tf_frame_normalize(transform.child_frame_id)
if pframe in self._frame_list and cframe in self._frame_list:
new_pframe = tf_frame_normalize(
tf_frame_join(self._frame_prefix, pframe))
new_cframe = tf_frame_normalize(
tf_frame_join(self._frame_prefix, cframe))
transform.header.frame_id = new_pframe
transform.child_frame_id = new_cframe
transforms.append(transform)
if transforms:
new_tf_msg = TFMessage()
map_odom_tf = TransformStamped()
map_odom_tf.header.stamp = ts
map_odom_tf.header.seq = seq
map_odom_tf.header.frame_id = self._map_frame
map_odom_tf.child_frame_id = tf_frame_normalize(
tf_frame_join(self._frame_prefix, self._odom_frame))
map_odom_tf.transform.translation = _zero_trans
map_odom_tf.transform.rotation.x = _zero_rot[0]
map_odom_tf.transform.rotation.y = _zero_rot[1]
map_odom_tf.transform.rotation.z = _zero_rot[2]
map_odom_tf.transform.rotation.w = _zero_rot[3]
new_tf_msg.transforms.append(map_odom_tf)
for transform in transforms:
new_tf_msg.transforms.append(transform)
self._pub_tf.publish(new_tf_msg)
def __add_tf_msg(self,
msg,
real_pose=False,
tf_prefix="",
update_laser_tf=True,
publish_map_odom=False):
"""
Appends the tf transforms to the passed message with the real/noisy pose of the robot
and (optionally) the laser sensor pose.
:param msg: (tf2_msgs.TFMessage) A TF message to append all the transforms to.
:param real_pose: (bool)[Default: False] Publish real pose if True, noisy pose if False.
:param tf_prefix: (string)[Default: ""] Prefix to be prepended to each TF Frame
:param update_laser_tf: (bool)[Default: True] Publish base_link->laser_link tf if True.
:param publish_map_odom: (bool)[Default: False] Publish the map->odom tf transform if True.
:return: None
"""
ts = self._current_time
seq = self._tf_msg_seq
odom_frame = tf_frame_normalize(
tf_frame_join(tf_prefix, str(self._params['odom_frame'])))
base_frame = tf_frame_normalize(
tf_frame_join(tf_prefix, str(self._params['base_frame'])))
if publish_map_odom:
map_frame = str(self._params['map_frame'])
zero_pos = Point(0.0, 0.0, 0.0)
zero_rot = quaternion_from_euler(0.0, 0.0, 0.0)
self.__add_transform(msg, ts, seq, map_frame, odom_frame, zero_pos,
zero_rot)
if real_pose:
pos_x = float(self._real_pose.position[0])
pos_y = float(self._real_pose.position[1])
theta = float(self._real_pose.orientation)
else:
pos_x = float(self._noisy_pose.position[0])
pos_y = float(self._noisy_pose.position[1])
theta = float(self._noisy_pose.orientation)
odom_pos = Point(pos_x, pos_y, 0.0)
odom_rot = quaternion_from_euler(0.0, 0.0, theta)
self.__add_transform(msg, ts, seq, odom_frame, base_frame, odom_pos,
odom_rot)
if update_laser_tf:
laser_frame = tf_frame_normalize(
tf_frame_join(tf_prefix, str(self._params['laser_frame'])))
lp_x = float(self._params['base_to_laser_tf'][0][0])
lp_y = float(self._params['base_to_laser_tf'][0][1])
lp_th = float(self._params['base_to_laser_tf'][1][0])
laser_pos = Point(lp_x, lp_y, 0.0)
laser_rot = quaternion_from_euler(0.0, 0.0, lp_th)
self.__add_transform(msg, ts, seq, base_frame, laser_frame,
laser_pos, laser_rot)