mirror of
https://github.com/caperren/school_archives.git
synced 2025-11-09 13:41:13 +00:00
Added new coursework, cleaned up structure
This commit is contained in:
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/.DS_Store
vendored
Normal file
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/.DS_Store
vendored
Normal file
Binary file not shown.
@@ -0,0 +1,19 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(rob456_hw3)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
gmapping
|
||||
nav_msgs
|
||||
roscpp
|
||||
rospy
|
||||
stage_ros
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
Binary file not shown.
@@ -0,0 +1,175 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package gmapping
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.3.9 (2017-10-22)
|
||||
------------------
|
||||
* remove unused file
|
||||
* add missing nodelet dependency to find_package
|
||||
* make rostest in CMakeLists optional (`ros/rosdistro#3010 <https://github.com/ros/rosdistro/issues/3010>`_)
|
||||
* Add nodelet implementation. (`#41 <https://github.com/ros-perception/slam_gmapping/issues/41>`_)
|
||||
* Add nodelet implementation.
|
||||
Add additional nodelet layer to mirror the node
|
||||
implementation. This allows the Slam GMapping
|
||||
library to be run as a nodelet instead. This
|
||||
would allow you to, for example, run it under
|
||||
the same nodelet manager as the nodelet producing
|
||||
the /scan output for greater efficiency.
|
||||
* Remove superfluous semicolons
|
||||
Removed superfluous semicolons and
|
||||
mildly clarified info stream output.
|
||||
* fix comment, change type from double to int (`#40 <https://github.com/ros-perception/slam_gmapping/issues/40>`_)
|
||||
* fix comment, change type from double to int
|
||||
* fix comment, iterations param is not double but int
|
||||
* Contributors: David Hodo, Kevin Wells, Lukas Bulwahn, Oscar Lima, Vincent Rabaud
|
||||
|
||||
1.3.8 (2015-07-31)
|
||||
------------------
|
||||
* fix a test that would take too long sometimes
|
||||
* better verbosity
|
||||
* add a test for upside down lasers
|
||||
* add a test for symmetry
|
||||
* make sure the laser sent to gmapping is always centered
|
||||
* do not display warning message if scan is processed at some point
|
||||
* Contributors: Vincent Rabaud
|
||||
|
||||
1.3.7 (2015-07-04)
|
||||
------------------
|
||||
* get replay to behave like live processing
|
||||
* Contributors: Vincent Rabaud
|
||||
|
||||
1.3.6 (2015-06-26)
|
||||
------------------
|
||||
* Don't crash on exit from replay.
|
||||
* replay: Add "on_done" command line parameter.
|
||||
Example usage:
|
||||
ros run gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag --on_done "rosrun map_server map_saver -f foo" _particles:=100 _maxUrange:=10
|
||||
* replay: Handle order-of-data-in-bag-file issues better at startup.
|
||||
Silently discard scans at startup that arrive prior to the TF frames
|
||||
required by those scans.
|
||||
TF can't interpolate from non-existant data, so discard scans that
|
||||
arrive with timestamps prior to the first TF frame. We do this
|
||||
expediently by discarding laser scans that throw
|
||||
ExtrapolationException's until/unless we successfully process our first
|
||||
scan.
|
||||
Similarly, ignore LookupException's at startup.
|
||||
* Fix indexing error for inverting scan.
|
||||
* add test dependencies
|
||||
* get replay test to pass
|
||||
* rename test
|
||||
* fixing include order
|
||||
removing pointer for Gmapping object
|
||||
* Minor Fix thanks to vrabaud comments
|
||||
* [new feature] replay on bag file
|
||||
The aim is to provide a way to get exactly the same map after running
|
||||
gmapping multiple times on the same rosbag file. It wasn't possible with the
|
||||
tool 'rosbag play', indeed one missing laser scan could provide really
|
||||
different results.
|
||||
Moreover, this modification allow to process rosbag offline at the maximum
|
||||
speed with the guarantee that all lasers scans are processed. It is
|
||||
useful in automatic tests and when finding optimal gmapping parameters with a script.
|
||||
Example usage:
|
||||
rosrun gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag _particles:=100 _maxUrange:=10
|
||||
more options:
|
||||
rosrun gmapping slam_gmapping_replay --help
|
||||
* spliting init and constructor
|
||||
The objective is to allow future handling of replay offline rosbag files.
|
||||
This commit also add a variable for the seed, with the objective is to allow to
|
||||
have repeateable run (for future offline rosbag replay)
|
||||
* Added cfloat include
|
||||
* Change arbitrary constant to FLT_EPSILON
|
||||
* Added check that scan goes from -x to x
|
||||
* Contributors: Laurent GEORGE, Patrick Doyle, Qiao Huang, Vincent Rabaud, Windel Bouwman
|
||||
|
||||
1.3.5 (2014-08-28)
|
||||
------------------
|
||||
* Fixed typo in slam_gmapping_pr2.launch
|
||||
Fixed a typo in the launchfile in the parameter "map_update_interval".
|
||||
* Contributors: DaMalo
|
||||
|
||||
1.3.4 (2014-08-07)
|
||||
------------------
|
||||
* Reenabled temporal update in slam_gmapping.cpp
|
||||
* Contributors: DaMalo
|
||||
|
||||
1.3.3 (2014-06-23)
|
||||
------------------
|
||||
* Adding the ability to set openslam_gmapping miminumScore through the ros parameter minimumScore
|
||||
* Contributors: Koen Lekkerkerker, William Woodall
|
||||
|
||||
1.3.2 (2014-01-14)
|
||||
------------------
|
||||
* Contributors: Vincent Rabaud
|
||||
|
||||
1.3.1 (2014-01-13)
|
||||
------------------
|
||||
* Fix usage of scoped locks so that they are not immediately destroyed.
|
||||
fixes `#11 <https://github.com/ros-perception/slam_gmapping/issues/11>`_
|
||||
* check for CATKIN_ENABLE_TESTING
|
||||
* Contributors: Lukas Bulwahn, Stefan Kohlbrecher, William Woodall
|
||||
|
||||
1.3.0 (2013-06-28)
|
||||
------------------
|
||||
* Renamed to gmapping, adding metapackage for slam_gmapping
|
||||
* catkinize slam_gmapping
|
||||
* Changed reference frame from base to laser to account for upside down and/or back facing laserscanners.
|
||||
- Added a check if the scanner is facing down
|
||||
- Added a safety check if the scanner is aligned planar
|
||||
- Made laserscan min- and max-angles global as they are needed later for scanners with a negative angle-increment
|
||||
- Replaced the base->laser pose for gmapping with the identity transform and included the base->laser part into the gmap_pose
|
||||
- Removed a transform-lookup from the map->odom transformation process as it is not needed anymore
|
||||
These changes should make gmapping more robust against laserscanners that are mounted upside down, facing backwards or are rotating counter-clockwise.
|
||||
It will also allow gmapping to work with panning laserscanners, since the transform base->laser is no longer considered fixed.
|
||||
* Fix poorly formed paths in patches
|
||||
These patches won't apply in Fedora because they contain "..", which is considered "unsafe"
|
||||
* Fixed test files to use the new rosbag command layout.
|
||||
* Respect tf_prefix when sending maps
|
||||
* Fixed tf expiration
|
||||
* Added tf_delay param
|
||||
* Add gcc 4.7 patch and Precise support by removing wiped during installed
|
||||
* Oneiric linker fixes, bump version to 1.2.6
|
||||
* Convert to not use bullet datatypes directly
|
||||
* Rejiggered linker lines to accommodate Oneiric's stricter linker behavior.
|
||||
* Now uses angle_increment provided in laser scan message, instead of computing it myself (not sure why I was doing that, anyway), `#4730 <https://github.com/ros-perception/slam_gmapping/issues/4730>`_
|
||||
* Applied patch to avoid assert when laser gives varying number of beams per
|
||||
scan, `#4856 <https://github.com/ros-perception/slam_gmapping/issues/4856>`_. Added the bag from that ticket as a test case.
|
||||
* Applied patch from `#4984 <https://github.com/ros-perception/slam_gmapping/issues/4984>`_ to fix occ grid generation with lasers that provide scans in reverse order
|
||||
* Applied patch from `#4583 <https://github.com/ros-perception/slam_gmapping/issues/4583>`_ with misc fixes to our patch against gmapping
|
||||
* Excluded test program from all build
|
||||
* Applied typo fix from Maurice Fallon
|
||||
* Added Ubuntu platform tags to manifest
|
||||
* Removed unused inverted_laser parameter
|
||||
* Added transform logic necessary to account for non-horizontal lasers. This
|
||||
change is intended to handle upside-down lasers, but should also work for
|
||||
non-planar lasers (as long as the vertical structure of the environment is
|
||||
continuous), `#3052 <https://github.com/ros-perception/slam_gmapping/issues/3052>`_. I tested minimally with a hacked version of Stage, but
|
||||
this functionality still needs to be validated on data from a real robot
|
||||
with an upside-down laser.
|
||||
* Reindexed bag used in testing
|
||||
* Added publication of entropy
|
||||
* add entropy computation method
|
||||
* Added occ_thresh parameter
|
||||
* Turning time based updates off by default
|
||||
* Updating so that gmapping updates on a timer when not moving. Added the temporalUpdate parameter and updated docs.
|
||||
* Updated md5sums for new bags
|
||||
* Threading publishing of transforms so that they are published regularly regardless of how long map updates take.
|
||||
* Updated patch to fix gcc 4.4 warning, and made top-level Makefile call through to Makefile.gmapping on clean
|
||||
* Updating to work with the navigation stack. Now publishes header information on map messages.
|
||||
* Applied patch to update tf usage, `#3457 <https://github.com/ros-perception/slam_gmapping/issues/3457>`_
|
||||
* Remove use of deprecated rosbuild macros
|
||||
* Removed unused parameter
|
||||
* Fix the position gmapping gives to the map's info. Was trying to center the map on the origin, when it should just have been using the world positiong of the map's origin (`#3037 <https://github.com/ros-perception/slam_gmapping/issues/3037>`_)
|
||||
* Added doc cleared to manifest
|
||||
* Switched sleep to WallDuration, to avoid getting stuck after rosplay has run out of time data to publish
|
||||
* Converted from tf::MessageNotifier to tf::MessageFilter.
|
||||
* Reverted accidental change to CMakeLists.txt
|
||||
* Added advertisement and publication of MapMetaData (docs are updated to
|
||||
match). Added minimal test for the resulting map. Updated local params to use
|
||||
NodeHandle("~").
|
||||
* Added latched topic version of map, API cleared
|
||||
* Updated manifest to explain version that we're using
|
||||
* Remove ros/node.h inclusion
|
||||
* tf publishes on new topic: \tf. See ticket `#2381 <https://github.com/ros-perception/slam_gmapping/issues/2381>`_
|
||||
* Merging in changes from reorgnization of laser pipeline.
|
||||
* removed redundant code (getOdomPose) that could result in unnecessary warnings
|
||||
* Contributors: Ben Struss, Dave Hershberger, Dereck Wonnacott, Mike Ferguson, Scott K Logan, Vincent Rabaud, William Woodall, duhadway-bosch, eitan, gerkey, jfaust, jleibs, kwc, meeussen, vrabaud, wheeler
|
||||
@@ -0,0 +1,90 @@
|
||||
cmake_minimum_required(VERSION 2.8)
|
||||
project(gmapping)
|
||||
|
||||
find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
|
||||
|
||||
find_package(Boost REQUIRED signals)
|
||||
|
||||
include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
|
||||
|
||||
include_directories(src)
|
||||
|
||||
catkin_package()
|
||||
|
||||
add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp)
|
||||
target_link_libraries(slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
if(catkin_EXPORTED_TARGETS)
|
||||
add_dependencies(slam_gmapping ${catkin_EXPORTED_TARGETS})
|
||||
endif()
|
||||
|
||||
add_library(slam_gmapping_nodelet src/slam_gmapping.cpp src/nodelet.cpp)
|
||||
target_link_libraries(slam_gmapping_nodelet ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp)
|
||||
target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
if(catkin_EXPORTED_TARGETS)
|
||||
add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS})
|
||||
endif()
|
||||
|
||||
install(TARGETS slam_gmapping slam_gmapping_replay
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
if(TARGET tests)
|
||||
add_executable(gmapping-rtest EXCLUDE_FROM_ALL test/rtest.cpp)
|
||||
target_link_libraries(gmapping-rtest ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
|
||||
add_dependencies(tests gmapping-rtest)
|
||||
endif()
|
||||
|
||||
# Need to make the tests more robust; currently the output map can differ
|
||||
# substantially between runs.
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_basic_localization_stage_indexed.bag
|
||||
http://download.ros.org/data/gmapping/basic_localization_stage_indexed.bag
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 322a0014f47bcfbb0ad16a317738b0dc)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag
|
||||
http://download.ros.org/data/gmapping/hallway_slow_2011-03-04-21-41-33.bag
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 e772b89713693adc610f4c5b96f5dc03)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm
|
||||
http://download.ros.org/data/gmapping/basic_localization_stage_groundtruth.pgm
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 abf208f721053915145215b18c98f9b3)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_test_replay_crash.bag
|
||||
https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_replay_crash.bag
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 bb0e086207eb4fccf0b13d3406f610a1)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_test_turtlebot.bag
|
||||
https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_turtlebot.bag
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 402e1e5f7c00445d2a446e58e3151830)
|
||||
catkin_download_test_data(
|
||||
${PROJECT_NAME}_test_upside_down.bag
|
||||
https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_upside_down.bag
|
||||
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
|
||||
MD5 3b026a2144ec14f3fdf218b5c7077d54)
|
||||
set(LOCAL_DEPENDENCIES gmapping-rtest ${PROJECT_NAME}_basic_localization_stage_indexed.bag
|
||||
${PROJECT_NAME}_test_replay_crash.bag
|
||||
${PROJECT_NAME}_test_turtlebot.bag
|
||||
${PROJECT_NAME}_test_upside_down.bag
|
||||
${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag
|
||||
${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm
|
||||
slam_gmapping
|
||||
slam_gmapping_replay
|
||||
)
|
||||
add_rostest(test/basic_localization_stage.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
add_rostest(test/basic_localization_stage_replay.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
add_rostest(test/basic_localization_stage_replay2.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
add_rostest(test/basic_localization_symmetry.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
add_rostest(test/basic_localization_upside_down.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
add_rostest(test/basic_localization_laser_different_beamcount.test DEPENDENCIES ${LOCAL_DEPENDENCIES})
|
||||
endif()
|
||||
@@ -0,0 +1,34 @@
|
||||
<launch>
|
||||
<param name="use_sim_time" value="true"/>
|
||||
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
|
||||
<remap from="scan" to="base_scan"/>
|
||||
<param name="map_update_interval" value="5.0"/>
|
||||
<param name="maxUrange" value="16.0"/>
|
||||
<param name="sigma" value="0.05"/>
|
||||
<param name="kernelSize" value="1"/>
|
||||
<param name="lstep" value="0.05"/>
|
||||
<param name="astep" value="0.05"/>
|
||||
<param name="iterations" value="5"/>
|
||||
<param name="lsigma" value="0.075"/>
|
||||
<param name="ogain" value="3.0"/>
|
||||
<param name="lskip" value="0"/>
|
||||
<param name="srr" value="0.1"/>
|
||||
<param name="srt" value="0.2"/>
|
||||
<param name="str" value="0.1"/>
|
||||
<param name="stt" value="0.2"/>
|
||||
<param name="linearUpdate" value="1.0"/>
|
||||
<param name="angularUpdate" value="0.5"/>
|
||||
<param name="temporalUpdate" value="3.0"/>
|
||||
<param name="resampleThreshold" value="0.5"/>
|
||||
<param name="particles" value="30"/>
|
||||
<param name="xmin" value="-50.0"/>
|
||||
<param name="ymin" value="-50.0"/>
|
||||
<param name="xmax" value="50.0"/>
|
||||
<param name="ymax" value="50.0"/>
|
||||
<param name="delta" value="0.05"/>
|
||||
<param name="llsamplerange" value="0.01"/>
|
||||
<param name="llsamplestep" value="0.01"/>
|
||||
<param name="lasamplerange" value="0.005"/>
|
||||
<param name="lasamplestep" value="0.005"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,7 @@
|
||||
<library path="lib/libslam_gmapping_nodelet">
|
||||
<class name="SlamGMappingNodelet" type="SlamGMappingNodelet" base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
Nodelet ROS wrapper for OpenSlam's Gmapping.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,33 @@
|
||||
<package>
|
||||
<name>gmapping</name>
|
||||
<version>1.3.9</version>
|
||||
<description>This package contains a ROS wrapper for OpenSlam's Gmapping.
|
||||
The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping),
|
||||
as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy
|
||||
grid map (like a building floorplan) from laser and pose data collected by a mobile robot.
|
||||
</description>
|
||||
<author>Brian Gerkey</author>
|
||||
<maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
|
||||
<license>CreativeCommons-by-nc-sa-2.0</license>
|
||||
|
||||
<url>http://ros.org/wiki/gmapping</url>
|
||||
|
||||
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
|
||||
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>openslam_gmapping</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rostest</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>nodelet</build_depend>
|
||||
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>openslam_gmapping</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>nodelet</run_depend>
|
||||
|
||||
<export>
|
||||
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,34 @@
|
||||
/*
|
||||
* slam_gmapping
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
*
|
||||
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
|
||||
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
|
||||
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
|
||||
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
|
||||
*
|
||||
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
|
||||
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
|
||||
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
|
||||
* CONDITIONS.
|
||||
*
|
||||
*/
|
||||
|
||||
/* Author: Brian Gerkey */
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include "slam_gmapping.h"
|
||||
|
||||
int
|
||||
main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "slam_gmapping");
|
||||
|
||||
SlamGMapping gn;
|
||||
gn.startLiveSlam();
|
||||
ros::spin();
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* slam_gmapping
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
*
|
||||
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
|
||||
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
|
||||
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
|
||||
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
|
||||
*
|
||||
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
|
||||
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
|
||||
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
|
||||
* CONDITIONS.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nodelet/nodelet.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
#include "slam_gmapping.h"
|
||||
|
||||
class SlamGMappingNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
SlamGMappingNodelet() {}
|
||||
|
||||
~SlamGMappingNodelet() {}
|
||||
|
||||
virtual void onInit()
|
||||
{
|
||||
NODELET_INFO_STREAM("Initialising Slam GMapping nodelet...");
|
||||
sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle()));
|
||||
NODELET_INFO_STREAM("Starting live SLAM...");
|
||||
sg_->startLiveSlam();
|
||||
}
|
||||
|
||||
private:
|
||||
boost::shared_ptr<SlamGMapping> sg_;
|
||||
};
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet)
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Copyright 2015 Aldebaran
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
*/
|
||||
#include "slam_gmapping.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include <boost/program_options.hpp>
|
||||
#include <ros/ros.h>
|
||||
|
||||
int
|
||||
main(int argc, char** argv)
|
||||
{
|
||||
/** Define and parse the program options
|
||||
*/
|
||||
namespace po = boost::program_options;
|
||||
po::options_description desc("Options");
|
||||
desc.add_options()
|
||||
("help", "Print help messages")
|
||||
("scan_topic", po::value<std::string>()->default_value("/scan") ,"topic that contains the laserScan in the rosbag")
|
||||
("bag_filename", po::value<std::string>()->required(), "ros bag filename")
|
||||
("seed", po::value<unsigned long int>()->default_value(0), "seed")
|
||||
("max_duration_buffer", po::value<unsigned long int>()->default_value(99999), "max tf buffer duration")
|
||||
("on_done", po::value<std::string>(), "command to execute when done") ;
|
||||
|
||||
po::variables_map vm;
|
||||
try
|
||||
{
|
||||
po::store(po::parse_command_line(argc, argv, desc),
|
||||
vm); // can throw
|
||||
|
||||
/** --help option
|
||||
*/
|
||||
if ( vm.count("help") )
|
||||
{
|
||||
std::cout << "Basic Command Line Parameter App" << std::endl
|
||||
<< desc << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
po::notify(vm); // throws on error, so do after help in case
|
||||
// there are any problems
|
||||
}
|
||||
catch(po::error& e)
|
||||
{
|
||||
std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
|
||||
std::cerr << desc << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::string bag_fname = vm["bag_filename"].as<std::string>();
|
||||
std::string scan_topic = vm["scan_topic"].as<std::string>();
|
||||
unsigned long int seed = vm["seed"].as<unsigned long int>();
|
||||
unsigned long int max_duration_buffer = vm["max_duration_buffer"].as<unsigned long int>();
|
||||
|
||||
ros::init(argc, argv, "slam_gmapping");
|
||||
SlamGMapping gn(seed, max_duration_buffer) ;
|
||||
gn.startReplay(bag_fname, scan_topic);
|
||||
ROS_INFO("replay stopped.");
|
||||
|
||||
if ( vm.count("on_done") )
|
||||
{
|
||||
// Run the "on_done" command and then exit
|
||||
system(vm["on_done"].as<std::string>().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ros::spin(); // wait so user can save the map
|
||||
}
|
||||
return(0);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,791 @@
|
||||
/*
|
||||
* slam_gmapping
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
*
|
||||
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
|
||||
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
|
||||
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
|
||||
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
|
||||
*
|
||||
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
|
||||
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
|
||||
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
|
||||
* CONDITIONS.
|
||||
*
|
||||
*/
|
||||
|
||||
/* Author: Brian Gerkey */
|
||||
/* Modified by: Charles DuHadway */
|
||||
|
||||
|
||||
/**
|
||||
|
||||
@mainpage slam_gmapping
|
||||
|
||||
@htmlinclude manifest.html
|
||||
|
||||
@b slam_gmapping is a wrapper around the GMapping SLAM library. It reads laser
|
||||
scans and odometry and computes a map. This map can be
|
||||
written to a file using e.g.
|
||||
|
||||
"rosrun map_server map_saver static_map:=dynamic_map"
|
||||
|
||||
<hr>
|
||||
|
||||
@section topic ROS topics
|
||||
|
||||
Subscribes to (name/type):
|
||||
- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner
|
||||
- @b "/tf": odometry from the robot
|
||||
|
||||
|
||||
Publishes to (name/type):
|
||||
- @b "/tf"/tf/tfMessage: position relative to the map
|
||||
|
||||
|
||||
@section services
|
||||
- @b "~dynamic_map" : returns the map
|
||||
|
||||
|
||||
@section parameters ROS parameters
|
||||
|
||||
Reads the following parameters from the parameter server
|
||||
|
||||
Parameters used by our GMapping wrapper:
|
||||
|
||||
- @b "~throttle_scans": @b [int] throw away every nth laser scan
|
||||
- @b "~base_frame": @b [string] the tf frame_id to use for the robot base pose
|
||||
- @b "~map_frame": @b [string] the tf frame_id where the robot pose on the map is published
|
||||
- @b "~odom_frame": @b [string] the tf frame_id from which odometry is read
|
||||
- @b "~map_update_interval": @b [double] time in seconds between two recalculations of the map
|
||||
|
||||
|
||||
Parameters used by GMapping itself:
|
||||
|
||||
Laser Parameters:
|
||||
- @b "~/maxRange" @b [double] maximum range of the laser scans. Rays beyond this range get discarded completely. (default: maximum laser range minus 1 cm, as received in the the first LaserScan message)
|
||||
- @b "~/maxUrange" @b [double] maximum range of the laser scanner that is used for map building (default: same as maxRange)
|
||||
- @b "~/sigma" @b [double] standard deviation for the scan matching process (cell)
|
||||
- @b "~/kernelSize" @b [int] search window for the scan matching process
|
||||
- @b "~/lstep" @b [double] initial search step for scan matching (linear)
|
||||
- @b "~/astep" @b [double] initial search step for scan matching (angular)
|
||||
- @b "~/iterations" @b [int] number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations), respectively.
|
||||
- @b "~/lsigma" @b [double] standard deviation for the scan matching process (single laser beam)
|
||||
- @b "~/ogain" @b [double] gain for smoothing the likelihood
|
||||
- @b "~/lskip" @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays)
|
||||
- @b "~/minimumScore" @b [double] minimum score for considering the outcome of the scanmatching good. Can avoid 'jumping' pose estimates in large open spaces when using laser scanners with limited range (e.g. 5m). (0 = default. Scores go up to 600+, try 50 for example when experiencing 'jumping' estimate issues)
|
||||
|
||||
Motion Model Parameters (all standard deviations of a gaussian noise model)
|
||||
- @b "~/srr" @b [double] linear noise component (x and y)
|
||||
- @b "~/stt" @b [double] angular noise component (theta)
|
||||
- @b "~/srt" @b [double] linear -> angular noise component
|
||||
- @b "~/str" @b [double] angular -> linear noise component
|
||||
|
||||
Others:
|
||||
- @b "~/linearUpdate" @b [double] the robot only processes new measurements if the robot has moved at least this many meters
|
||||
- @b "~/angularUpdate" @b [double] the robot only processes new measurements if the robot has turned at least this many rads
|
||||
|
||||
- @b "~/resampleThreshold" @b [double] threshold at which the particles get resampled. Higher means more frequent resampling.
|
||||
- @b "~/particles" @b [int] (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled
|
||||
|
||||
Likelihood sampling (used in scan matching)
|
||||
- @b "~/llsamplerange" @b [double] linear range
|
||||
- @b "~/lasamplerange" @b [double] linear step size
|
||||
- @b "~/llsamplestep" @b [double] linear range
|
||||
- @b "~/lasamplestep" @b [double] angular step size
|
||||
|
||||
Initial map dimensions and resolution:
|
||||
- @b "~/xmin" @b [double] minimum x position in the map [m]
|
||||
- @b "~/ymin" @b [double] minimum y position in the map [m]
|
||||
- @b "~/xmax" @b [double] maximum x position in the map [m]
|
||||
- @b "~/ymax" @b [double] maximum y position in the map [m]
|
||||
- @b "~/delta" @b [double] size of one pixel [m]
|
||||
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "slam_gmapping.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <time.h>
|
||||
|
||||
#include "ros/ros.h"
|
||||
#include "ros/console.h"
|
||||
#include "nav_msgs/MapMetaData.h"
|
||||
|
||||
#include "gmapping/sensor/sensor_range/rangesensor.h"
|
||||
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
|
||||
|
||||
#include <rosbag/bag.h>
|
||||
#include <rosbag/view.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#define foreach BOOST_FOREACH
|
||||
|
||||
// compute linear index for given map coords
|
||||
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
|
||||
|
||||
SlamGMapping::SlamGMapping():
|
||||
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
|
||||
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
|
||||
{
|
||||
seed_ = time(NULL);
|
||||
init();
|
||||
}
|
||||
|
||||
SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
|
||||
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
|
||||
laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
|
||||
{
|
||||
seed_ = time(NULL);
|
||||
init();
|
||||
}
|
||||
|
||||
SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
|
||||
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
|
||||
laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
|
||||
seed_(seed), tf_(ros::Duration(max_duration_buffer))
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
|
||||
void SlamGMapping::init()
|
||||
{
|
||||
// log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
|
||||
|
||||
// The library is pretty chatty
|
||||
//gsp_ = new GMapping::GridSlamProcessor(std::cerr);
|
||||
gsp_ = new GMapping::GridSlamProcessor();
|
||||
ROS_ASSERT(gsp_);
|
||||
|
||||
tfB_ = new tf::TransformBroadcaster();
|
||||
ROS_ASSERT(tfB_);
|
||||
|
||||
gsp_laser_ = NULL;
|
||||
gsp_odom_ = NULL;
|
||||
|
||||
got_first_scan_ = false;
|
||||
got_map_ = false;
|
||||
|
||||
|
||||
|
||||
// Parameters used by our GMapping wrapper
|
||||
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
|
||||
throttle_scans_ = 1;
|
||||
if(!private_nh_.getParam("base_frame", base_frame_))
|
||||
base_frame_ = "base_link";
|
||||
if(!private_nh_.getParam("map_frame", map_frame_))
|
||||
map_frame_ = "map";
|
||||
if(!private_nh_.getParam("odom_frame", odom_frame_))
|
||||
odom_frame_ = "odom";
|
||||
|
||||
private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
|
||||
|
||||
double tmp;
|
||||
if(!private_nh_.getParam("map_update_interval", tmp))
|
||||
tmp = 5.0;
|
||||
map_update_interval_.fromSec(tmp);
|
||||
|
||||
// Parameters used by GMapping itself
|
||||
maxUrange_ = 0.0; maxRange_ = 0.0; // preliminary default, will be set in initMapper()
|
||||
if(!private_nh_.getParam("minimumScore", minimum_score_))
|
||||
minimum_score_ = 0;
|
||||
if(!private_nh_.getParam("sigma", sigma_))
|
||||
sigma_ = 0.05;
|
||||
if(!private_nh_.getParam("kernelSize", kernelSize_))
|
||||
kernelSize_ = 1;
|
||||
if(!private_nh_.getParam("lstep", lstep_))
|
||||
lstep_ = 0.05;
|
||||
if(!private_nh_.getParam("astep", astep_))
|
||||
astep_ = 0.05;
|
||||
if(!private_nh_.getParam("iterations", iterations_))
|
||||
iterations_ = 5;
|
||||
if(!private_nh_.getParam("lsigma", lsigma_))
|
||||
lsigma_ = 0.075;
|
||||
if(!private_nh_.getParam("ogain", ogain_))
|
||||
ogain_ = 3.0;
|
||||
if(!private_nh_.getParam("lskip", lskip_))
|
||||
lskip_ = 0;
|
||||
if(!private_nh_.getParam("srr", srr_))
|
||||
srr_ = 0.1;
|
||||
if(!private_nh_.getParam("srt", srt_))
|
||||
srt_ = 0.2;
|
||||
if(!private_nh_.getParam("str", str_))
|
||||
str_ = 0.1;
|
||||
if(!private_nh_.getParam("stt", stt_))
|
||||
stt_ = 0.2;
|
||||
if(!private_nh_.getParam("linearUpdate", linearUpdate_))
|
||||
linearUpdate_ = 1.0;
|
||||
if(!private_nh_.getParam("angularUpdate", angularUpdate_))
|
||||
angularUpdate_ = 0.5;
|
||||
if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
|
||||
temporalUpdate_ = -1.0;
|
||||
if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
|
||||
resampleThreshold_ = 0.5;
|
||||
if(!private_nh_.getParam("particles", particles_))
|
||||
particles_ = 30;
|
||||
if(!private_nh_.getParam("xmin", xmin_))
|
||||
xmin_ = -100.0;
|
||||
if(!private_nh_.getParam("ymin", ymin_))
|
||||
ymin_ = -100.0;
|
||||
if(!private_nh_.getParam("xmax", xmax_))
|
||||
xmax_ = 100.0;
|
||||
if(!private_nh_.getParam("ymax", ymax_))
|
||||
ymax_ = 100.0;
|
||||
if(!private_nh_.getParam("delta", delta_))
|
||||
delta_ = 0.05;
|
||||
if(!private_nh_.getParam("occ_thresh", occ_thresh_))
|
||||
occ_thresh_ = 0.25;
|
||||
if(!private_nh_.getParam("llsamplerange", llsamplerange_))
|
||||
llsamplerange_ = 0.01;
|
||||
if(!private_nh_.getParam("llsamplestep", llsamplestep_))
|
||||
llsamplestep_ = 0.01;
|
||||
if(!private_nh_.getParam("lasamplerange", lasamplerange_))
|
||||
lasamplerange_ = 0.005;
|
||||
if(!private_nh_.getParam("lasamplestep", lasamplestep_))
|
||||
lasamplestep_ = 0.005;
|
||||
|
||||
if(!private_nh_.getParam("tf_delay", tf_delay_))
|
||||
tf_delay_ = transform_publish_period_;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void SlamGMapping::startLiveSlam()
|
||||
{
|
||||
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
|
||||
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
|
||||
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
|
||||
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
|
||||
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
|
||||
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
|
||||
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
|
||||
|
||||
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
|
||||
}
|
||||
|
||||
void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
|
||||
{
|
||||
double transform_publish_period;
|
||||
ros::NodeHandle private_nh_("~");
|
||||
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
|
||||
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
|
||||
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
|
||||
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
|
||||
|
||||
rosbag::Bag bag;
|
||||
bag.open(bag_fname, rosbag::bagmode::Read);
|
||||
|
||||
std::vector<std::string> topics;
|
||||
topics.push_back(std::string("/tf"));
|
||||
topics.push_back(scan_topic);
|
||||
rosbag::View viewall(bag, rosbag::TopicQuery(topics));
|
||||
|
||||
// Store up to 5 messages and there error message (if they cannot be processed right away)
|
||||
std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
|
||||
foreach(rosbag::MessageInstance const m, viewall)
|
||||
{
|
||||
tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
|
||||
if (cur_tf != NULL) {
|
||||
for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
|
||||
{
|
||||
geometry_msgs::TransformStamped transformStamped;
|
||||
tf::StampedTransform stampedTf;
|
||||
transformStamped = cur_tf->transforms[i];
|
||||
tf::transformStampedMsgToTF(transformStamped, stampedTf);
|
||||
tf_.setTransform(stampedTf);
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
|
||||
if (s != NULL) {
|
||||
if (!(ros::Time(s->header.stamp)).is_zero())
|
||||
{
|
||||
s_queue.push(std::make_pair(s, ""));
|
||||
}
|
||||
// Just like in live processing, only process the latest 5 scans
|
||||
if (s_queue.size() > 5) {
|
||||
ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
|
||||
s_queue.pop();
|
||||
}
|
||||
// ignoring un-timestamped tf data
|
||||
}
|
||||
|
||||
// Only process a scan if it has tf data
|
||||
while (!s_queue.empty())
|
||||
{
|
||||
try
|
||||
{
|
||||
tf::StampedTransform t;
|
||||
tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
|
||||
this->laserCallback(s_queue.front().first);
|
||||
s_queue.pop();
|
||||
}
|
||||
// If tf does not have the data yet
|
||||
catch(tf2::TransformException& e)
|
||||
{
|
||||
// Store the error to display it if we cannot process the data after some time
|
||||
s_queue.front().second = std::string(e.what());
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bag.close();
|
||||
}
|
||||
|
||||
void SlamGMapping::publishLoop(double transform_publish_period){
|
||||
if(transform_publish_period == 0)
|
||||
return;
|
||||
|
||||
ros::Rate r(1.0 / transform_publish_period);
|
||||
while(ros::ok()){
|
||||
publishTransform();
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
SlamGMapping::~SlamGMapping()
|
||||
{
|
||||
if(transform_thread_){
|
||||
transform_thread_->join();
|
||||
delete transform_thread_;
|
||||
}
|
||||
|
||||
delete gsp_;
|
||||
if(gsp_laser_)
|
||||
delete gsp_laser_;
|
||||
if(gsp_odom_)
|
||||
delete gsp_odom_;
|
||||
if (scan_filter_)
|
||||
delete scan_filter_;
|
||||
if (scan_filter_sub_)
|
||||
delete scan_filter_sub_;
|
||||
}
|
||||
|
||||
bool
|
||||
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
|
||||
{
|
||||
// Get the pose of the centered laser at the right time
|
||||
centered_laser_pose_.stamp_ = t;
|
||||
// Get the laser's pose that is centered
|
||||
tf::Stamped<tf::Transform> odom_pose;
|
||||
try
|
||||
{
|
||||
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
|
||||
}
|
||||
catch(tf::TransformException e)
|
||||
{
|
||||
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
|
||||
return false;
|
||||
}
|
||||
double yaw = tf::getYaw(odom_pose.getRotation());
|
||||
|
||||
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
|
||||
odom_pose.getOrigin().y(),
|
||||
yaw);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
|
||||
{
|
||||
laser_frame_ = scan.header.frame_id;
|
||||
// Get the laser's pose, relative to base.
|
||||
tf::Stamped<tf::Pose> ident;
|
||||
tf::Stamped<tf::Transform> laser_pose;
|
||||
ident.setIdentity();
|
||||
ident.frame_id_ = laser_frame_;
|
||||
ident.stamp_ = scan.header.stamp;
|
||||
try
|
||||
{
|
||||
tf_.transformPose(base_frame_, ident, laser_pose);
|
||||
}
|
||||
catch(tf::TransformException e)
|
||||
{
|
||||
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
|
||||
e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
// create a point 1m above the laser position and transform it into the laser-frame
|
||||
tf::Vector3 v;
|
||||
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
|
||||
tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
|
||||
base_frame_);
|
||||
try
|
||||
{
|
||||
tf_.transformPoint(laser_frame_, up, up);
|
||||
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
|
||||
}
|
||||
catch(tf::TransformException& e)
|
||||
{
|
||||
ROS_WARN("Unable to determine orientation of laser: %s",
|
||||
e.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
|
||||
if (fabs(fabs(up.z()) - 1) > 0.001)
|
||||
{
|
||||
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
|
||||
up.z());
|
||||
return false;
|
||||
}
|
||||
|
||||
gsp_laser_beam_count_ = scan.ranges.size();
|
||||
|
||||
double angle_center = (scan.angle_min + scan.angle_max)/2;
|
||||
|
||||
if (up.z() > 0)
|
||||
{
|
||||
do_reverse_range_ = scan.angle_min > scan.angle_max;
|
||||
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
|
||||
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
|
||||
ROS_INFO("Laser is mounted upwards.");
|
||||
}
|
||||
else
|
||||
{
|
||||
do_reverse_range_ = scan.angle_min < scan.angle_max;
|
||||
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
|
||||
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
|
||||
ROS_INFO("Laser is mounted upside down.");
|
||||
}
|
||||
|
||||
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
|
||||
laser_angles_.resize(scan.ranges.size());
|
||||
// Make sure angles are started so that they are centered
|
||||
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
|
||||
for(unsigned int i=0; i<scan.ranges.size(); ++i)
|
||||
{
|
||||
laser_angles_[i]=theta;
|
||||
theta += std::fabs(scan.angle_increment);
|
||||
}
|
||||
|
||||
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
|
||||
scan.angle_increment);
|
||||
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
|
||||
laser_angles_.back(), std::fabs(scan.angle_increment));
|
||||
|
||||
GMapping::OrientedPoint gmap_pose(0, 0, 0);
|
||||
|
||||
// setting maxRange and maxUrange here so we can set a reasonable default
|
||||
ros::NodeHandle private_nh_("~");
|
||||
if(!private_nh_.getParam("maxRange", maxRange_))
|
||||
maxRange_ = scan.range_max - 0.01;
|
||||
if(!private_nh_.getParam("maxUrange", maxUrange_))
|
||||
maxUrange_ = maxRange_;
|
||||
|
||||
// The laser must be called "FLASER".
|
||||
// We pass in the absolute value of the computed angle increment, on the
|
||||
// assumption that GMapping requires a positive angle increment. If the
|
||||
// actual increment is negative, we'll swap the order of ranges before
|
||||
// feeding each scan to GMapping.
|
||||
gsp_laser_ = new GMapping::RangeSensor("FLASER",
|
||||
gsp_laser_beam_count_,
|
||||
fabs(scan.angle_increment),
|
||||
gmap_pose,
|
||||
0.0,
|
||||
maxRange_);
|
||||
ROS_ASSERT(gsp_laser_);
|
||||
|
||||
GMapping::SensorMap smap;
|
||||
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
|
||||
gsp_->setSensorMap(smap);
|
||||
|
||||
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
|
||||
ROS_ASSERT(gsp_odom_);
|
||||
|
||||
|
||||
/// @todo Expose setting an initial pose
|
||||
GMapping::OrientedPoint initialPose;
|
||||
if(!getOdomPose(initialPose, scan.header.stamp))
|
||||
{
|
||||
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
|
||||
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
|
||||
kernelSize_, lstep_, astep_, iterations_,
|
||||
lsigma_, ogain_, lskip_);
|
||||
|
||||
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
|
||||
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
|
||||
gsp_->setUpdatePeriod(temporalUpdate_);
|
||||
gsp_->setgenerateMap(false);
|
||||
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
|
||||
delta_, initialPose);
|
||||
gsp_->setllsamplerange(llsamplerange_);
|
||||
gsp_->setllsamplestep(llsamplestep_);
|
||||
/// @todo Check these calls; in the gmapping gui, they use
|
||||
/// llsamplestep and llsamplerange intead of lasamplestep and
|
||||
/// lasamplerange. It was probably a typo, but who knows.
|
||||
gsp_->setlasamplerange(lasamplerange_);
|
||||
gsp_->setlasamplestep(lasamplestep_);
|
||||
gsp_->setminimumScore(minimum_score_);
|
||||
|
||||
// Call the sampling function once to set the seed.
|
||||
GMapping::sampleGaussian(1,seed_);
|
||||
|
||||
ROS_INFO("Initialization complete");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
|
||||
{
|
||||
if(!getOdomPose(gmap_pose, scan.header.stamp))
|
||||
return false;
|
||||
|
||||
if(scan.ranges.size() != gsp_laser_beam_count_)
|
||||
return false;
|
||||
|
||||
// GMapping wants an array of doubles...
|
||||
double* ranges_double = new double[scan.ranges.size()];
|
||||
// If the angle increment is negative, we have to invert the order of the readings.
|
||||
if (do_reverse_range_)
|
||||
{
|
||||
ROS_DEBUG("Inverting scan");
|
||||
int num_ranges = scan.ranges.size();
|
||||
for(int i=0; i < num_ranges; i++)
|
||||
{
|
||||
// Must filter out short readings, because the mapper won't
|
||||
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
|
||||
ranges_double[i] = (double)scan.range_max;
|
||||
else
|
||||
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
|
||||
}
|
||||
} else
|
||||
{
|
||||
for(unsigned int i=0; i < scan.ranges.size(); i++)
|
||||
{
|
||||
// Must filter out short readings, because the mapper won't
|
||||
if(scan.ranges[i] < scan.range_min)
|
||||
ranges_double[i] = (double)scan.range_max;
|
||||
else
|
||||
ranges_double[i] = (double)scan.ranges[i];
|
||||
}
|
||||
}
|
||||
|
||||
GMapping::RangeReading reading(scan.ranges.size(),
|
||||
ranges_double,
|
||||
gsp_laser_,
|
||||
scan.header.stamp.toSec());
|
||||
|
||||
// ...but it deep copies them in RangeReading constructor, so we don't
|
||||
// need to keep our array around.
|
||||
delete[] ranges_double;
|
||||
|
||||
reading.setPose(gmap_pose);
|
||||
|
||||
/*
|
||||
ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
|
||||
scan.header.stamp.toSec(),
|
||||
gmap_pose.x,
|
||||
gmap_pose.y,
|
||||
gmap_pose.theta);
|
||||
*/
|
||||
ROS_DEBUG("processing scan");
|
||||
|
||||
return gsp_->processScan(reading);
|
||||
}
|
||||
|
||||
void
|
||||
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
|
||||
{
|
||||
laser_count_++;
|
||||
if ((laser_count_ % throttle_scans_) != 0)
|
||||
return;
|
||||
|
||||
static ros::Time last_map_update(0,0);
|
||||
|
||||
// We can't initialize the mapper until we've got the first scan
|
||||
if(!got_first_scan_)
|
||||
{
|
||||
if(!initMapper(*scan))
|
||||
return;
|
||||
got_first_scan_ = true;
|
||||
}
|
||||
|
||||
GMapping::OrientedPoint odom_pose;
|
||||
|
||||
if(addScan(*scan, odom_pose))
|
||||
{
|
||||
ROS_DEBUG("scan processed");
|
||||
|
||||
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
|
||||
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
|
||||
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
|
||||
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
|
||||
|
||||
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
|
||||
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
|
||||
|
||||
map_to_odom_mutex_.lock();
|
||||
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
|
||||
map_to_odom_mutex_.unlock();
|
||||
|
||||
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
|
||||
{
|
||||
updateMap(*scan);
|
||||
last_map_update = scan->header.stamp;
|
||||
ROS_DEBUG("Updated the map");
|
||||
}
|
||||
} else
|
||||
ROS_DEBUG("cannot process scan");
|
||||
}
|
||||
|
||||
double
|
||||
SlamGMapping::computePoseEntropy()
|
||||
{
|
||||
double weight_total=0.0;
|
||||
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
|
||||
it != gsp_->getParticles().end();
|
||||
++it)
|
||||
{
|
||||
weight_total += it->weight;
|
||||
}
|
||||
double entropy = 0.0;
|
||||
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
|
||||
it != gsp_->getParticles().end();
|
||||
++it)
|
||||
{
|
||||
if(it->weight/weight_total > 0.0)
|
||||
entropy += it->weight/weight_total * log(it->weight/weight_total);
|
||||
}
|
||||
return -entropy;
|
||||
}
|
||||
|
||||
void
|
||||
SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
|
||||
{
|
||||
ROS_DEBUG("Update map");
|
||||
boost::mutex::scoped_lock map_lock (map_mutex_);
|
||||
GMapping::ScanMatcher matcher;
|
||||
|
||||
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
|
||||
gsp_laser_->getPose());
|
||||
|
||||
matcher.setlaserMaxRange(maxRange_);
|
||||
matcher.setusableRange(maxUrange_);
|
||||
matcher.setgenerateMap(true);
|
||||
|
||||
GMapping::GridSlamProcessor::Particle best =
|
||||
gsp_->getParticles()[gsp_->getBestParticleIndex()];
|
||||
std_msgs::Float64 entropy;
|
||||
entropy.data = computePoseEntropy();
|
||||
if(entropy.data > 0.0)
|
||||
entropy_publisher_.publish(entropy);
|
||||
|
||||
if(!got_map_) {
|
||||
map_.map.info.resolution = delta_;
|
||||
map_.map.info.origin.position.x = 0.0;
|
||||
map_.map.info.origin.position.y = 0.0;
|
||||
map_.map.info.origin.position.z = 0.0;
|
||||
map_.map.info.origin.orientation.x = 0.0;
|
||||
map_.map.info.origin.orientation.y = 0.0;
|
||||
map_.map.info.origin.orientation.z = 0.0;
|
||||
map_.map.info.origin.orientation.w = 1.0;
|
||||
}
|
||||
|
||||
GMapping::Point center;
|
||||
center.x=(xmin_ + xmax_) / 2.0;
|
||||
center.y=(ymin_ + ymax_) / 2.0;
|
||||
|
||||
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
|
||||
delta_);
|
||||
|
||||
ROS_DEBUG("Trajectory tree:");
|
||||
for(GMapping::GridSlamProcessor::TNode* n = best.node;
|
||||
n;
|
||||
n = n->parent)
|
||||
{
|
||||
ROS_DEBUG(" %.3f %.3f %.3f",
|
||||
n->pose.x,
|
||||
n->pose.y,
|
||||
n->pose.theta);
|
||||
if(!n->reading)
|
||||
{
|
||||
ROS_DEBUG("Reading is NULL");
|
||||
continue;
|
||||
}
|
||||
matcher.invalidateActiveArea();
|
||||
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
|
||||
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
|
||||
}
|
||||
|
||||
// the map may have expanded, so resize ros message as well
|
||||
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
|
||||
|
||||
// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
|
||||
// so we must obtain the bounding box in a different way
|
||||
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
|
||||
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
|
||||
xmin_ = wmin.x; ymin_ = wmin.y;
|
||||
xmax_ = wmax.x; ymax_ = wmax.y;
|
||||
|
||||
ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
|
||||
xmin_, ymin_, xmax_, ymax_);
|
||||
|
||||
map_.map.info.width = smap.getMapSizeX();
|
||||
map_.map.info.height = smap.getMapSizeY();
|
||||
map_.map.info.origin.position.x = xmin_;
|
||||
map_.map.info.origin.position.y = ymin_;
|
||||
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
|
||||
|
||||
ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
|
||||
}
|
||||
|
||||
for(int x=0; x < smap.getMapSizeX(); x++)
|
||||
{
|
||||
for(int y=0; y < smap.getMapSizeY(); y++)
|
||||
{
|
||||
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
|
||||
GMapping::IntPoint p(x, y);
|
||||
double occ=smap.cell(p);
|
||||
assert(occ <= 1.0);
|
||||
if(occ < 0)
|
||||
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
|
||||
else if(occ > occ_thresh_)
|
||||
{
|
||||
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
|
||||
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
|
||||
}
|
||||
else
|
||||
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
|
||||
}
|
||||
}
|
||||
got_map_ = true;
|
||||
|
||||
//make sure to set the header information on the map
|
||||
map_.map.header.stamp = ros::Time::now();
|
||||
map_.map.header.frame_id = tf_.resolve( map_frame_ );
|
||||
|
||||
sst_.publish(map_.map);
|
||||
sstm_.publish(map_.map.info);
|
||||
}
|
||||
|
||||
bool
|
||||
SlamGMapping::mapCallback(nav_msgs::GetMap::Request &req,
|
||||
nav_msgs::GetMap::Response &res)
|
||||
{
|
||||
boost::mutex::scoped_lock map_lock (map_mutex_);
|
||||
if(got_map_ && map_.map.info.width && map_.map.info.height)
|
||||
{
|
||||
res = map_;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
void SlamGMapping::publishTransform()
|
||||
{
|
||||
map_to_odom_mutex_.lock();
|
||||
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
|
||||
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
|
||||
map_to_odom_mutex_.unlock();
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
* slam_gmapping
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
*
|
||||
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
|
||||
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
|
||||
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
|
||||
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
|
||||
*
|
||||
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
|
||||
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
|
||||
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
|
||||
* CONDITIONS.
|
||||
*
|
||||
*/
|
||||
|
||||
/* Author: Brian Gerkey */
|
||||
|
||||
#include "ros/ros.h"
|
||||
#include "sensor_msgs/LaserScan.h"
|
||||
#include "std_msgs/Float64.h"
|
||||
#include "nav_msgs/GetMap.h"
|
||||
#include "tf/transform_listener.h"
|
||||
#include "tf/transform_broadcaster.h"
|
||||
#include "message_filters/subscriber.h"
|
||||
#include "tf/message_filter.h"
|
||||
|
||||
#include "gmapping/gridfastslam/gridslamprocessor.h"
|
||||
#include "gmapping/sensor/sensor_base/sensor.h"
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
class SlamGMapping
|
||||
{
|
||||
public:
|
||||
SlamGMapping();
|
||||
SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);
|
||||
~SlamGMapping();
|
||||
|
||||
void init();
|
||||
void startLiveSlam();
|
||||
void startReplay(const std::string & bag_fname, std::string scan_topic);
|
||||
void publishTransform();
|
||||
|
||||
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
|
||||
bool mapCallback(nav_msgs::GetMap::Request &req,
|
||||
nav_msgs::GetMap::Response &res);
|
||||
void publishLoop(double transform_publish_period);
|
||||
|
||||
private:
|
||||
ros::NodeHandle node_;
|
||||
ros::Publisher entropy_publisher_;
|
||||
ros::Publisher sst_;
|
||||
ros::Publisher sstm_;
|
||||
ros::ServiceServer ss_;
|
||||
tf::TransformListener tf_;
|
||||
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
|
||||
tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
|
||||
tf::TransformBroadcaster* tfB_;
|
||||
|
||||
GMapping::GridSlamProcessor* gsp_;
|
||||
GMapping::RangeSensor* gsp_laser_;
|
||||
// The angles in the laser, going from -x to x (adjustment is made to get the laser between
|
||||
// symmetrical bounds as that's what gmapping expects)
|
||||
std::vector<double> laser_angles_;
|
||||
// The pose, in the original laser frame, of the corresponding centered laser with z facing up
|
||||
tf::Stamped<tf::Pose> centered_laser_pose_;
|
||||
// Depending on the order of the elements in the scan and the orientation of the scan frame,
|
||||
// We might need to change the order of the scan
|
||||
bool do_reverse_range_;
|
||||
unsigned int gsp_laser_beam_count_;
|
||||
GMapping::OdometrySensor* gsp_odom_;
|
||||
|
||||
bool got_first_scan_;
|
||||
|
||||
bool got_map_;
|
||||
nav_msgs::GetMap::Response map_;
|
||||
|
||||
ros::Duration map_update_interval_;
|
||||
tf::Transform map_to_odom_;
|
||||
boost::mutex map_to_odom_mutex_;
|
||||
boost::mutex map_mutex_;
|
||||
|
||||
int laser_count_;
|
||||
int throttle_scans_;
|
||||
|
||||
boost::thread* transform_thread_;
|
||||
|
||||
std::string base_frame_;
|
||||
std::string laser_frame_;
|
||||
std::string map_frame_;
|
||||
std::string odom_frame_;
|
||||
|
||||
void updateMap(const sensor_msgs::LaserScan& scan);
|
||||
bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
|
||||
bool initMapper(const sensor_msgs::LaserScan& scan);
|
||||
bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose);
|
||||
double computePoseEntropy();
|
||||
|
||||
// Parameters used by GMapping
|
||||
double maxRange_;
|
||||
double maxUrange_;
|
||||
double maxrange_;
|
||||
double minimum_score_;
|
||||
double sigma_;
|
||||
int kernelSize_;
|
||||
double lstep_;
|
||||
double astep_;
|
||||
int iterations_;
|
||||
double lsigma_;
|
||||
double ogain_;
|
||||
int lskip_;
|
||||
double srr_;
|
||||
double srt_;
|
||||
double str_;
|
||||
double stt_;
|
||||
double linearUpdate_;
|
||||
double angularUpdate_;
|
||||
double temporalUpdate_;
|
||||
double resampleThreshold_;
|
||||
int particles_;
|
||||
double xmin_;
|
||||
double ymin_;
|
||||
double xmax_;
|
||||
double ymax_;
|
||||
double delta_;
|
||||
double occ_thresh_;
|
||||
double llsamplerange_;
|
||||
double llsamplestep_;
|
||||
double lasamplerange_;
|
||||
double lasamplestep_;
|
||||
|
||||
ros::NodeHandle private_nh_;
|
||||
|
||||
unsigned long int seed_;
|
||||
|
||||
double transform_publish_period_;
|
||||
double tf_delay_;
|
||||
};
|
||||
@@ -0,0 +1,11 @@
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node pkg="rosbag" type="play" name="rosplay"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find gmapping)/test/hallway_slow_2011-03-04-21-41-33.bag"/>
|
||||
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
|
||||
<remap from="scan" to="base_laser1_scan"/>
|
||||
</node>
|
||||
<!--test time-limit="180" test-name="basic_localization_stage" pkg="gmapping"
|
||||
type="test_map.py" args="90.0"/-->
|
||||
<test time-limit="180" test-name="map_data_different_beamcount_test" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.002 0.005"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,11 @@
|
||||
<launch>
|
||||
<param name="/use_sim_time" value="true"/>
|
||||
<node pkg="rosbag" type="play" name="rosplay"
|
||||
args="-d 5 -r 1 --clock --hz 10 $(find gmapping)/test/basic_localization_stage_indexed.bag"/>
|
||||
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
|
||||
<remap from="scan" to="base_scan"/>
|
||||
</node>
|
||||
<!--test time-limit="180" test-name="basic_localization_stage" pkg="gmapping"
|
||||
type="test_map.py" args="90.0"/-->
|
||||
<test time-limit="180" test-name="map_data_test" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.005 0.010"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,8 @@
|
||||
<launch>
|
||||
<node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
|
||||
args="--bag_filename $(find gmapping)/test/basic_localization_stage_indexed.bag --scan_topic /base_scan"/>
|
||||
<!--test time-limit="30" test-name="basic_localization_stage" pkg="gmapping"
|
||||
type="test_map.py" args="90.0"/-->
|
||||
<!-- TODO: Fix the rtest to work with replay, for now it's not working -->
|
||||
<test time-limit="250" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
|
||||
args="--bag_filename $(find gmapping)/test/test_replay_crash.bag --scan_topic /scan"/>
|
||||
<test time-limit="20" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="10.0 0.05 4000 4000 0.0001 0.010"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
|
||||
args="--bag_filename $(find gmapping)/test/test_turtlebot.bag --scan_topic /scan"/>
|
||||
<test time-limit="120" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.0005 0.010"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
|
||||
args="--bag_filename $(find gmapping)/test/test_upside_down.bag --scan_topic /laserscan/fix/raw _base_frame:=base_floor"/>
|
||||
<test time-limit="190" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
|
||||
</launch>
|
||||
@@ -0,0 +1,180 @@
|
||||
/*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
/* Author: Brian Gerkey */
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <ros/service.h>
|
||||
#include <ros/ros.h>
|
||||
#include <nav_msgs/GetMap.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <nav_msgs/MapMetaData.h>
|
||||
|
||||
|
||||
ros::NodeHandle* g_n=NULL;
|
||||
double g_res, g_width, g_height, g_min_free_ratio, g_max_free_ratio;
|
||||
|
||||
class MapClientTest : public testing::Test
|
||||
{
|
||||
public:
|
||||
MapClientTest()
|
||||
{
|
||||
got_map_ = false;
|
||||
got_map_metadata_ = false;
|
||||
}
|
||||
|
||||
~MapClientTest()
|
||||
{
|
||||
}
|
||||
|
||||
bool got_map_;
|
||||
boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
|
||||
void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
|
||||
{
|
||||
map_ = map;
|
||||
got_map_ = true;
|
||||
}
|
||||
|
||||
bool got_map_metadata_;
|
||||
boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
|
||||
void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
|
||||
{
|
||||
map_metadata_ = map_metadata;
|
||||
got_map_metadata_ = true;
|
||||
}
|
||||
|
||||
void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata)
|
||||
{
|
||||
EXPECT_FLOAT_EQ(map_metadata.resolution, g_res);
|
||||
EXPECT_FLOAT_EQ(map_metadata.width, g_width);
|
||||
EXPECT_FLOAT_EQ(map_metadata.height, g_height);
|
||||
}
|
||||
|
||||
void checkMapData(const nav_msgs::OccupancyGrid& map)
|
||||
{
|
||||
unsigned int i;
|
||||
unsigned int free_cnt = 0;
|
||||
for(i=0; i < map.info.width * map.info.height; i++)
|
||||
{
|
||||
if(map.data[i] == 0)
|
||||
free_cnt++;
|
||||
}
|
||||
double free_ratio = free_cnt / (double)(i);
|
||||
ROS_INFO("Min / ratio / Max free ratio: %f / %f / %f", g_min_free_ratio, free_ratio, g_max_free_ratio);
|
||||
EXPECT_GE(free_ratio, g_min_free_ratio);
|
||||
EXPECT_LE(free_ratio, g_max_free_ratio);
|
||||
}
|
||||
};
|
||||
|
||||
/* Try to retrieve the map via service, and compare to ground truth */
|
||||
TEST_F(MapClientTest, call_service)
|
||||
{
|
||||
nav_msgs::GetMap::Request req;
|
||||
nav_msgs::GetMap::Response resp;
|
||||
ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000));
|
||||
ASSERT_TRUE(ros::service::call("dynamic_map", req, resp));
|
||||
checkMapMetaData(resp.map.info);
|
||||
checkMapData(resp.map);
|
||||
}
|
||||
|
||||
/* Try to retrieve the map via topic, and compare to ground truth */
|
||||
TEST_F(MapClientTest, subscribe_topic)
|
||||
{
|
||||
ros::Subscriber sub = g_n->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
|
||||
|
||||
// Try a few times, because the server may not be up yet.
|
||||
int i=20;
|
||||
while(!got_map_ && i > 0)
|
||||
{
|
||||
ros::spinOnce();
|
||||
ros::WallDuration d(0.25);
|
||||
d.sleep();
|
||||
i--;
|
||||
}
|
||||
ASSERT_TRUE(got_map_);
|
||||
checkMapMetaData(map_->info);
|
||||
checkMapData(*(map_.get()));
|
||||
}
|
||||
|
||||
/* Try to retrieve the metadata via topic, and compare to ground truth */
|
||||
TEST_F(MapClientTest, subscribe_topic_metadata)
|
||||
{
|
||||
ros::Subscriber sub = g_n->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
|
||||
|
||||
// Try a few times, because the server may not be up yet.
|
||||
int i=20;
|
||||
while(!got_map_metadata_ && i > 0)
|
||||
{
|
||||
ros::spinOnce();
|
||||
ros::WallDuration d(0.25);
|
||||
d.sleep();
|
||||
i--;
|
||||
}
|
||||
ASSERT_TRUE(got_map_metadata_);
|
||||
checkMapMetaData(*(map_metadata_.get()));
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
ros::init(argc, argv, "map_client_test");
|
||||
g_n = new ros::NodeHandle();
|
||||
|
||||
// Required args are, in order:
|
||||
// <delay> <resolution> <width> <height> <min_free_ratio> <max_free_ratio>
|
||||
ROS_ASSERT(argc == 7);
|
||||
ros::Duration delay = ros::Duration(atof(argv[1]));
|
||||
g_res = atof(argv[2]);
|
||||
g_width = atof(argv[3]);
|
||||
g_height = atof(argv[4]);
|
||||
g_min_free_ratio = atof(argv[5]);
|
||||
g_max_free_ratio = atof(argv[6]);
|
||||
|
||||
while(ros::Time::now().toSec() == 0.0)
|
||||
{
|
||||
ROS_INFO("Waiting for initial time publication");
|
||||
ros::Duration(0.25).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
ros::Time start_time = ros::Time::now();
|
||||
while((ros::Time::now() - start_time) < delay)
|
||||
{
|
||||
ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f",
|
||||
ros::Time::now().toSec(),
|
||||
start_time.toSec(),
|
||||
delay.toSec());
|
||||
ros::Duration(0.25).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
int result = RUN_ALL_TESTS();
|
||||
delete g_n;
|
||||
return result;
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
#!/usr/bin/env python
|
||||
# Software License Agreement (BSD License)
|
||||
#
|
||||
# Copyright (c) 2008, Willow Garage, Inc.
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above
|
||||
# copyright notice, this list of conditions and the following
|
||||
# disclaimer in the documentation and/or other materials provided
|
||||
# with the distribution.
|
||||
# * Neither the name of the Willow Garage nor the names of its
|
||||
# contributors may be used to endorse or promote products derived
|
||||
# from this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
|
||||
|
||||
import PIL.Image
|
||||
import unittest
|
||||
import subprocess
|
||||
import sys
|
||||
|
||||
import roslib
|
||||
import os
|
||||
roslib.load_manifest('gmapping')
|
||||
import rostest
|
||||
|
||||
class TestGmapping(unittest.TestCase):
|
||||
|
||||
# Test that 2 map files are approximately the same
|
||||
def cmp_maps(self, f0, f1):
|
||||
im0 = PIL.Image.open(f0+'.pgm')
|
||||
im1 = PIL.Image.open(f1+'.pgm')
|
||||
|
||||
size = 100,100
|
||||
im0.thumbnail(size,PIL.Image.ANTIALIAS)
|
||||
im1.thumbnail(size,PIL.Image.ANTIALIAS)
|
||||
|
||||
# get raw data for comparison
|
||||
im0d = im0.getdata()
|
||||
im1d = im1.getdata()
|
||||
|
||||
# assert len(i0)==len(i1)
|
||||
self.assertTrue(len(im0d) == len(im1d))
|
||||
|
||||
#compare pixel by pixel for thumbnails
|
||||
error_count = 0
|
||||
error_total = 0
|
||||
pixel_tol = 0
|
||||
total_error_tol = 0.1
|
||||
for i in range(len(im0d)):
|
||||
(p0) = im0d[i]
|
||||
(p1) = im1d[i]
|
||||
if abs(p0-p1) > pixel_tol:
|
||||
error_count = error_count + 1
|
||||
error_total = error_total + abs(p0-p1)
|
||||
error_avg = float(error_total)/float(len(im0d))
|
||||
print '%d / %d = %.6f (%.6f)'%(error_total,len(im0d),error_avg,total_error_tol)
|
||||
self.assertTrue(error_avg <= total_error_tol)
|
||||
|
||||
def test_basic_localization_stage(self):
|
||||
if sys.argv > 1:
|
||||
target_time = float(sys.argv[1])
|
||||
|
||||
import time
|
||||
import rospy
|
||||
rospy.init_node('test', anonymous=True)
|
||||
while(rospy.rostime.get_time() == 0.0):
|
||||
print 'Waiting for initial time publication'
|
||||
time.sleep(0.1)
|
||||
start_time = rospy.rostime.get_time()
|
||||
|
||||
while (rospy.rostime.get_time() - start_time) < target_time:
|
||||
print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
|
||||
time.sleep(0.1)
|
||||
|
||||
f0 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_groundtruth')
|
||||
f1 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_generated')
|
||||
|
||||
cmd = ['rosrun', 'map_server', 'map_saver', 'map:=dynamic_map', '-f', f1]
|
||||
self.assertTrue(subprocess.call(cmd) == 0)
|
||||
|
||||
self.cmp_maps(f0,f1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
rostest.run('gmapping', 'gmapping_slam', TestGmapping, sys.argv)
|
||||
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/launch/.DS_Store
vendored
Normal file
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/launch/.DS_Store
vendored
Normal file
Binary file not shown.
@@ -0,0 +1,13 @@
|
||||
<launch>
|
||||
<node pkg="stage_ros" type="stageros" name="simulator" args="$(find rob456_hw3)/worlds/manyDotsNoisyOdom.world"/>
|
||||
<node pkg="rob456_hw3" type="hw3.py" name="lab3" output="screen" />
|
||||
|
||||
<!-- Insert a call to the slam_gmapping node -->
|
||||
<node pkg="gmapping" type="slam_gmapping" name="slam">
|
||||
<remap from="scan" to="base_scan"/>
|
||||
</node>
|
||||
|
||||
<!-- Insert a call to rviz (optional) -->
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rob456_hw3)/rvizsetup.rviz"/>
|
||||
<!-- args="-d $(find rob456_hw3)/rvizsetup.rviz" -->
|
||||
</launch>
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,60 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>rob456_hw3</name>
|
||||
<version>0.0.0</version>
|
||||
<description>ROB456 HW3</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="rob456@todo.todo">rob456</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>BSD</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/rob456_hw3</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
<!-- <run_depend>message_runtime</run_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>gmapping</build_depend>
|
||||
<build_depend>nav_msgs</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>stage_ros</build_depend>
|
||||
<run_depend>gmapping</run_depend>
|
||||
<run_depend>nav_msgs</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>stage_ros</run_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,186 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 775
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: LaserScan
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 1
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 1
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.00999999978
|
||||
Style: Flat Squares
|
||||
Topic: /base_scan
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.699999988
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Unreliable: false
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
- Angle Tolerance: 0.100000001
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.300000012
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Keep: 100
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.100000001
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.100000001
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.300000012
|
||||
Head Radius: 0.100000001
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.0500000007
|
||||
Value: Arrow
|
||||
Topic: /base_pose_ground_truth
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 113.321236
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -1.69929993
|
||||
Y: -21.3828716
|
||||
Z: -3.49176645
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.0500000007
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 1.24039745
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.295395941
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000031900000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007490000003efc0100000002fb0000000800540069006d00650100000000000007490000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003150000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1865
|
||||
X: 55
|
||||
Y: 24
|
||||
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/src/.DS_Store
vendored
Normal file
BIN
OSU Coursework/ROB 456 - Intelligent Robotics/Homework 3 - Part 1 - Occupancy Grids/src/.DS_Store
vendored
Normal file
Binary file not shown.
@@ -0,0 +1,118 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
import math
|
||||
import tf
|
||||
from tf.transformations import euler_from_quaternion
|
||||
import message_filters
|
||||
|
||||
# The laser scan message
|
||||
from sensor_msgs.msg import LaserScan
|
||||
|
||||
# The odometry message
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
# the velocity command message
|
||||
from geometry_msgs.msg import Twist
|
||||
|
||||
# instantiate global variables "globalOdom"
|
||||
globalOdom = Odometry()
|
||||
|
||||
# global pi
|
||||
pi = math.pi
|
||||
|
||||
# method to control the robot
|
||||
def callback(scan,odom):
|
||||
# the odometry parameter should be global
|
||||
global globalOdom
|
||||
globalOdom = odom
|
||||
|
||||
# make a new twist message
|
||||
command = Twist()
|
||||
|
||||
# Fill in the fields. Field values are unspecified
|
||||
# until they are actually assigned. The Twist message
|
||||
# holds linear and angular velocities.
|
||||
command.linear.x = 0.0
|
||||
command.linear.y = 0.0
|
||||
command.linear.z = 0.0
|
||||
command.angular.x = 0.0
|
||||
command.angular.y = 0.0
|
||||
command.angular.z = 0.0
|
||||
|
||||
# find current (x,y) position of robot based on odometry
|
||||
currentX = globalOdom.pose.pose.position.x
|
||||
currentY = globalOdom.pose.pose.position.y
|
||||
|
||||
# find current orientation of robot based on odometry (quaternion coordinates)
|
||||
xOr = globalOdom.pose.pose.orientation.x
|
||||
yOr = globalOdom.pose.pose.orientation.y
|
||||
zOr = globalOdom.pose.pose.orientation.z
|
||||
wOr = globalOdom.pose.pose.orientation.w
|
||||
|
||||
# find orientation of robot (Euler coordinates)
|
||||
(roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])
|
||||
|
||||
# find currentAngle of robot (equivalent to yaw)
|
||||
# now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
|
||||
currentAngle = yaw
|
||||
|
||||
# find laser scanner properties (min scan angle, max scan angle, scan angle increment)
|
||||
maxAngle = scan.angle_max
|
||||
minAngle = scan.angle_min
|
||||
angleIncrement = scan.angle_increment
|
||||
|
||||
# find current laser angle, max scan length, distance array for all scans, and number of laser scans
|
||||
currentLaserTheta = minAngle
|
||||
maxScanLength = scan.range_max
|
||||
distanceArray = scan.ranges
|
||||
numScans = len(distanceArray)
|
||||
|
||||
# the code below (currently commented) shows how
|
||||
# you can print variables to the terminal (may
|
||||
# be useful for debugging)
|
||||
#print 'x: {0}'.format(currentX)
|
||||
#print 'y: {0}'.format(currentY)
|
||||
#print 'theta: {0}'.format(currentAngle)
|
||||
|
||||
# for each laser scan
|
||||
distThreshold = 2.0 # obstacle avoidance threshold
|
||||
minScan = distanceArray[0]
|
||||
velocity = 5.0
|
||||
bearing = 0.0
|
||||
for curScan in range(0, numScans):
|
||||
if currentLaserTheta > -pi/2 and currentLaserTheta < 0 and distanceArray[curScan] < distThreshold:
|
||||
bearing = 1.0 # turn left
|
||||
elif currentLaserTheta >= 0 and currentLaserTheta < pi/2 and distanceArray[curScan] < distThreshold:
|
||||
bearing = -1.0 # turn right
|
||||
if distanceArray[curScan] < minScan:
|
||||
minScan = distanceArray[curScan]
|
||||
currentLaserTheta = currentLaserTheta + angleIncrement
|
||||
|
||||
# set the robot motion
|
||||
# commanded velocities
|
||||
command.linear.x = velocity*min(1.0,minScan/distThreshold) # slow down if within threshold distance
|
||||
command.angular.z = bearing
|
||||
pub.publish(command)
|
||||
|
||||
# main function call
|
||||
if __name__ == "__main__":
|
||||
# Initialize the node
|
||||
rospy.init_node('lab3', log_level=rospy.DEBUG)
|
||||
|
||||
# subscribe to laser scan message
|
||||
sub = message_filters.Subscriber('base_scan', LaserScan)
|
||||
|
||||
# subscribe to odometry message
|
||||
sub2 = message_filters.Subscriber('odom', Odometry)
|
||||
|
||||
# synchronize laser scan and odometry data
|
||||
ts = message_filters.TimeSynchronizer([sub, sub2], 10)
|
||||
ts.registerCallback(callback)
|
||||
|
||||
# publish twist message
|
||||
pub = rospy.Publisher('cmd_vel',Twist)
|
||||
|
||||
# Turn control over to ROS
|
||||
rospy.spin()
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
import math
|
||||
import tf
|
||||
from tf.transformations import euler_from_quaternion
|
||||
import message_filters
|
||||
|
||||
# The odometry message
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
def callback(truth,odom):
|
||||
# Calculate error between truth and odometry estimate
|
||||
x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
|
||||
y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
|
||||
|
||||
xo = odom.pose.pose.orientation.x
|
||||
yo = odom.pose.pose.orientation.y
|
||||
zo = odom.pose.pose.orientation.z
|
||||
wo = odom.pose.pose.orientation.w
|
||||
|
||||
xt = truth.pose.pose.orientation.x
|
||||
yt = truth.pose.pose.orientation.y
|
||||
zt = truth.pose.pose.orientation.z
|
||||
wt = truth.pose.pose.orientation.w
|
||||
|
||||
# find orientation of robot (Euler coordinates)
|
||||
(ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])
|
||||
(rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
|
||||
t_ODOM_error = yt-yo
|
||||
|
||||
print "{0},{1},{2},{3}".format(rospy.get_time(),x_ODOM_error, y_ODOM_error, t_ODOM_error)
|
||||
|
||||
|
||||
# main function call
|
||||
if __name__ == "__main__":
|
||||
# Initialize the node
|
||||
rospy.init_node('odom_error_printer')
|
||||
|
||||
# subscribe to truth pose message
|
||||
sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
|
||||
|
||||
# subscribe to odometry message
|
||||
sub_odom = message_filters.Subscriber('odom', Odometry)
|
||||
|
||||
# synchronize truth pose and odometry data
|
||||
ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
|
||||
ts.registerCallback(callback)
|
||||
|
||||
# Turn control over to ROS
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
import math
|
||||
import tf
|
||||
from tf.transformations import euler_from_quaternion
|
||||
import message_filters
|
||||
|
||||
# The odometry message
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
def callback(truth,odom):
|
||||
# Calculate error between truth and SLAM estimate
|
||||
x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
|
||||
y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
|
||||
|
||||
xo = odom.pose.pose.orientation.x
|
||||
yo = odom.pose.pose.orientation.y
|
||||
zo = odom.pose.pose.orientation.z
|
||||
wo = odom.pose.pose.orientation.w
|
||||
|
||||
xt = truth.pose.pose.orientation.x
|
||||
yt = truth.pose.pose.orientation.y
|
||||
zt = truth.pose.pose.orientation.z
|
||||
wt = truth.pose.pose.orientation.w
|
||||
|
||||
# find orientation of robot (Euler coordinates)
|
||||
(ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])
|
||||
(rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
|
||||
t_ODOM_error = yt-yo
|
||||
|
||||
# query SLAM solution
|
||||
map_listener = tf.TransformListener()
|
||||
try:
|
||||
map_listener.waitForTransform("/map", "/odom", rospy.Time(), rospy.Duration(4.0))
|
||||
(trans,rot) = map_listener.lookupTransform('/map', '/odom', rospy.Time(0))
|
||||
x_SLAM_error = trans[0]-x_ODOM_error
|
||||
y_SLAM_error = trans[1]-y_ODOM_error
|
||||
t_SLAM_error = rot[2]-t_ODOM_error
|
||||
|
||||
print "{0},{1},{2},{3}".format(rospy.get_time(), x_SLAM_error, y_SLAM_error, t_SLAM_error)
|
||||
|
||||
except(tf.LookupException, tf.ConnectivityException):
|
||||
pass
|
||||
|
||||
|
||||
|
||||
# main function call
|
||||
if __name__ == "__main__":
|
||||
# Initialize the node
|
||||
rospy.init_node('slam_error_printer')
|
||||
|
||||
# subscribe to truth pose message
|
||||
sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
|
||||
|
||||
# subscribe to odometry message
|
||||
sub_odom = message_filters.Subscriber('odom', Odometry)
|
||||
|
||||
# synchronize truth pose and odometry data
|
||||
ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
|
||||
ts.registerCallback(callback)
|
||||
|
||||
# Turn control over to ROS
|
||||
rospy.spin()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,73 @@
|
||||
define block model
|
||||
(
|
||||
size [0.5 0.5 0.5]
|
||||
gui_nose 0
|
||||
)
|
||||
|
||||
define topurg ranger
|
||||
(
|
||||
sensor(
|
||||
range [ 0.0 20.0 ]
|
||||
fov 270.25
|
||||
samples 1081
|
||||
)
|
||||
|
||||
# generic model properties
|
||||
color "black"
|
||||
size [ 0.05 0.05 0.1 ]
|
||||
)
|
||||
|
||||
define erratic position
|
||||
(
|
||||
#size [0.415 0.392 0.25]
|
||||
size [0.35 0.35 0.25]
|
||||
origin [-0.05 0 0 0]
|
||||
gui_nose 0
|
||||
drive "diff"
|
||||
localization "odom"
|
||||
topurg(pose [ 0.050 0.000 0 0.000 ])
|
||||
odom_error [0.05 0.05 0.01 0.02 0.01 0.02]
|
||||
)
|
||||
|
||||
define floorplan model
|
||||
(
|
||||
# sombre, sensible, artistic
|
||||
color "gray30"
|
||||
|
||||
# most maps will need a bounding box
|
||||
boundary 1
|
||||
|
||||
gui_nose 0
|
||||
gui_grid 0
|
||||
|
||||
gui_outline 0
|
||||
gripper_return 0
|
||||
fiducial_return 0
|
||||
laser_return 1
|
||||
)
|
||||
|
||||
# set the resolution of the underlying raytrace model in meters
|
||||
resolution 0.005
|
||||
|
||||
interval_sim 100 # simulation timestep in milliseconds
|
||||
|
||||
|
||||
window
|
||||
(
|
||||
size [ 745.000 448.000 ]
|
||||
|
||||
rotate [ 0.000 0.000 ]
|
||||
scale 5
|
||||
)
|
||||
|
||||
# load an environment bitmap
|
||||
floorplan
|
||||
(
|
||||
name "manyDots"
|
||||
bitmap "manyDots.pgm"
|
||||
size [54.0 58.7 0.5]
|
||||
pose [ 0 0.0 0 90.000 ]
|
||||
)
|
||||
|
||||
# throw in a robot
|
||||
erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])
|
||||
Reference in New Issue
Block a user