Skip to content

Commit

Permalink
RSS State Estimation Tutorial (#48)
Browse files Browse the repository at this point in the history 
* structure for tutorial session

* first running and plotting version of tutorial

* tutorial settings for 1.1

* tutorial 1 and 2 finished

* adding tutorial 3
  • Loading branch information
@nubertj @mantelt
nubertj authored and mantelt committed Jul 10, 2023
1 parent f98e417 commit 3cfcf3e
Show file tree
Hide file tree
Showing 16 changed files with 521 additions and 147 deletions.
1 change: 0 additions & 1 deletion smb_msf_graph/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@ install(DIRECTORY
# Python nodes
catkin_install_python(PROGRAMS
smb_msf_graph/smb_msf_graph/plotting_node.py
smb_msf_graph/smb_msf_graph/pose_noisifier.py
smb_msf_graph/smb_msf_graph/imu_noisifier.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
21 changes: 0 additions & 21 deletions smb_msf_graph/launch/plotting.launch
View file

This file was deleted.

12 changes: 11 additions & 1 deletion smb_msf_graph/launch/smb_msf_graph_o3d_replay.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,23 @@
<arg name="use_sim_time" value="true" />

<!-- Parameters -->
<arg name="imu_topic_name" default="/imu" />
<arg name="lidar_odometry_topic_name" default="/mapping_node/scan2map_odometry"/>
<arg name="launch_o3d_slam" default="true"/>

<!-- Config Files -->
<arg name="core_graph_param_file" default="$(find smb_estimator_graph)/config/core/core_graph_params.yaml"/>
<arg name="smb_graph_param_file" default="$(find smb_estimator_graph)/config/smb_specific/smb_graph_params.yaml"/>

<!-- Alias to smb_estimator_graph replay script -->
<include file="$(find smb_estimator_graph)/launch/smb_estimator_graph_replay.launch">
<arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)" />
<arg name="use_sim_time" value="$(arg use_sim_time)" />
<!-- Topics -->
<arg name="imu_topic_name" value="$(arg imu_topic_name)" />
<arg name="lidar_odometry_topic_name" value="$(arg lidar_odometry_topic_name)" />
<!-- Config Files -->
<arg name="core_graph_param_file" value="$(arg core_graph_param_file)" />
<arg name="smb_graph_param_file" value="$(arg smb_graph_param_file)" />
</include>

<!-- Load Description -->
Expand Down
39 changes: 27 additions & 12 deletions smb_msf_graph/smb_msf_graph/smb_msf_graph/imu_noisifier.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,26 +5,41 @@
import numpy as np
from sensor_msgs.msg import Imu

class PoseNoisifier:
class ImuNoisifier:

def __init__(self):

self.imu_topic_name = "/versavis/imu"
self.imu_topic_name = "/imu"

# Flags
self.is_first_imu = True

# ROS
rospy.init_node("PoseNoisifierNode", anonymous=True)

# Publisher
self.imu_publisher = rospy.Publisher("/versavis/imu_noisified", Imu, queue_size=10)
self.imu_publisher = rospy.Publisher("/imu/noisified", Imu, queue_size=10)

# Bias Amplitude
self.x_bias = 0.0
self.y_bias = 0.0
self.z_bias = 0.1

# Initial Time
self.initial_time = []

# Period Duration
self.period_duration = 20.0

def subscriber_callback(self, imu):
print("Received imu message.")
x_bias = 0.0
y_bias = 0.0
z_bias = 0.8
imu.linear_acceleration.x += x_bias
imu.linear_acceleration.y += y_bias
imu.linear_acceleration.z += z_bias
if self.is_first_imu:
print("Received imu message.")
self.is_first_imu = False
self.initial_time = imu.header.stamp.to_sec()

imu.linear_acceleration.x += self.x_bias * np.sin(2 * np.pi / self.period_duration * (imu.header.stamp.to_sec() - self.initial_time))
imu.linear_acceleration.y += self.y_bias * np.sin(2 * np.pi / self.period_duration * (imu.header.stamp.to_sec() - self.initial_time))
imu.linear_acceleration.z += self.z_bias * np.sin(2 * np.pi / self.period_duration * (imu.header.stamp.to_sec() - self.initial_time))
self.imu_publisher.publish(imu)

def start_noisifying(self):
Expand All @@ -36,5 +51,5 @@ def start_noisifying(self):

if __name__ == "__main__":

pose_noisifier = PoseNoisifier()
pose_noisifier.start_noisifying()
imu_noisifier = ImuNoisifier()
imu_noisifier.start_noisifying()
210 changes: 136 additions & 74 deletions smb_msf_graph/smb_msf_graph/smb_msf_graph/plotting_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,138 +8,200 @@
import time

# Custom
from msf_core.msg import DoubleArrayStamped
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Vector3Stamped

# Name of the experiment
experiment_name = ""


def plot_2d(x_coords: list, y_coords: list, x_axis_name: str, y_axis_name: str, plot_name: str):
# Check length
print("Plot: " + plot_name + ". Length of " + x_axis_name + ": " + str(len(x_coords)) + ". Length of " + y_axis_name + ": " + str(len(y_coords)))
assert np.abs(len(x_coords) - len(y_coords)) <= 1
if len(x_coords) - len(y_coords) == 1:
x_coords.pop() # remove last element
elif len(y_coords) - len(x_coords) == 1:
y_coords.pop()
# Home-dir
home_dir = os.path.expanduser('~')
# Plot
plt.rcParams['lines.markersize'] = 0.5
plt.rcParams["figure.figsize"] = (6,6)
plt.xticks(fontsize=24)
plt.yticks(fontsize=24)
fig, ax = plt.subplots(1,1)
ax.set(xlabel=x_axis_name, ylabel=y_axis_name, title=plot_name)
ax.grid()
plt.tight_layout()
ax.plot(x_coords, y_coords, c="green", linewidth=0.7, alpha=1.0)
fig.savefig(os.path.join(home_dir, "rss_plots", experiment_name + "_" + plot_name + ".png"))

# Only plot if there are values
if len(x_coords) > 0:
assert np.abs(len(x_coords) - len(y_coords)) <= 1
if len(x_coords) - len(y_coords) == 1:
x_coords.pop() # remove last element
elif len(y_coords) - len(x_coords) == 1:
y_coords.pop()
# Home-dir
home_dir = os.path.expanduser('~')
# Plot
plt.rcParams['lines.markersize'] = 0.5
plt.rcParams["figure.figsize"] = (6,6)
plt.xticks(fontsize=24)
plt.yticks(fontsize=24)
fig, ax = plt.subplots(1,1)
ax.set(xlabel=x_axis_name, ylabel=y_axis_name, title=plot_name)
ax.grid()
plt.tight_layout()
ax.plot(x_coords, y_coords, c="green", linewidth=0.7, alpha=1.0)
fig.savefig(os.path.join(home_dir, "rss_plots", experiment_name + "_" + plot_name + ".png"))

class MsfPlotter:

def __init__(self):

self.state_topic_name = "/msf_core/state_out"
self.state_odometry_topic_name = "/graph_msf/est_odometry_world_imu"
self.wheel_odometry_topic_name = "/control/smb_diff_drive/odom"
self.imu_bias_topic_name = "/graph_msf/accel_bias"

# ROS
rospy.init_node('MSF Publisher Node', anonymous=True)
rospy.init_node('GMSF Plotter Node', anonymous=True)

# Create dir
home_dir = os.path.expanduser('~')
if not os.path.exists(os.path.join(home_dir, "rss_plots")):
os.makedirs(os.path.join(home_dir, "rss_plots"))
HOME_DIR = os.path.expanduser('~')
if not os.path.exists(os.path.join(HOME_DIR, "rss_plots")):
os.makedirs(os.path.join(HOME_DIR, "rss_plots"))

# Stored messages
# State Messages
## Time
self.initial_time = []
self.time = []
self.initial_state_time = []
self.state_time = []
## Position
self.x_pos = []
self.y_pos = []
self.z_pos = []
## Velocity
self.x_vel = []
self.y_vel = []
# IMU Bias Messages
## Time
self.initial_imu_bias_time = []
self.imu_bias_time = []
## Acc Bias
self.x_acc_bias = []
self.y_acc_bias = []
self.z_acc_bias = []
# Wheel odometry position
## Time
self.initial_wheel_time = []
## Position
self.x_wheel_pos = []
self.y_wheel_pos = []
self.z_wheel_pos = []

def plot(self):

# Plot diagrams when destructor is called
print("Saving plots...")
# Position plots -------------------------------------------
## x-y-plot
plot_name = "position-x-y"
plot_name = "state-position-x-y"
print(plot_name)
x_axis_name = "x[m]"
y_axis_name = "y[m]"
plot_2d(x_coords=self.x_pos, y_coords=self.y_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## z-plot
plot_name = "position-z"
plot_name = "state-position-z"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "z[m]"
plot_2d(x_coords=self.time, y_coords=self.z_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
plot_2d(x_coords=self.state_time, y_coords=self.z_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
# Velcoity plots -------------------------------------------
## x-velocity
plot_name = "velocity-x"
plot_name = "state-velocity-x"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "v[m/s]"
plot_2d(x_coords=self.time, y_coords=self.x_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
plot_2d(x_coords=self.state_time, y_coords=self.x_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## y-velocity
plot_name = "velocity-y"
plot_name = "state-velocity-y"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "v[m/s]"
plot_2d(x_coords=self.time, y_coords=self.y_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## x-Bias
plot_name = "acc-bias-x"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.time, y_coords=self.x_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## x-Bias
plot_name = "acc-bias-y"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.time, y_coords=self.y_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## x-Bias
plot_name = "acc-bias-z"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.time, y_coords=self.z_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)


def subscriber_callback(self, state_estimate):
print("Received state message.")
plot_2d(x_coords=self.state_time, y_coords=self.y_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
# Bias plots -----------------------------------------------
if experiment_name == "1.3" or experiment_name == "2.1" or experiment_name == "2.2" or experiment_name == "3.1" or experiment_name == "3.2":
## x-Bias
plot_name = "acc-bias-x"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.imu_bias_time, y_coords=self.x_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## y-Bias
plot_name = "acc-bias-y"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.imu_bias_time, y_coords=self.y_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
## z-Bias
plot_name = "acc-bias-z"
print(plot_name)
x_axis_name = "t[s]"
y_axis_name = "b[m/s^2]"
plot_2d(x_coords=self.imu_bias_time, y_coords=self.z_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)
# Wheel odometry plots -------------------------------------
if experiment_name == "1.2":
## x-y-plot
plot_name = "wheel-position-x-y"
print(plot_name)
x_axis_name = "x[m]"
y_axis_name = "y[m]"
plot_2d(x_coords=self.x_wheel_pos, y_coords=self.y_wheel_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name)


def state_odometry_callback(self, state_estimate):
# Time
if not self.initial_state_time:
self.initial_state_time = state_estimate.header.stamp.to_sec()
print("Received state message.")
return
elif state_estimate.header.stamp.to_sec() - self.initial_state_time > 0.5:
self.state_time = self.state_time + [state_estimate.header.stamp.to_sec() - self.initial_state_time]
# Position
self.x_pos = self.x_pos + [state_estimate.pose.pose.position.x]
self.y_pos = self.y_pos + [state_estimate.pose.pose.position.y]
self.z_pos = self.z_pos + [state_estimate.pose.pose.position.z]
# Velocity
self.x_vel = self.x_vel + [state_estimate.twist.twist.linear.x]
self.y_vel = self.y_vel + [state_estimate.twist.twist.linear.y]

def wheel_odometry_callback(self, wheel_odometry):
# Time
if not self.initial_time:
self.initial_time = state_estimate.header.stamp.to_sec()
self.time = self.time + [state_estimate.header.stamp.to_sec() - self.initial_time]
# Position
self.x_pos = self.x_pos + [state_estimate.data[0]]
self.y_pos = self.y_pos + [state_estimate.data[1]]
self.z_pos = self.z_pos + [state_estimate.data[2]]
# Velocity
self.x_vel = self.x_vel + [state_estimate.data[3]]
self.y_vel = self.y_vel + [state_estimate.data[4]]
# Acc. Bias
self.x_acc_bias = self.x_acc_bias + [state_estimate.data[13]]
self.y_acc_bias = self.y_acc_bias + [state_estimate.data[14]]
self.z_acc_bias = self.z_acc_bias + [state_estimate.data[15]]
if not self.initial_wheel_time:
self.initial_wheel_time = wheel_odometry.header.stamp.to_sec()
print("Received wheel message.")
return
elif wheel_odometry.header.stamp.to_sec() - self.initial_wheel_time > 0.5:
self.x_wheel_pos = self.x_wheel_pos + [wheel_odometry.pose.pose.position.x]
self.y_wheel_pos = self.y_wheel_pos + [wheel_odometry.pose.pose.position.y]
self.z_wheel_pos = self.z_wheel_pos + [wheel_odometry.pose.pose.position.z]

def bias_callback(self, bias_estimate):
# Time
if not self.initial_imu_bias_time:
self.initial_imu_bias_time = bias_estimate.header.stamp.to_sec()
print("Received bias message.")
return
elif bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time > 0.5:
self.imu_bias_time = self.imu_bias_time + [bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time]
# Acc. Bias
self.x_acc_bias = self.x_acc_bias + [bias_estimate.vector.x]
self.y_acc_bias = self.y_acc_bias + [bias_estimate.vector.y]
self.z_acc_bias = self.z_acc_bias + [bias_estimate.vector.z]

def start_plotting(self):
print("======================")
print("Starting plotting...")
print("======================")
rospy.Subscriber(self.state_topic_name, DoubleArrayStamped,
self.subscriber_callback)

# Odometry Subscriber
rospy.Subscriber(self.state_odometry_topic_name, Odometry,
self.state_odometry_callback)

# Wheel Odometry Subscriber
rospy.Subscriber(self.wheel_odometry_topic_name, Odometry,
self.wheel_odometry_callback)

# Bias Subscriber
rospy.Subscriber(self.imu_bias_topic_name, Vector3Stamped,
self.bias_callback)

# Start spinning
rospy.spin()

# Plot after shutdown
self.plot()

if __name__ == "__main__":
Expand Down
Loading

0 comments on commit 3cfcf3e

Please sign in to comment.