diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch index 0b074634a0211c11e57855b9d99d2315aa844eb9..bf1cbdd9eba45feccbb43295da9df03cf92681f3 100644 --- a/launch/imu2d_demo.launch +++ b/launch/imu2d_demo.launch @@ -1,6 +1,8 @@ <!-- --> <launch> <param name="use_sim_time" value="true" /> + <arg name="record" default="true" /> + <arg name="rviz" default="true" /> <arg name="speed" default="1" /> <arg name="sec" default="0" /> <arg name="profiling" default="false" /> @@ -14,7 +16,10 @@ <!-- using "clock" option to use the simulated time. Rosbag launch as the first node --> <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/> - <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/> + + <group if="$(arg record)"> + <node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/> + </group> <node pkg="tf" type="static_transform_publisher" @@ -42,10 +47,13 @@ required="true" args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/> - <node type="rviz" - name="rviz" - pkg="rviz" - args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/> + <group if="$(arg rviz)"> + <node type="rviz" + name="rviz" + pkg="rviz" + args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/> + </group> + <node type="wolf_ros_node" name="wolf_ros_node" pkg="wolf_ros_node" diff --git a/yaml/imu2d_test10.yaml b/yaml/imu2d_test10.yaml new file mode 100644 index 0000000000000000000000000000000000000000..00406b56b52c2c3f178dc880b61e24c83dabc2ab --- /dev/null +++ b/yaml/imu2d_test10.yaml @@ -0,0 +1,115 @@ +config: + debug: + profiling: true + profiling_file: "~/wolf_demo_profiling_imu2d_test0.txt" + print_problem: false + print_period: 2 + print_depth: 4 + print_constr_by: false + print_metric: true + print_state_blocks: true + problem: + tree_manager: + type: "none" + frame_structure: "POV" + # dimension: 2 + # prior: + # mode: "fix" + # $state: + # P: [0,0] + # O: [0] + # V: [0,0] + # time_tolerance: 0.1 + dimension: 3 + prior: + mode: "fix" + $state: + P: [0,0,0] + O: [0,0,0,1] + V: [0,0,0] + time_tolerance: 0.1 + node_rate: 100 + map: + type: "MapBase" + plugin: "core" + + solver: + follow: "solver.yaml" + + sensors: + - + type: "SensorLaser2d" + name: "scanner_front_left" + plugin: "laser" + follow: "test_laser_params.yaml" + + processors: + - + type: "ProcessorOdomIcp" + name: "processorodomicp" + sensor_name: "scanner_front_left" + plugin: "laser" + follow: "test_laser_processor.yaml" + # - + # type: "ProcessorLoopClosureIcp" + # name: "processorloopclosureicp" + # sensor_name: "scanner_front_left" + # plugin: "laser" + # time_tolerance: 0.1 + # apply_loss_function: true + # keyframe_vote: + # voting_active: false + # recent_frames_ignored: 3 + # frames_ignored_after_loop: 0 + # max_error_threshold: 0.02 + # min_points_percent: 40 + # max_loops: 3 + # max_candidates: 5 + # max_attempts: 15 + # candidate_generation: "random" # 'random' or 'tree' + # icp: + # follow: "csm.yaml" + + + ROS subscriber: + - + package: "wolf_ros_laser" + type: "SubscriberLaser2d" + topic: "/ana/sensors/scan" + sensor_name: "scanner_front_left" + load_params_from_msg: true + + ROS publisher: + - + package: "wolf_ros_node" + type: "PublisherTf" + topic: " " + period: 0.1 + follow: "test_publisher_ros_node.yaml" + - + package: "wolf_ros_node" + type: "PublisherGraph" + topic: "graph" + period: 1 + viz_overlapped_factors: true + # - + # package: "wolf_ros_node" + # type: "PublisherTrajectory" + # topic: "trajectory" + # period: 1 + # frame_id: "map" + # - + # package: "wolf_ros_laser" + # type: "PublisherLaserMap" + # topic: "map" + # period: 1 + # map_frame_id: "map" + # update_dist_th: 0.05 + # update_angle_th: 0.05 + # max_n_cells: 1000000 + # grid_size: 0.1 + # p_free: 0.3 + # p_obst: 0.8 + # p_free_th: 0.2 + # p_obst_th: 0.9 + # discard_max_range: true diff --git a/yaml/imu2d_test15.yaml b/yaml/imu2d_test15.yaml new file mode 100644 index 0000000000000000000000000000000000000000..853c457a44352782cdb64cf11e7b9a27f6b954ca --- /dev/null +++ b/yaml/imu2d_test15.yaml @@ -0,0 +1,168 @@ +config: + debug: + profiling: true + profiling_file: "~/wolf_demo_profiling_imu2d_test5.txt" + print_problem: false + print_period: 2 + print_depth: 4 + print_constr_by: false + print_metric: true + print_state_blocks: true + problem: + tree_manager: + type: "none" + frame_structure: "POV" + # dimension: 2 + # prior: + # mode: "fix" + # $state: + # P: [0,0] + # O: [0] + # V: [0,0] + # time_tolerance: 0.1 + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + V: [0,0,0] + $sigma: + P: [0.01, 0.01, 0.01] + O: [1, 1, 1] + V: [0.01, 0.01, 0.01] + time_tolerance: 0.1 + node_rate: 100 + map: + type: "MapBase" + plugin: "core" + + solver: + follow: "solver.yaml" + + + sensors: + - + type: "SensorLaser2d" + name: "scanner_front_left" + plugin: "laser" + follow: "test_laser_params.yaml" + - + type: "SensorImu" + name: "bno" + plugin: "imu" + extrinsic: + pose: [0,0,0,0,0,0,1] + follow: "test_imu_params_bno.yaml" + - + type: "SensorImu" + name: "microstrain" + plugin: "imu" + extrinsic: + pose: [0,0,0,0,0,0,1] + follow: "test_imu_params_microstrain.yaml" + + processors: + - + type: "ProcessorOdomIcp" + name: "processorodomicp" + sensor_name: "scanner_front_left" + plugin: "laser" + follow: "test_laser_processor.yaml" + # - + # type: "ProcessorLoopClosureIcp" + # name: "processorloopclosureicp" + # sensor_name: "scanner_front_left" + # plugin: "laser" + # time_tolerance: 0.1 + # apply_loss_function: true + # keyframe_vote: + # voting_active: false + # recent_frames_ignored: 10 + # frames_ignored_after_loop: 0 + # max_error_threshold: 0.02 + # min_points_percent: 40 + # max_loops: 1 + # max_candidates: 5 + # max_attempts: 5 + # candidate_generation: "random" # 'random' or 'tree' + # icp: + # follow: "csm.yaml" + - + type: "ProcessorImu" + name: "processorimubno" + sensor_name: "bno" + plugin: "imu" + follow: "test_imu_processor_bno.yaml" + - + type: "ProcessorImu" + name: "processorimumicro" + sensor_name: "microstrain" + plugin: "imu" + follow: "test_imu_processor_microstrain.yaml" + + ROS subscriber: + - + package: "wolf_ros_laser" + type: "SubscriberLaser2d" + # topic: "/ana/sensors/scan" + topic: "/helena/sensors/scan" + sensor_name: "scanner_front_left" + load_params_from_msg: true + - + package: "wolf_ros_imu" + type: "SubscriberImuEnableable" + # topic: "/ana/sensors/bno055_imu/imumal" + topic: "/helena/sensors/bno055_imu/imumal" + sensor_name: "bno" + follow: "test_imu_subscriber_bno.yaml" + - + package: "wolf_ros_imu" + type: "SubscriberImuEnableable" + # topic: "/ana/imu/data" + topic: "/helena/imu/data" + sensor_name: "microstrain" + follow: "test_imu_subscriber_microstrain.yaml" + + ROS publisher: + - + package: "wolf_ros_node" + type: "PublisherTf" + topic: " " + period: 0.1 + follow: "test_publisher_ros_node.yaml" + - + package: "wolf_ros_node" + type: "PublisherGraph" + topic: "graph" + period: 1 + viz_overlapped_factors: true + # - + # package: "wolf_ros_node" + # type: "PublisherTrajectory" + # topic: "trajectory" + # period: 1 + # frame_id: "map" + - + package: "wolf_ros_node" + type: "PublisherPose" + topic: "pose" + extrinsics: false + period: 1 + frame_id: "map" + # - + # package: "wolf_ros_laser" + # type: "PublisherLaserMap" + # topic: "map" + # period: 1 + # map_frame_id: "map" + # update_dist_th: 0.05 + # update_angle_th: 0.05 + # max_n_cells: 1000000 + # grid_size: 0.1 + # p_free: 0.3 + # p_obst: 0.8 + # p_free_th: 0.2 + # p_obst_th: 0.9 + # discard_max_range: true +