Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Complied for python3 and added parameters in config file #14

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Complied for python3 and added parameters in config file #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
12189da
particle filter sample commit
va26 Sep 13, 2023
6761df5
first complied particle filter
va26 Sep 13, 2023
4b4343b
clicked pose tested
va26 Sep 13, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 47 additions & 0 deletions config/pf_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
# particle_filter_params.yaml

# Particle Filter
scan_topic: /scan
odometry_topic: /odom

# range data is downsampled by this factor
angle_step: 18
max_particles: 10000
max_viz_particles: 10

# range_method: glt
range_method: cddt
squash_factor: 3.2
theta_discretization: 180

# max sensor range in meters
max_range: 10
viz: false
fine_timing: false
publish_odom: true

# sensor model constants
z_short: 0.01
z_max: 0.07
z_rand: 0.12
z_hit: 0.75
sigma_hit: 8.0

# motion model dispersion constant


# motion model dispersion constant, this could be improved. Large values will spread out
# the particle distribution more. Too much and it is overly computational to represent the
# true prob. dist, too little and the particle filter can diverge at high speeds where the
# ackermann motion model is inaccurate

motion_dispersion_x: 0.05
motion_dispersion_y: 0.025
motion_dispersion_theta: 0.025

# rangelib_variant
# This option switches between different sensor model variants, high values are more
# optimized. range_variant 3 does not work for rmgpu, but variant 2 is very good. variant
# 4 only works with cddt or pcddt range_method options

rangelib_variant: 4
40 changes: 40 additions & 0 deletions launch/localise.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
<launch>

<!-- Map Server -->
<arg name="map_file" default="$(env HOME)/f1tenth_ws/maps/map.yaml"/>
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />


<!-- Particle Filter -->
<rosparam command="load" file="$(find particle_filter)/config/pf_params.yaml" />

<node pkg="particle_filter" type="particle_filter.py" name="Particle_filter" output="screen">

<param name="range_method" value="cddt"/>
<!-- 4 for cddt and 2 for glt-->
<param name="rangelib_variant" value="4"/>

</node>

</launch>


<!-- Publish Odom
<group if="$(arg publish_covariance)">
<param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
<param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
<param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
</group>
<node pkg="particle_filter" type="pose_to_odom.py" name="pose_to_odom" ></node>
-->
<!-- Laser Scan Matcher
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">

<remap from="imu/data" to="imu"/>
<param name="max_iterations" value="10"/>
<param name="use_imu" value="true"/>
<param name="use_odom" value="false"/>
<param name="fixed_frame" value="map"/>

</node>
-->
130 changes: 130 additions & 0 deletions launch/localise_bag.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,130 @@
<launch>
<!--

This launch file initializes particle filter localization.

arguments:
scan_topic - which ROS topic to listen for laser scan information
odom_topic - which ROS topic to listen for odometry information
max_particles - number of particles to maintain
angle_step - laser scan ranges are downsampled by this factor
viz - whether or not to publish visualization topics.
even if this is enabled, topics are not published unless there are subscribers
squash_factor - exponent used to "squash" the particle weights before normaliztation
helps reduce problems associated with highly peaked prob. distributions

important parameters:
range_method: determines which RangeLibc algorithm to use. Options:
"bl": Bresenham's line. Slowest performance.
"rm": Ray Marching. Reasonably fast but suboptimal performance.
"rmgpu": GPU implementation of Ray Marching. Requires RangeLibc to be compiled WITH_CUDA=ON
"cddt": Compressed Directional Distance Transform. Compressed map representation. Fast query time.
"pcddt": Same as "cddt" but the data structure is pruned to remove unnecessary elements
"glt": Lookup table. Precomputes ray cast distances for a discretized state space.
Fast performance on the CPU but long initialization time.
max_range: maximum range of the laser scanner in meters. Measured ranges beyond max_range are clipped.
fine_timing: prints extra timing information, useful for profiling the particle filter
publish_odom: whether or not to publish inferred pose as an odometry message.
publishes on /pf/pose/odom
-->
<param name="/use_sim_time" value="true"/>

<arg name="map_file" default="/home/jetson2/ninad/map.yaml"/>
<arg name="scan_topic" default="/scan"/>

<!-- <arg name="odometry_topic" default="/vesc/odom"/> -->
<!-- <arg name="odometry_topic" default="/camera/odom/sample"/> -->
<arg name="odometry_topic" default="/odom"/>


<arg name="angle_step" default="18"/>
<arg name="max_particles" default="10000"/>
<arg name="squash_factor" default="2.2"/>
<arg name="viz" default="1"/>

<arg name="publish_covariance" default="false"/>


<!-- Map Server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />

<!-- Publish Odom -->
<group if="$(arg publish_covariance)">
<param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
<param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
<param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
</group>
<node pkg="particle_filter" type="pose_to_odom.py" name="pose_to_odom" ></node>

<!-- Laser Scan Matcher -->
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">

<remap from="imu/data" to="imu"/>
<param name="max_iterations" value="10"/>
<param name="use_imu" value="true"/>
<param name="use_odom" value="false"/>
<param name="fixed_frame" value="map"/>

</node>

<!-- Particle Filter -->
<node pkg="particle_filter" type="particle_filter.py" name="Particle_filter" output="screen">
<remap from="/map" to="map"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<param name="odometry_topic" value="$(arg odometry_topic)"/>

<!-- range data is downsampled by this factor -->
<param name="angle_step" value="$(arg angle_step)"/>
<param name="max_particles" value="$(arg max_particles)"/>
<param name="max_viz_particles" value="10"/>

<!-- <param name="range_method" value="glt"/> -->
<param name="range_method" value="cddt"/>
<param name="squash_factor" value="$(arg squash_factor)"/>
<param name="theta_discretization" value="180"/>

<!-- max sensor range in meters -->
<param name="max_range" value="10"/>
<param name="viz" value="$(arg viz)"/>
<param name="fine_timing" value="0"/>
<param name="publish_odom" value="1"/>

<!-- sensor model constants, see sensor model section in the Lab5 guide document -->
<param name="z_short" value="0.01"/>
<param name="z_max" value="0.07"/>
<param name="z_rand" value="0.12"/>
<param name="z_hit" value="0.75"/>
<param name="sigma_hit" value="8.0"/>

<!-- motion model dispersion constant, this could be improved. Large values will spread out
the particle distribution more. Too much and it is overly computational to represent the
true prob. dist, too little and the particle filter can diverge at high speeds where the
ackermann motion model is inaccurate
-->
<param name="motion_dispersion_x" value="0.05"/>
<param name="motion_dispersion_y" value="0.025"/>
<param name="motion_dispersion_theta" value="0.025"/>

<!-- this option switches between different sensor model variants, high values are more
optimized. range_variant 3 does not work for rmgpu, but variant 2 is very good. variant
4 only works with cddt or pcddt range_method options
-->
<param name="rangelib_variant" value="4"/>

</node>

<!-- Path Publisher -->
<node name="path_publisher" pkg="particle_filter" type="path_publisher.py" output="screen">
<param name="path_publisher_name" value="pf_cddt_path_node" />
<param name="pub_topic" value="pf_cddt_path" />
<param name="sub_topic" value="/pf/viz/inferred_pose" /> <!--/mcl_pose-->
<param name="groud_truth_aval" value="1" />
<param name="groud_truth_topic" value="/camera/odom/sample" />
</node>

<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find particle_filter)/rviz/pf_vis.rviz"/>


</launch>
5 changes: 3 additions & 2 deletions launch/map_server.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
<launch>
<arg name="map" default="$(find particle_filter)/maps/basement_fixed.map.yaml"/>
<!-- <arg name="map" default="$(find racer)/maps/map.yaml"/> -->
<arg name="map" default="$(find racer)/maps/hector.yaml"/>
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map)" />
</launch>
</launch>
13 changes: 13 additions & 0 deletions launch/path_publish.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<?xml version="1.0"?>

<launch>

<node name="path_publisher" pkg="racer" type="path_publisher.py" output="screen">
<param name="path_publisher_name" value="pf_cddt_path_node" />
<param name="pub_topic" value="pf_cddt_path" />
<param name="sub_topic" value="/pf/viz/inferred_pose" /> <!--/mcl_pose-->
<param name="groud_truth_aval" value="1" />
<param name="groud_truth_topic" value="/camera/odom/sample" />
</node>

</launch>
Loading