diff --git a/launch/inertia_estimation_simulation.launch b/launch/inertia_estimation_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..788f54009f51bf6958e17612a613d96e9fe18de8 --- /dev/null +++ b/launch/inertia_estimation_simulation.launch @@ -0,0 +1,51 @@ +<!-- --> +<launch> + <!--USER ARGS--> + <arg name="rviz" default="true" /> + <arg name="speed" default="1" /> + <arg name="sec" default="0" /> + <arg name="sim_time" default="true" /> + + <arg name="profiling" default="false" /> + <arg name="gdb" default="false" /> + <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/> + <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" /> + <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" /> + + <arg name="bag" default="rosbag_borinot_sim_general_V7"/> + + + <!--VISUALIZATION--> + <group if="$(arg rviz)"> + <node name="rviz" + pkg="rviz" + type="rviz" + args="-d $(find wolf_demo_uav_identification)/rviz/online.rviz" /> + </group> + + + + <!--WOLF--> + <node type="wolf_ros_node" + name="wolf_ros_node" + pkg="wolf_ros_node" + output="screen" + required="true" + launch-prefix="$(arg launch_pref)"> + <param name="~yaml_file_path" value="$(find wolf_demo_uav_identification)/yaml/inertia_estimation_simulation.yaml" /> + <remap from="/wrench_accel" to="/fmu/sensor_accel/out"/> + <remap from="/wrench_gyro" to="/fmu/sensor_gyro/out"/> + <remap from="/wrench_actuators" to="/fmu/actuator_outputs/out"/> + </node> + + <!-- ROSBAG --> + <node pkg="rosbag" + type="play" + name="player" + required="true" + args="-r $(arg speed) + -s $(arg sec) + --clock + $(find wolf_demo_uav_identification)/bag/$(arg bag).bag"/> + +</launch> diff --git a/yaml/inertia_estimation_simulation.yaml b/yaml/inertia_estimation_simulation.yaml index bb090377f374f35a673aec44bfa9acc1fe7a66ea..fdbbfe30a5b76e72dff85f2d0e75bdbcc77b6e29 100644 --- a/yaml/inertia_estimation_simulation.yaml +++ b/yaml/inertia_estimation_simulation.yaml @@ -20,13 +20,13 @@ config: prior: mode: "initial_guess" ## $state: - P: [0,0,2.5] # amg # initial position + P: [0,0,0] # amg # initial position O: [0,0,0,1] # amg # initial orientation V: [0,0,0] L: [0,0,0] $sigma: - P: [0.31, 0.31, 0.31] - O: [0.31, 0.31, 0.31] + P: [10, 10, 10] + O: [10, 10, 10] V: [100,100,100] L: [100,100,100] time_tolerance: 0.005 @@ -77,7 +77,7 @@ config: set_mass_prior: true prior_mass_std: 1 set_inertia_prior: true - prior_inertia_std: [1,1,1] + prior_inertia_std: [0.1,0.1,0.1] set_com_prior: true prior_com_std: [1,1,1] set_bias_prior: true @@ -89,7 +89,7 @@ config: type: "SensorPose" plugin: "core" extrinsic: - pose: [0.0,0.0,0.0, 0,0,0,1] #amg + pose: [0.0,0.0,0.0, 0,0,0,1] std_p: 0.001 std_o: 0.001 @@ -152,7 +152,7 @@ config: - package: "wolf_ros_node" type: "SubscriberPose" - topic: "/pose_from_tf" # amg # msg generated from tf to mimic the optitrack msg + topic: "/pose_from_tf" # msg generated from tf to mimic the optitrack msg sensor_name: "sensor Pose"