diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77839e2..cb7d82f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,8 +1,10 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
project(particle_filter)
-## Add support for C++11, supported in ROS Kinetic and newer
-# add_definitions(-std=c++11)
+# Default to C++14
+if (NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -13,106 +15,11 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
)
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-#catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-## * add a build_depend tag for "message_generation"
-## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-## * add "message_generation" and every package in MSG_DEP_SET to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * add "message_runtime" and every package in MSG_DEP_SET to
-## catkin_package(CATKIN_DEPENDS ...)
-## * uncomment the add_*_files sections below as needed
-## and list every .msg/.srv/.action file to be processed
-## * uncomment the generate_messages entry below
-## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-#add_message_files(
-# DIRECTORY msg
-# FILES
-# Detection.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# sensor_msgs std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
+# Specify catkin package
catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES particle_filter
CATKIN_DEPENDS message_runtime rospy sensor_msgs std_msgs
-# DEPENDS system_lib
)
-###########
-## Build ##
-###########
-
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
@@ -120,80 +27,16 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/particle_filter.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/particle_filter_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_particle_filter.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+### Install python nodes
+install(PROGRAMS
+src/particle_filter.py
+src/utils.py
+DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+# Install launch and other files
+install(DIRECTORY
+ launch
+ maps
+ rviz
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/README.md b/README.md
index 93eaca3..c0a8b9c 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,7 @@ To run this, you need to ensure that both the map_server ROS package, and the py
For the map server:
```
sudo apt-get update
-rosdep install -r --from-paths src --ignore-src --rosdistro kinetic -y
+rosdep install --from-paths src/ --ignore-src -y
```
For [RangeLibc](https://github.com/kctess5/range_libc):
diff --git a/launch/localize.launch b/launch/localize.launch
index 56358a4..df6f608 100644
--- a/launch/localize.launch
+++ b/launch/localize.launch
@@ -27,7 +27,7 @@
publish_odom: whether or not to publish inferred pose as an odometry message.
publishes on /pf/pose/odom
-->
-
+
@@ -45,7 +45,7 @@
-
+
diff --git a/package.xml b/package.xml
index 057a47c..515d66b 100644
--- a/package.xml
+++ b/package.xml
@@ -1,60 +1,20 @@
-
+
particle_filter
0.1.0
Particle Filter Localization using RangeLibc for accelerated ray casting.
-
-
-
+
Corey Walsh
+ Jane Doe
-
-
-
-
MIT
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
catkin
- rospy
- message_runtime
- sensor_msgs
- std_msgs
- map_server
- rospy
- sensor_msgs
- std_msgs
- map_server
- message_runtime
-
-
-
-
-
-
-
+ rospy
+ message_runtime
+ sensor_msgs
+ std_msgs
+ map_server
diff --git a/src/particle_filter.py b/src/particle_filter.py
index 05082f8..7a16821 100755
--- a/src/particle_filter.py
+++ b/src/particle_filter.py
@@ -122,7 +122,7 @@ def __init__(self):
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1)
self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1)
- print "Finished initializing, waiting on messages..."
+ print("Finished initializing, waiting on messages...")
def get_omap(self):
'''
@@ -140,13 +140,13 @@ def get_omap(self):
self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution)
# initialize range method
- print "Initializing range method:", self.WHICH_RM
+ print("Initializing range method:", self.WHICH_RM)
if self.WHICH_RM == "bl":
self.range_method = range_libc.PyBresenhamsLine(oMap, self.MAX_RANGE_PX)
elif "cddt" in self.WHICH_RM:
self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if self.WHICH_RM == "pcddt":
- print "Pruning..."
+ print("Pruning...")
self.range_method.prune()
elif self.WHICH_RM == "rm":
self.range_method = range_libc.PyRayMarching(oMap, self.MAX_RANGE_PX)
@@ -154,7 +154,7 @@ def get_omap(self):
self.range_method = range_libc.PyRayMarchingGPU(oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == "glt":
self.range_method = range_libc.PyGiantLUTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
- print "Done loading map"
+ print("Done loading map")
# 0: permissible, -1: unmapped, 100: blocked
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
@@ -263,12 +263,12 @@ def lidarCB(self, msg):
Initializes reused buffers, and stores the relevant laser scanner data for later use.
'''
if not isinstance(self.laser_angles, np.ndarray):
- print "...Received first LiDAR message"
+ print("...Received first LiDAR message")
self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
self.downsampled_angles = np.copy(self.laser_angles[0::self.ANGLE_STEP]).astype(np.float32)
self.viz_queries = np.zeros((self.downsampled_angles.shape[0],3), dtype=np.float32)
self.viz_ranges = np.zeros(self.downsampled_angles.shape[0], dtype=np.float32)
- print self.downsampled_angles.shape[0]
+ print(self.downsampled_angles.shape[0])
# store the necessary scanner information for later processing
self.downsampled_ranges = np.array(msg.ranges[::self.ANGLE_STEP])
@@ -299,7 +299,7 @@ def odomCB(self, msg):
self.last_stamp = msg.header.stamp
self.odom_initialized = True
else:
- print "...Received first Odometry message"
+ print("...Received first Odometry message")
self.last_pose = pose
# this topic is slower than lidar, so update every time we receive a message
@@ -318,8 +318,8 @@ def initialize_particles_pose(self, pose):
'''
Initialize particles in the general region of the provided pose.
'''
- print "SETTING POSE"
- print pose
+ print("SETTING POSE")
+ print(pose)
self.state_lock.acquire()
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
self.particles[:,0] = pose.position.x + np.random.normal(loc=0.0,scale=0.5,size=self.MAX_PARTICLES)
@@ -331,7 +331,7 @@ def initialize_global(self):
'''
Spread the particle distribution over the permissible region of the state space.
'''
- print "GLOBAL INITIALIZATION"
+ print("GLOBAL INITIALIZATION")
# randomize over grid coordinate space
self.state_lock.acquire()
permissible_x, permissible_y = np.where(self.permissible_region == 1)
@@ -355,7 +355,7 @@ def precompute_sensor_model(self):
This table is indexed by the sensor model at runtime by discretizing the measurements
and computed ranges from RangeLibc.
'''
- print "Precomputing sensor model"
+ print("Precomputing sensor model")
# sensor model constants
z_short = self.Z_SHORT
z_max = self.Z_MAX
@@ -368,11 +368,11 @@ def precompute_sensor_model(self):
t = time.time()
# d is the computed range from RangeLibc
- for d in xrange(table_width):
+ for d in range(table_width):
norm = 0.0
sum_unkown = 0.0
# r is the observed range from the lidar unit
- for r in xrange(table_width):
+ for r in range(table_width):
prob = 0.0
z = float(r-d)
# reflects from the intended object
@@ -496,7 +496,7 @@ def sensor_model(self, proposal_dist, obs, weights):
# apply the squash factor
self.weights = np.power(self.weights, self.INV_SQUASH_FACTOR)
else:
- print "Cannot use radial optimizations with non-CDDT based methods, use rangelib_variant 2"
+ print("Cannot use radial optimizations with non-CDDT based methods, use rangelib_variant 2")
elif self.RANGELIB_VAR == VAR_REPEAT_ANGLES_EVAL_SENSOR_ONE_SHOT:
self.queries[:,:] = proposal_dist[:,:]
self.range_method.calc_range_repeat_angles_eval_sensor_model(self.queries, self.downsampled_angles, obs, self.weights)
@@ -521,8 +521,8 @@ def sensor_model(self, proposal_dist, obs, weights):
t_total = (t_squash - t_start) / 100.0
if self.SHOW_FINE_TIMING and self.iters % 10 == 0:
- print "sensor_model: init: ", np.round((t_init-t_start)/t_total, 2), "range:", np.round((t_range-t_init)/t_total, 2), \
- "eval:", np.round((t_eval-t_range)/t_total, 2), "squash:", np.round((t_squash-t_eval)/t_total, 2)
+ print("sensor_model: init: ", np.round((t_init-t_start)/t_total, 2), "range:", np.round((t_range-t_init)/t_total, 2), \
+ "eval:", np.round((t_eval-t_range)/t_total, 2), "squash:", np.round((t_squash-t_eval)/t_total, 2))
elif self.RANGELIB_VAR == VAR_CALC_RANGE_MANY_EVAL_SENSOR:
# this version demonstrates what this would look like with coordinate space conversion pushed to rangelib
# this part is inefficient since it requires a lot of effort to construct this redundant array
@@ -556,12 +556,12 @@ def sensor_model(self, proposal_dist, obs, weights):
intrng = np.rint(ranges).astype(np.uint16)
# compute the weight for each particle
- for i in xrange(self.MAX_PARTICLES):
+ for i in range(self.MAX_PARTICLES):
weight = np.product(self.sensor_model_table[intobs,intrng[i*num_rays:(i+1)*num_rays]])
weight = np.power(weight, self.INV_SQUASH_FACTOR)
weights[i] = weight
else:
- print "PLEASE SET rangelib_variant PARAM to 0-4"
+ print("PLEASE SET rangelib_variant PARAM to 0-4")
def MCL(self, a, o):
'''
@@ -598,8 +598,8 @@ def MCL(self, a, o):
t_total = (t_norm - t)/100.0
if self.SHOW_FINE_TIMING and self.iters % 10 == 0:
- print "MCL: propose: ", np.round((t_propose-t)/t_total, 2), "motion:", np.round((t_motion-t_propose)/t_total, 2), \
- "sensor:", np.round((t_sensor-t_motion)/t_total, 2), "norm:", np.round((t_norm-t_sensor)/t_total, 2)
+ print("MCL: propose: ", np.round((t_propose-t)/t_total, 2), "motion:", np.round((t_motion-t_propose)/t_total, 2), \
+ "sensor:", np.round((t_sensor-t_motion)/t_total, 2), "norm:", np.round((t_norm-t_sensor)/t_total, 2))
# save the particles
self.particles = proposal_distribution
@@ -616,7 +616,7 @@ def update(self):
'''
if self.lidar_initialized and self.odom_initialized and self.map_initialized:
if self.state_lock.locked():
- print "Concurrency error avoided"
+ print("Concurrency error avoided")
else:
self.state_lock.acquire()
self.timer.tick()
@@ -642,7 +642,7 @@ def update(self):
ips = 1.0 / (t2 - t1)
self.smoothing.append(ips)
if self.iters % 10 == 0:
- print "iters per sec:", int(self.timer.fps()), " possible:", int(self.smoothing.mean())
+ print("iters per sec:", int(self.timer.fps()), " possible:", int(self.smoothing.mean()))
self.visualize()
@@ -656,7 +656,7 @@ def load_params_from_yaml(fp):
with open(fp, 'r') as infile:
yaml_data = load(infile)
for param in yaml_data:
- print "param:", param, ":", yaml_data[param]
+ print("param:", param, ":", yaml_data[param])
rospy.set_param("~"+param, yaml_data[param])
# this function can be used to generate flame graphs easily
diff --git a/src/utils.py b/src/utils.py
old mode 100644
new mode 100755
index 4d26c34..148d995
--- a/src/utils.py
+++ b/src/utils.py
@@ -82,7 +82,7 @@ def particles_to_poses(particles):
''' Converts a two dimensional array of particles into an array of Poses.
Particles can be a array like [[x0, y0, theta0], [x1, y1, theta1]...]
'''
- return map(particle_to_pose, particles)
+ return [*map(particle_to_pose, particles)]
def make_header(frame_id, stamp=None):
''' Creates a Header object for stamped ROS objects '''