#!/bin/sh

dir=/media/enrico/Dati/workspace/scoop_demonstrations/book/Sync

#echo $dir
 

for i in ${dir}/*.bag
do
	echo $i
	
	
	rostopic echo /sync_force -p > $dir/$(basename $i).ft2.txt &
	rostopic echo /sync_joints -p > $dir/$(basename $i).joint2.txt &
	rostopic echo /sync_franka -p > $dir/$(basename $i).cart2.txt &
	rostopic echo /cmd_gripper -p > $dir/$(basename $i).cmd_gripper.txt &

	rosbag play $i -r5 --try-future-version 

	killall -9 rostopic
	
	sed -i -n -e '2,$p' $dir/$(basename $i).cmd_gripper.txt
	
	awk -F, '{$2=$3=$4="";}1' $dir/$(basename $i).ft2.txt > $dir/$(basename $i).ft.txt
	#awk -F, '{$2=$3=$4=$5=$6=$7=$8=$9=$10=$11=$19=$20=$21=$22=$23=$24=$25=$26=$27=$28=$29=$30=$31=$32="";}1' $dir/$(basename $i).joint2.txt > $dir/$(basename $i).joint.txt # position only
	awk -F, '{$2=$3=$4=$5=$6=$7=$8=$9=$10=$11=$19="";}1' $dir/$(basename $i).joint2.txt > $dir/$(basename $i).joint.txt #position + velocity + effort
n=189
	awk -F, '{
    for (i=2; i<=189; i++) {
        $i="";
    }
    for (i=206; i<=251+140; i++) {
        $i="";
    }
    sub(/, /, ",");
    print;
}' n="$189"  $dir/$(basename $i).cart2.txt >  $dir/$(basename $i).cart.txt # O_EE_T ee w.r.t base


	sed -i -n -e '2,$p' $dir/$(basename $i).ft.txt
	sed -i -n -e '2,$p' $dir/$(basename $i).joint.txt
	sed -i -n -e '2,$p' $dir/$(basename $i).cart.txt

	rm $dir/*2.txt
#	mv $dir/*.txt /home/enrico/workspace/paletta_ws/src/ece_paletta/matlab/figure_generation/data/
done

