def __init__(self):
stats_top = rospy.get_param('~pf_stats_top', 'stats')
self.stats_sub = rospy.Subscriber(stats_top, numpy_msg(Floats),
self.stat_cb)
self.path_img = rospy.get_param('~background_img_path',
'default_real_mean_depth.png')
self.map_frame = rospy.get_param('~map_frame', 'map')
self.odom_frame = rospy.get_param('~odom_frame', 'odom')
self.survey_name = rospy.get_param('~survey_name', 'survey')
# Real mbes pings subscriber
mbes_pings_top = rospy.get_param("~mbes_pings_topic", 'mbes_pings')
# PF ping subscriber
pf_pings_top = rospy.get_param("~particle_sim_mbes_topic",
'pf_mbes_pings')
self.real_pings_sub = message_filters.Subscriber(
mbes_pings_top, PointCloud2)
self.pf_pings_sub = message_filters.Subscriber(pf_pings_top,
PointCloud2)
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.real_pings_sub, self.pf_pings_sub],
20,
slop=20.0,
allow_headerless=False)
self.ts.registerCallback(self.ping_cb)
self.pings_vec = np.zeros((1, 6))
self.filter_cnt = 1
self.datagram_size = 17
self.filt_vec = np.zeros((self.datagram_size, 1))
self.img = plt.imread(self.path_img)
# Map to odom transform to plot AUV pose on top of image
tfBuffer = tf2_ros.Buffer()
tf2_ros.TransformListener(tfBuffer)
try:
rospy.loginfo("Waiting for transforms")
m2o_tf = tfBuffer.lookup_transform(self.map_frame, self.odom_frame,
rospy.Time(0),
rospy.Duration(35))
self.m2o_mat = matrix_from_tf(m2o_tf)
rospy.loginfo("Transforms locked - stats node")
except:
rospy.logerr("Stats node: Could not lookup transforms")
# When the survey is finished, save the data to disk
finished_top = rospy.get_param("~survey_finished_top",
'/survey_finished')
self.synch_pub = rospy.Subscriber(finished_top, Bool, self.synch_cb)
self.survey_finished = False
self.cov_traces = [0.]
rospy.spin()
def update(self, meas_mbes, odom):
# Compute AUV MBES ping ranges
particle_tf = Transform()
particle_tf.translation = odom.pose.pose.position
particle_tf.rotation = odom.pose.pose.orientation
tf_mat = matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat, np.matmul(tf_mat, self.base2mbes_mat))
# this will not work in simulation ***
mbes_meas_ranges = self.ping2ranges(meas_mbes)
self.pf_mbes_pub.publish(meas_mbes)
# Measurement update of each particle
for i in range(0, self.pc, 8):
for j in range(i, i + 8):
if j < self.pc:
mbes_goal = self.particles[j].get_mbes_goal()
#print(mbes_goal)
self.ac_mbes[j].send_goal(mbes_goal)
else:
break
for j in range(i, i + 8):
if j < self.pc:
if self.ac_mbes[j].wait_for_result(rospy.Duration(0.03)):
mbes_res = self.ac_mbes[j].get_result()
got_result = True
else:
mbes_res = None
got_result = False
self.particles[j].meas_update(mbes_res, mbes_meas_ranges,
got_result)
else:
break
weights = []
for i in range(self.pc):
weights.append(self.particles[i].w)
self.miss_meas = weights.count(1.e-50)
weights_array = np.asarray(weights)
# Add small non-zero value to avoid hitting zero
weights_array += 1.e-30
return weights_array
def update(self, meas_mbes, odom):
# Compute AUV MBES ping ranges
particle_tf = Transform()
particle_tf.translation = odom.pose.pose.position
particle_tf.rotation = odom.pose.pose.orientation
tf_mat = matrix_from_tf(particle_tf)
m2auv = np.matmul(self.m2o_mat, np.matmul(tf_mat, self.base2mbes_mat))
mbes_meas_ranges = pcloud2ranges(meas_mbes, m2auv)
# Measurement update of each particle
weights = []
for i in range(self.pc):
self.particles[i].meas_update(mbes_meas_ranges)
weights.append(self.particles[i].w)
weights_array = np.asarray(weights)
# Add small non-zero value to avoid hitting zero
weights_array += 1.e-30
return weights_array
def __init__(self):
# Read necessary parameters
self.pc = rospy.get_param('~particle_count', 10) # Particle Count
map_frame = rospy.get_param('~map_frame', 'map') # map frame_id
odom_frame = rospy.get_param('~odom_frame', 'odom')
meas_model_as = rospy.get_param('~mbes_as',
'/mbes_sim_server') # map frame_id
mbes_pc_top = rospy.get_param("~particle_sim_mbes_topic", '/sim_mbes')
beams_num = rospy.get_param("~num_beams_sim", 20)
self.beams_real = rospy.get_param("~n_beams_mbes", 512)
# Initialize tf listener
tfBuffer = tf2_ros.Buffer()
tf2_ros.TransformListener(tfBuffer)
try:
rospy.loginfo("Waiting for transforms")
mbes_tf = tfBuffer.lookup_transform('hugin/base_link',
'hugin/mbes_link',
rospy.Time(0),
rospy.Duration(20))
self.base2mbes_mat = matrix_from_tf(mbes_tf)
m2o_tf = tfBuffer.lookup_transform(map_frame, odom_frame,
rospy.Time(0),
rospy.Duration(20))
self.m2o_mat = matrix_from_tf(m2o_tf)
rospy.loginfo("Transforms locked - pf node")
except:
rospy.loginfo(
"ERROR: Could not lookup transform from base_link to mbes_link"
)
# Read covariance values
meas_cov = float(rospy.get_param('~measurement_covariance', 0.01))
cov_string = rospy.get_param('~motion_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
motion_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~init_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
init_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~resampling_noise_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
self.res_noise_cov = list(map(float, cov_list))
# Initialize list of particles
self.particles = np.empty(self.pc, dtype=object)
for i in range(self.pc):
self.particles[i] = Particle(beams_num,
self.pc,
i,
self.base2mbes_mat,
self.m2o_mat,
init_cov=init_cov,
meas_cov=meas_cov,
process_cov=motion_cov,
map_frame=map_frame,
odom_frame=odom_frame,
meas_as=meas_model_as,
pc_mbes_top=mbes_pc_top)
#Initialize an ac per particle for the mbes updates
self.ac_mbes = np.empty(self.pc, dtype=object)
self.nr_of_callbacks = 0
for i in range(self.pc):
self.ac_mbes[i] = actionlib.SimpleActionClient(
meas_model_as, MbesSimAction)
self.ac_mbes[i].wait_for_server()
self.time = None
self.old_time = None
self.pred_odom = None
self.latest_mbes = PointCloud2()
self.prev_mbes = PointCloud2()
self.poses = PoseArray()
self.poses.header.frame_id = odom_frame
self.avg_pose = PoseWithCovarianceStamped()
self.avg_pose.header.frame_id = odom_frame
# Initialize particle poses publisher
pose_array_top = rospy.get_param("~particle_poses_topic",
'/particle_poses')
self.pf_pub = rospy.Publisher(pose_array_top, PoseArray, queue_size=10)
# Initialize average of poses publisher
avg_pose_top = rospy.get_param("~average_pose_topic", '/average_pose')
self.avg_pub = rospy.Publisher(avg_pose_top,
PoseWithCovarianceStamped,
queue_size=10)
# Establish subscription to mbes pings message
mbes_pings_top = rospy.get_param("~mbes_pings_topic", 'mbes_pings')
rospy.Subscriber(mbes_pings_top, PointCloud2, self.mbes_callback)
# Establish subscription to odometry message (intentionally last)
odom_top = rospy.get_param("~odometry_topic", 'odom')
rospy.Subscriber(odom_top, Odometry, self.odom_callback)
# Expected meas of PF outcome at every time step
pf_mbes_top = rospy.get_param("~average_mbes_topic", '/avg_mbes')
self.pf_mbes_pub = rospy.Publisher(pf_mbes_top,
PointCloud2,
queue_size=1)
self.stats = rospy.Publisher('/pf/n_eff', Float32, queue_size=10)
rospy.loginfo("Particle filter class successfully created")
self.update_rviz()
rospy.spin()
def __init__(self):
# Read necessary parameters
self.pc = rospy.get_param('~particle_count', 10) # Particle Count
self.map_frame = rospy.get_param('~map_frame', 'map') # map frame_id
self.mbes_frame = rospy.get_param('~mbes_link',
'mbes_link') # mbes frame_id
odom_frame = rospy.get_param('~odom_frame', 'odom')
meas_model_as = rospy.get_param('~mbes_as',
'/mbes_sim_server') # map frame_id
self.beams_num = rospy.get_param("~num_beams_sim", 20)
self.beams_real = rospy.get_param("~n_beams_mbes", 512)
self.mbes_angle = rospy.get_param("~mbes_open_angle",
np.pi / 180. * 60.)
# Initialize tf listener
tfBuffer = tf2_ros.Buffer()
tf2_ros.TransformListener(tfBuffer)
# Read covariance values
meas_std = float(rospy.get_param('~measurement_std', 0.01))
cov_string = rospy.get_param('~motion_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
motion_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~init_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
init_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~resampling_noise_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
self.res_noise_cov = list(map(float, cov_list))
# Global variables
self.pred_odom = None
self.n_eff_filt = 0.
self.n_eff_mask = [self.pc] * 3
self.latest_mbes = PointCloud2()
self.prev_mbes = PointCloud2()
self.poses = PoseArray()
self.poses.header.frame_id = odom_frame
self.avg_pose = PoseWithCovarianceStamped()
self.avg_pose.header.frame_id = odom_frame
# Initialize particle poses publisher
pose_array_top = rospy.get_param("~particle_poses_topic",
'/particle_poses')
self.pf_pub = rospy.Publisher(pose_array_top, PoseArray, queue_size=10)
# Initialize average of poses publisher
avg_pose_top = rospy.get_param("~average_pose_topic", '/average_pose')
self.avg_pub = rospy.Publisher(avg_pose_top,
PoseWithCovarianceStamped,
queue_size=10)
# Expected meas of PF outcome at every time step
pf_mbes_top = rospy.get_param("~average_mbes_topic", '/avg_mbes')
self.pf_mbes_pub = rospy.Publisher(pf_mbes_top,
PointCloud2,
queue_size=1)
stats_top = rospy.get_param('~pf_stats_top', 'stats')
self.stats = rospy.Publisher(stats_top,
numpy_msg(Floats),
queue_size=10)
mbes_pc_top = rospy.get_param("~particle_sim_mbes_topic", '/sim_mbes')
self.pcloud_pub = rospy.Publisher(mbes_pc_top,
PointCloud2,
queue_size=10)
# Load mesh
svp_path = rospy.get_param('~sound_velocity_prof')
mesh_path = rospy.get_param('~mesh_path')
if svp_path.split('.')[1] != 'cereal':
sound_speeds = csv_data.csv_asvp_sound_speed.parse_file(svp_path)
else:
sound_speeds = csv_data.csv_asvp_sound_speed.read_data(svp_path)
data = np.load(mesh_path)
V, F, bounds = data['V'], data['F'], data['bounds']
print("Mesh loaded")
# Create draper
self.draper = base_draper.BaseDraper(V, F, bounds, sound_speeds)
self.draper.set_ray_tracing_enabled(False)
data = None
V = None
F = None
bounds = None
sound_speeds = None
print("draper created")
print("Size of draper: ", sys.getsizeof(self.draper))
# Load GP
gp_path = rospy.get_param("~gp_path", 'gp.path')
self.gp = gp.SVGP.load(1000, gp_path)
print("Size of GP: ", sys.getsizeof(self.gp))
# Action server for MBES pings sim (necessary to be able to use UFO maps as well)
sim_mbes_as = rospy.get_param('~mbes_sim_as', '/mbes_sim_server')
self.as_ping = actionlib.SimpleActionServer('/mbes_sim_server',
MbesSimAction,
execute_cb=self.mbes_as_cb,
auto_start=True)
# Subscription to real mbes pings
mbes_pings_top = rospy.get_param("~mbes_pings_topic", 'mbes_pings')
rospy.Subscriber(mbes_pings_top, PointCloud2, self.mbes_real_cb)
# Establish subscription to odometry message (intentionally last)
odom_top = rospy.get_param("~odometry_topic", 'odom')
rospy.Subscriber(odom_top, Odometry, self.odom_callback)
# Create expected MBES beams directions
angle_step = self.mbes_angle / self.beams_num
self.beams_dir = []
for i in range(0, self.beams_num):
roll_step = rotation_matrix(-self.mbes_angle / 2. + angle_step * i,
(1, 0, 0))[0:3, 0:3]
rot = roll_step[:, 2]
self.beams_dir.append(rot / np.linalg.norm(rot))
# Shift for fast ray tracing in 2D
beams = np.asarray(self.beams_dir)
n_beams = len(self.beams_dir)
self.beams_dir_2d = np.concatenate(
(beams[:, 0].reshape(n_beams, 1), np.roll(
beams[:, 1:3], 1, axis=1).reshape(n_beams, 2)),
axis=1)
self.beams_dir_2d = np.array([1, -1, 1]) * self.beams_dir_2d
# Start to play survey data. Necessary to keep the PF and auv_2_ros in synch
synch_top = rospy.get_param("~synch_topic", '/pf_synch')
self.synch_pub = rospy.Publisher(synch_top, Bool, queue_size=10)
msg = Bool()
msg.data = True
# Transforms from auv_2_ros
try:
rospy.loginfo("Waiting for transforms")
mbes_tf = tfBuffer.lookup_transform('hugin/base_link',
'hugin/mbes_link',
rospy.Time(0),
rospy.Duration(35))
self.base2mbes_mat = matrix_from_tf(mbes_tf)
m2o_tf = tfBuffer.lookup_transform(self.map_frame, odom_frame,
rospy.Time(0),
rospy.Duration(35))
self.m2o_mat = matrix_from_tf(m2o_tf)
rospy.loginfo("Transforms locked - pf node")
except:
rospy.loginfo(
"ERROR: Could not lookup transform from base_link to mbes_link"
)
# Initialize list of particles
self.particles = np.empty(self.pc, dtype=object)
for i in range(self.pc):
self.particles[i] = Particle(self.beams_num,
self.pc,
i,
self.base2mbes_mat,
self.m2o_mat,
init_cov=init_cov,
meas_std=meas_std,
process_cov=motion_cov)
# PF filter created. Start auv_2_ros survey playing
rospy.loginfo("Particle filter class successfully created")
self.synch_pub.publish(msg)
finished_top = rospy.get_param("~survey_finished_top",
'/survey_finished')
self.finished_sub = rospy.Subscriber(finished_top, Bool, self.synch_cb)
self.survey_finished = False
# Start timing now
self.time = rospy.Time.now().to_sec()
self.old_time = rospy.Time.now().to_sec()
# Create particle to compute DR
self.dr_particle = Particle(self.beams_num,
self.pc,
self.pc + 1,
self.base2mbes_mat,
self.m2o_mat,
init_cov=[0.] * 6,
meas_std=meas_std,
process_cov=motion_cov)
rospy.spin()
def __init__(self):
# Read necessary parameters
param = rospy.search_param("particle_count")
self.pc = rospy.get_param(param) # Particle Count
param = rospy.search_param("map_frame")
map_frame = rospy.get_param(param) # map frame_id
# Initialize connection to MbesSim action server
self.ac_mbes = actionlib.SimpleActionClient('/mbes_sim_server',
MbesSimAction)
rospy.loginfo("Waiting for MbesSim action server")
self.ac_mbes.wait_for_server()
rospy.loginfo("Connected MbesSim action server")
# Initialize tf listener
tfBuffer = tf2_ros.Buffer()
tf2_ros.TransformListener(tfBuffer)
try:
rospy.loginfo("Waiting for transform from base_link to mbes_link")
mbes_tf = tfBuffer.lookup_transform('hugin/mbes_link',
'hugin/base_link',
rospy.Time.now(),
rospy.Duration(10))
mbes_matrix = matrix_from_tf(mbes_tf)
rospy.loginfo(
"Transform locked from base_link to mbes_link - pf node")
except:
rospy.loginfo(
"ERROR: Could not lookup transform from base_link to mbes_link"
)
# Read covariance values
param = rospy.search_param("measurement_covariance")
meas_cov = float(rospy.get_param(param))
param = rospy.search_param("motion_covariance")
cov_string = rospy.get_param(param)
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
predict_cov = list(map(float, cov_list)) # [xv_cov, yv_cov, yaw_v_cov]
# Initialize list of particles
self.particles = []
for i in range(self.pc): # index starts from 0
self.particles.append(
Particle(i,
mbes_matrix,
meas_cov=meas_cov,
process_cov=predict_cov,
map_frame=map_frame))
# Initialize class/callback variables
self.time = None
self.old_time = None
self.pred_odom = None
self.mbes_true_pc = None
self.poses = PoseArray()
self.poses.header.frame_id = map_frame
self.avg_pose = PoseWithCovarianceStamped()
self.avg_pose.header.frame_id = map_frame
# Initialize particle poses publisher
param = rospy.search_param("particle_poses_topic")
pose_array_top = rospy.get_param(param)
self.pf_pub = rospy.Publisher(pose_array_top, PoseArray, queue_size=10)
# Initialize average of poses publisher
param = rospy.search_param("average_pose_topic")
avg_pose_top = rospy.get_param(param)
self.avg_pub = rospy.Publisher(avg_pose_top,
PoseWithCovarianceStamped,
queue_size=10)
# Initialize sim_mbes pointcloud publisher
param = rospy.search_param("particle_sim_mbes_topic")
mbes_pc_top = rospy.get_param(param)
self.pcloud_pub = rospy.Publisher(mbes_pc_top,
PointCloud2,
queue_size=10)
# Establish subscription to mbes pings message
param = rospy.search_param("mbes_pings_topic")
mbes_pings_top = rospy.get_param(param)
rospy.Subscriber(mbes_pings_top, PointCloud2, self._mbes_callback)
# Establish subscription to odometry message (intentionally last)
param = rospy.search_param("odometry_topic")
odom_top = rospy.get_param(param)
rospy.Subscriber(odom_top, Odometry, self.odom_callback)
rospy.sleep(0.5)
def __init__(self):
# Read necessary parameters
self.pc = rospy.get_param('~particle_count', 10) # Particle Count
self.map_frame = rospy.get_param('~map_frame', 'map') # map frame_id
self.mbes_frame = rospy.get_param('~mbes_link',
'mbes_link') # mbes frame_id
odom_frame = rospy.get_param('~odom_frame', 'odom')
meas_model_as = rospy.get_param('~mbes_as',
'/mbes_sim_server') # map frame_id
self.beams_num = rospy.get_param("~num_beams_sim", 20)
self.beams_real = rospy.get_param("~n_beams_mbes", 512)
self.mbes_angle = rospy.get_param("~mbes_open_angle",
np.pi / 180. * 60.)
# Initialize tf listener
tfBuffer = tf2_ros.Buffer()
tf2_ros.TransformListener(tfBuffer)
try:
rospy.loginfo("Waiting for transforms")
mbes_tf = tfBuffer.lookup_transform('hugin/base_link',
'hugin/mbes_link',
rospy.Time(0),
rospy.Duration(20))
self.base2mbes_mat = matrix_from_tf(mbes_tf)
m2o_tf = tfBuffer.lookup_transform(self.map_frame, odom_frame,
rospy.Time(0),
rospy.Duration(20))
self.m2o_mat = matrix_from_tf(m2o_tf)
rospy.loginfo("Transforms locked - pf node")
except:
rospy.loginfo(
"ERROR: Could not lookup transform from base_link to mbes_link"
)
# Read covariance values
meas_cov = float(rospy.get_param('~measurement_covariance', 0.01))
cov_string = rospy.get_param('~motion_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
motion_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~init_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
init_cov = list(map(float, cov_list))
cov_string = rospy.get_param('~resampling_noise_covariance')
cov_string = cov_string.replace('[', '')
cov_string = cov_string.replace(']', '')
cov_list = list(cov_string.split(", "))
self.res_noise_cov = list(map(float, cov_list))
# Initialize list of particles
self.particles = np.empty(self.pc, dtype=object)
for i in range(self.pc):
self.particles[i] = Particle(self.beams_num,
self.pc,
i,
self.base2mbes_mat,
self.m2o_mat,
init_cov=init_cov,
meas_cov=meas_cov,
process_cov=motion_cov)
self.time = None
self.old_time = None
self.pred_odom = None
self.latest_mbes = PointCloud2()
self.prev_mbes = PointCloud2()
self.poses = PoseArray()
self.poses.header.frame_id = odom_frame
self.avg_pose = PoseWithCovarianceStamped()
self.avg_pose.header.frame_id = odom_frame
# Initialize particle poses publisher
pose_array_top = rospy.get_param("~particle_poses_topic",
'/particle_poses')
self.pf_pub = rospy.Publisher(pose_array_top, PoseArray, queue_size=10)
# Initialize average of poses publisher
avg_pose_top = rospy.get_param("~average_pose_topic", '/average_pose')
self.avg_pub = rospy.Publisher(avg_pose_top,
PoseWithCovarianceStamped,
queue_size=10)
# Establish subscription to odometry message (intentionally last)
odom_top = rospy.get_param("~odometry_topic", 'odom')
rospy.Subscriber(odom_top, Odometry, self.odom_callback)
# Expected meas of PF outcome at every time step
pf_mbes_top = rospy.get_param("~average_mbes_topic", '/avg_mbes')
self.pf_mbes_pub = rospy.Publisher(pf_mbes_top,
PointCloud2,
queue_size=1)
self.stats = rospy.Publisher('/pf/n_eff', Float32, queue_size=10)
rospy.loginfo("Particle filter class successfully created")
mbes_pc_top = rospy.get_param("~particle_sim_mbes_topic", '/sim_mbes')
self.pcloud_pub = rospy.Publisher(mbes_pc_top,
PointCloud2,
queue_size=10)
# Load mesh
print("Loading data")
svp_path = rospy.get_param('~sound_velocity_prof')
mesh_path = rospy.get_param('~mesh_path')
sound_speeds = csv_data.csv_asvp_sound_speed.parse_file(svp_path)
data = np.load(mesh_path)
V, F, bounds = data['V'], data['F'], data['bounds']
print("Mesh loaded")
# Create draper
self.draper = base_draper.BaseDraper(V, F, bounds, sound_speeds)
self.draper.set_ray_tracing_enabled(False)
print("draper created")
# Action server for MBES pings sim (necessary to be able to use UFO maps as well)
self.as_ping = actionlib.SimpleActionServer('/mbes_sim_server',
MbesSimAction,
execute_cb=self.mbes_as_cb)
# Subscription to real mbes pings
mbes_pings_top = rospy.get_param("~mbes_pings_topic", 'mbes_pings')
rospy.Subscriber(mbes_pings_top, PointCloud2, self.mbes_real_cb)
# Create expected MBES beams directions
angle_step = self.mbes_angle / self.beams_num
self.beams_dir = []
for i in range(0, self.beams_num):
roll_step = rotation_matrix(-self.mbes_angle / 2. + angle_step * i,
(1, 0, 0))[0:3, 0:3]
rot = roll_step[:, 2]
self.beams_dir.append(rot / np.linalg.norm(rot))
self.update_rviz()
rospy.spin()