diff --git a/launch/general_estimation_simulation.launch b/launch/general_estimation_simulation.launch new file mode 100644 index 0000000000000000000000000000000000000000..5b26da435471c914be3538883cfcab16ac161248 --- /dev/null +++ b/launch/general_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)" /> + + + <!--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/general_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/general_estimation_simulation.yaml b/yaml/general_estimation_simulation.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6bd2a74a2d98abb196a480b9a615696350cf8270 --- /dev/null +++ b/yaml/general_estimation_simulation.yaml @@ -0,0 +1,180 @@ +config: + + debug: + profiling: true + profiling_file: "~/wolf_demo_uav_identification.txt" + print_problem: true + print_depth: 1 # only if print_problem + print_constr_by: false # only if print_problem + print_metric: true # only if print_problem + print_state_blocks: true # only if print_problem + print_period: 3 # only if print_problem + + + + + problem: + node_rate: 100 + frame_structure: "POVL" + dimension: 3 + prior: + mode: "initial_guess" ## + $state: + P: [0,0,2.5] # 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] + V: [100,100,100] + L: [100,100,100] + time_tolerance: 0.005 + tree_manager: + type: "TreeManagerSlidingWindow" + plugin: "core" + n_frames: 1000 + n_fix_first_frames: 0 + viral_remove_empty_parent: true + map: + type: "MapBase" + plugin: "core" + + solver: + follow: "solver.yaml" + + sensors: + - + name: "sensor FTI" + type: "SensorForceTorqueInertial" + plugin: "bodydynamics" + extrinsic: + pose: [0,0,0, 0,0,0,1] + + # IMU + acc_noise_std: .01 # std dev of acc noise in m/s2 # 0.6/4 + gyro_noise_std: .01 # std dev of gyro noise in rad/s + accb_initial_std: 0.5 # m/s2 - initial bias + gyrob_initial_std: 0.1 # rad/sec - initial bias + acc_drift_std: 0.1 # std dev of acc drift m/s2/sqrt(s) + gyro_drift_std: 0.1 # std dev of gyro drift rad/s/sqrt(s) + bias: [0,0,0,0,0,0] # bias prior, usefull for fixing the bias when known + imu_bias_fix: true + + #FT + force_noise_std: 1 # std dev of force noise in N + torque_noise_std: 1 # std dev of torque noise in N/m + + # Dynamics + com: [0.00,0.00,0.0] # center of mass [m] + inertia: [0.014958,0.014648,0.0237319] # moments of inertia i_xx, i_yy, i_zz [kg m2] + mass: 2.02 # mass [kg] + com_fix: false + inertia_fix: false + mass_fix: false + + # priors + set_mass_prior: true + prior_mass_std: 1 + set_inertia_prior: true + prior_inertia_std: [0.1,0.1,0.1] + set_com_prior: true + prior_com_std: [0.1,0.1,0.1] + set_bias_prior: true + prior_bias: [0,0,0,0,0,0] + prior_bias_std: [0.1,0.1,0.1,0.1,0.1,0.1] + - + name: "sensor Pose" + type: "SensorPose" + plugin: "core" + extrinsic: + pose: [0.0,0.0,0.0, 0,0,0,1] #amg + std_p: 0.001 + std_o: 0.001 + + + processors: + - + name: "proc FTID" + type: "ProcessorForceTorqueInertialDynamics" + sensor_name: "sensor FTI" + plugin: "bodydynamics" + time_tolerance: 0.01 # amg 1/50/2 + apply_loss_function: false + unmeasured_perturbation_std: 0.00001 + state_getter: true + state_priority: 1 + keyframe_vote: + voting_active: true + max_time_span: 0.2 # De moment el que funciona millor és 1 s o 2s + max_buff_length: 999 # motion deltas + dist_traveled: 999 # meters + angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg) + + - + name: "processor Pose" + type: "ProcessorPose" + sensor_name: "sensor Pose" + plugin: "core" + time_tolerance: 0.01 # 1 / 100Hz / 2 = 0.005 + apply_loss_function: false + keyframe_vote: + voting_active: false + + + + ROS subscriber: + - + package: "wolf_ros_bodydynamics" + type: "SubscriberForceTorqueInertialDrone" + topic: " " + topic_acc: "/fmu/sensor_accel/out" + topic_gyro: "/fmu/sensor_gyro/out" + topic_sensorCombined: "/fmu/sensor_combined/out" + topic_actuators: "/fmu/actuator_outputs/out" + topic_batteryStatus: "/fmu/battery_status/out" + sensor_name: "sensor FTI" + imu_x_axis: 1 + imu_y_axis: -2 + imu_z_axis: -3 + cov_imu_source: "sensor" + in_degrees: false + synchronization_mode: "imu" + duty_min: 1000 + duty_max: 2000 + w_min: 100 + w_max: 2216 + # w_max: 1116 + quadratic_aprox: true # true in simulation + follow: "./borinot_platform_simulation.yaml" + - + package: "wolf_ros_node" + type: "SubscriberPose" + topic: "/pose_from_tf" # amg # msg generated from tf to mimic the optitrack msg + sensor_name: "sensor Pose" + + + ROS publisher: + - + type: "PublisherGraph" + topic: "graph" + package: "wolf_ros_node" + period: 0.05 + viz_scale: 0.3 + text_scale: 0.3 + landmark_text_z_offset: 0.3 + landmark_length: 0.5 + frame_width: 0.05 + frame_length: 0.5 + frame_vel_scale: 1.0 + factors_absolute_height: 00.1 + - + type: "PublisherTf" + topic: " " + package: "wolf_ros_node" + period: 0.1 + map_frame_id: "map" + odom_frame_id: "odom" + base_frame_id: "base_footprint" + publish_odom_tf: true +