def get_RosTopicFilteringScheme_and_time_series_remapping_array(
datatypes, frequency, offset_points, target_points):
tfc = RosTopicFilteringScheme(resampling_rate=frequency)
joint_based_datatypes = [
i for i in datatypes if i in JOINT_BASED_DATATYPES
]
joint_based_datatypes_size, joint_based_datatypes_shape = _setup_joint_based_datatype_filter(
tfc, joint_based_datatypes, offset_points, target_points)
other_datatypes = [i for i in datatypes if i not in JOINT_BASED_DATATYPES]
other_datatypes_size, other_datatypes_shape = _setup_other_datatype_filter(
tfc, other_datatypes)
datatypes_of_new_order = joint_based_datatypes + other_datatypes
datatypes_size_of_new_order = joint_based_datatypes_size + other_datatypes_size
datatypes_end_idx_of_new_order = np.cumsum(datatypes_size_of_new_order)
remapping_array = []
for dtype in datatypes:
dtype_idx_in_new_order = datatypes_of_new_order.index(dtype)
end_idx = datatypes_end_idx_of_new_order[dtype_idx_in_new_order]
start_idx = end_idx - datatypes_size_of_new_order[
dtype_idx_in_new_order]
remapping_array += range(start_idx, end_idx)
return tfc, remapping_array
def process_rosbag_to_SampleResult(rosbag_path,
datatypes,
frequency,
offset_points,
target_points,
T,
episode_start_time_in_sec=None,
episode_end_time_in_sec=None):
assert (offset_points.shape[0] == 3)
assert (target_points.shape[0] == 3)
assert (offset_points.shape[1] == target_points.shape[1])
num_of_points = offset_points.shape[1]
tfc = RosTopicFilteringScheme(resampling_rate=frequency)
joint_based_datatypes = [
i for i in datatypes if i in JOINT_BASED_DATATYPES
]
joint_based_datatypes_size, joint_based_datatypes_shape = _setup_joint_based_datatype_filter(
tfc, joint_based_datatypes, offset_points, target_points)
other_datatypes = [i for i in datatypes if i not in JOINT_BASED_DATATYPES]
other_datatypes_size, other_datatypes_shape = _setup_other_datatype_filter(
tfc, other_datatypes)
# re-order datatypes so that they match with the order of filtering
datatypes = joint_based_datatypes + other_datatypes
datatypes_size = joint_based_datatypes_size + other_datatypes_size
datatypes_shape = joint_based_datatypes_shape + other_datatypes_shape
ofrt = OfflineRostopicsToTimeseries(tfc)
time_stamps, mat = ofrt.get_timeseries_mat(rosbag_path,
episode_start_time_in_sec,
episode_end_time_in_sec)
time_stamps = time_stamps[:T]
mat = mat[:T, :]
datatypes_falttened = np.split(
mat, indices_or_sections=np.cumsum(datatypes_size)[:-1], axis=1)
assert len(datatypes_falttened) == len(datatypes)
sr = SampleResult()
for i in range(len(datatypes)):
data_type = datatypes[i]
shape = (T, ) + datatypes_shape[i]
data = datatypes_falttened[i].flatten()
sr.sensor_data.append(
DataType(data_type=data_type, shape=shape, data=data))
return sr
msg.pose.position.z,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w,
]
@staticmethod
def vector_size():
return 7
@staticmethod
def vector_meaning():
return [
'baxter_enpoint_pose.pose.position.x', \
'baxter_enpoint_pose.pose.position.y', \
'baxter_enpoint_pose.pose.position.z', \
'baxter_enpoint_pose.pose.orientation.x', \
'baxter_enpoint_pose.pose.orientation.y', \
'baxter_enpoint_pose.pose.orientation.z', \
'baxter_enpoint_pose.pose.orientation.w', \
]
dmp_cmd_timeseries_hz = 100
dmp_cmd_timeseries_config = RosTopicFilteringScheme(dmp_cmd_timeseries_hz)
dmp_cmd_timeseries_config.add_filter(
"/robot/limb/right/endpoint_state",
EndpointState,
BaxterEndpointFilterForDmpCmd,
)
OfflineRostopicsToTimeseries,
)
from rostopics_to_timeseries.TopicMsgFilter import BaxterEndpointStateFilter, BaxterEndpointStateFilterForTwistLinear
from rostopics_to_timeseries.Smoother import WindowBasedSmoother_factory
import baxter_core_msgs.msg
import rospy
import matplotlib.pyplot as plt
from scipy import signal
import os
dir_of_this_script = os.path.dirname(os.path.realpath(__file__))
if __name__ == '__main__':
tfc = RosTopicFilteringScheme(resampling_rate=10)
tfc.add_filter(
"/robot/limb/right/endpoint_state",
baxter_core_msgs.msg.EndpointState,
BaxterEndpointStateFilter,
)
tfc.add_filter(
"/robot/limb/right/endpoint_state",
baxter_core_msgs.msg.EndpointState,
BaxterEndpointStateFilterForTwistLinear,
)
tfc.smoother_class = WindowBasedSmoother_factory(signal.triang(5))
tfc.smoother_class = None
print 'timeseries_header:', tfc.timeseries_header
from rostopics_to_timeseries import RosTopicFilteringScheme
from rostopics_to_timeseries.Smoother import WindowBasedSmoother_factory
import smach_based_introspection_framework.offline_part.anomaly_classification_feature_selection.msg_filters_with_scaling as cl_msg_filters_with_scaling
import tactilesensors4.msg
from baxter_core_msgs.msg import EndpointState
from geometry_msgs.msg import WrenchStamped
from scipy import signal
anomaly_classification_timeseries_hz = 10
anomaly_classification_confidence_threshold = 0.7
anomaly_window_size = [2, 2]
anomaly_classification_timeseries_config = RosTopicFilteringScheme(
anomaly_classification_timeseries_hz)
anomaly_classification_timeseries_config.add_filter(
"/TactileSensor4/StaticData",
tactilesensors4.msg.StaticData,
cl_msg_filters_with_scaling.TactileStaticStdFilter,
)
anomaly_classification_timeseries_config.add_filter(
"/robotiq_force_torque_wrench",
WrenchStamped,
cl_msg_filters_with_scaling.WrenchStampedNormFilter,
)
anomaly_classification_timeseries_config.add_filter(
"/robotiq_force_torque_wrench",
WrenchStamped,
cl_msg_filters_with_scaling.WrenchStampedFilter,
)
anomaly_classification_timeseries_config.add_filter(
"/robot/limb/right/endpoint_state",
EndpointState,
msg_filters_with_scaling.BaxterEndpointTwistNormFilter,
],
[
"/robot/limb/right/endpoint_state",
EndpointState,
msg_filters_with_scaling.BaxterEndpointTwistFilter,
],
]
filters_args = []
smoother_args = []
smoother_args.append(WindowBasedSmoother_factory(signal.boxcar(5)))
for smoother_class in smoother_args:
for prod in itertools.product(*[(None, i) for i in filters_args]):
anomaly_detection_timeseries_config = RosTopicFilteringScheme(
anomaly_detection_timeseries_hz)
if smoother_class is not None:
anomaly_detection_timeseries_config.smoother_class = smoother_class
for k in fixed_filters:
anomaly_detection_timeseries_config.add_filter(*k)
for j in prod:
if j is not None:
for k in j:
anomaly_detection_timeseries_config.add_filter(*k)
if len(anomaly_detection_timeseries_config.timeseries_header) != 0:
filtering_schemes.append(anomaly_detection_timeseries_config)