Skip to content
Snippets Groups Projects
Commit d9593d99 authored by Projekt Robbie's avatar Projekt Robbie
Browse files

Rebase from 'debian/kinetic/homer_mapping'

parent 339c9bc2
No related branches found
No related merge requests found
......@@ -2,6 +2,23 @@
Changelog for package homer_mapping
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.11 (2017-01-19)
-------------------
* cool changelogs
* navigation with dynamic maps
* dynamic masking map
* no more squared maps
* dynamic map size and wall kernel
* dynamic map size
* Contributors: Lisa
* navigation with dynamic maps
* dynamic masking map
* no more squared maps
* dynamic map size and wall kernel
* dynamic map size
* Contributors: Lisa
0.1.10 (2016-12-08)
-------------------
......
/homer_mapping/size: 40 # size of one edge of the map in m. map is quadratic
/homer_mapping/resolution: 0.05 # m meter per cell
/homer_mapping/max_laser: 20.0 # m max range for including range into map
/homer_mapping/size: 4 # size of one edge of the map in m. map is quadratic
/homer_mapping/resolution: 0.04 # m meter per cell
#map config values
/homer_mapping/backside_checking: false # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
/homer_mapping/backside_checking: true # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
/homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
/homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation
......@@ -11,20 +10,20 @@
/particlefilter/error_values/rotation_error_rotating: 10.0 # percent
/particlefilter/error_values/rotation_error_translating: 2.0 # degrees per meter
/particlefilter/error_values/translation_error_translating: 10.0 # percent
/particlefilter/error_values/translation_error_rotating: 0.05 # m per degree
/particlefilter/error_values/move_jitter_while_turning: 0.1 # m per degree
/particlefilter/error_values/translation_error_translating: 1.0 # percent
/particlefilter/error_values/translation_error_rotating: 0.09 # m per degree
/particlefilter/error_values/move_jitter_while_turning: 0.05 # m per degree
/particlefilter/hyper_slamfilter/particlefilter_num: 1
/particlefilter/particle_num: 1000
/particlefilter/max_rotation_per_second: 0.4 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
/particlefilter/max_rotation_per_second: 0.1 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
/particlefilter/wait_time: 0.05 # minimum time to wait between two slam steps in seconds
#the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed
/particlefilter/update_min_move_angle: 2 # degrees
/particlefilter/update_min_move_dist: 0.05 # m
/particlefilter/max_update_interval: 0.5 # sec
/particlefilter/max_update_interval: 0.2 # sec
/selflocalization/scatter_var_xy: 0.1 # m
/selflocalization/scatter_var_theta: 0.2 # radiants
......
ros-kinetic-homer-mapping (0.1.10-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 07 Dec 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.9-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Tue, 22 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.8-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Sun, 20 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.7-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 16 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.6-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Thu, 03 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.5-1xenial) xenial; urgency=high
-- Viktor Seib <vseib@uni-koblenz.de> Thu, 03 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.4-1xenial) xenial; urgency=high
* updated changelog
* fixes
* Contributors: Niklas Yann Wettengel
* fixes
* Contributors: Niklas Yann Wettengel
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 02 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.3-1xenial) xenial; urgency=high
* more fixes
* Contributors: Niklas Yann Wettengel
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 02 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.2-1xenial) xenial; urgency=high
* install launch files
* Contributors: Niklas Yann Wettengel
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 02 Nov 2016 23:00:00 -0000
ros-kinetic-homer-mapping (0.1.1-1xenial) xenial; urgency=high
* fixes
* initial commit
* Contributors: Niklas Yann Wettengel
-- Viktor Seib <vseib@uni-koblenz.de> Wed, 02 Nov 2016 23:00:00 -0000
@[for change_version, change_date, changelog, main_name, main_email in changelogs]@(Package) (@(change_version)-@(DebianInc)@(Distribution)) @(Distribution); urgency=high
@(changelog)
-- @(main_name) <@(main_email)> @(change_date)
@[end for]
9
\ No newline at end of file
@(debhelper_version)
\ No newline at end of file
Source: ros-kinetic-homer-mapping
Section: misc
Priority: extra
Maintainer: Viktor Seib <vseib@uni-koblenz.de>
Build-Depends: debhelper (>= 9.0.0), libeigen3-dev, libqt5core5a, libqt5gui5, libqt5widgets5, qtbase5-dev, ros-kinetic-catkin, ros-kinetic-cmake-modules, ros-kinetic-homer-mapnav-msgs, ros-kinetic-homer-nav-libs, ros-kinetic-nav-msgs, ros-kinetic-roscpp, ros-kinetic-roslib, ros-kinetic-sensor-msgs, ros-kinetic-tf
Homepage:
Standards-Version: 3.9.2
Package: ros-kinetic-homer-mapping
Architecture: any
Depends: ${shlibs:Depends}, ${misc:Depends}, libeigen3-dev, libqt5core5a, libqt5gui5, libqt5widgets5, ros-kinetic-homer-mapnav-msgs, ros-kinetic-homer-nav-libs, ros-kinetic-nav-msgs, ros-kinetic-roscpp, ros-kinetic-roslib, ros-kinetic-sensor-msgs, ros-kinetic-std-msgs, ros-kinetic-tf
Description: homer_mapping
Source: @(Package)
Section: misc
Priority: extra
Maintainer: @(Maintainer)
Build-Depends: debhelper (>= @(debhelper_version).0.0), @(', '.join(BuildDepends))
Homepage: @(Homepage)
Standards-Version: 3.9.2
Package: @(Package)
Architecture: any
Depends: ${shlibs:Depends}, ${misc:Depends}, @(', '.join(Depends))
@[if Conflicts]Conflicts: @(', '.join(Conflicts))@\n@[end if]@
@[if Replaces]Replaces: @(', '.join(Replaces))@\n@[end if]@
Description: @(Description)
[git-buildpackage]
upstream-tag=release/kinetic/homer_mapping/0.1.10-1
upstream-tag=@(release_tag)
upstream-tree=tag
......@@ -14,29 +14,29 @@ export DH_OPTIONS=-v --buildsystem=cmake
# https://code.ros.org/trac/ros/ticket/2977
# https://code.ros.org/trac/ros/ticket/3842
export LDFLAGS=
export PKG_CONFIG_PATH=/opt/ros/kinetic/lib/pkgconfig
export PKG_CONFIG_PATH=@(InstallationPrefix)/lib/pkgconfig
# Explicitly enable -DNDEBUG, see:
# https://github.com/ros-infrastructure/bloom/issues/327
export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG
%:
dh $@
dh $@@
override_dh_auto_configure:
# In case we're installing to a non-standard location, look for a setup.sh
# in the install tree that was dropped by catkin, and source it. It will
# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.
if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \
if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \
dh_auto_configure -- \
-DCATKIN_BUILD_BINARY_PACKAGE="1" \
-DCMAKE_INSTALL_PREFIX="/opt/ros/kinetic" \
-DCMAKE_PREFIX_PATH="/opt/ros/kinetic"
-DCMAKE_INSTALL_PREFIX="@(InstallationPrefix)" \
-DCMAKE_PREFIX_PATH="@(InstallationPrefix)"
override_dh_auto_build:
# In case we're installing to a non-standard location, look for a setup.sh
# in the install tree that was dropped by catkin, and source it. It will
# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.
if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \
if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \
dh_auto_build
override_dh_auto_test:
......@@ -44,19 +44,19 @@ override_dh_auto_test:
# in the install tree that was dropped by catkin, and source it. It will
# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.
echo -- Running tests. Even if one of them fails the build is not canceled.
if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \
if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \
dh_auto_test || true
override_dh_shlibdeps:
# In case we're installing to a non-standard location, look for a setup.sh
# in the install tree that was dropped by catkin, and source it. It will
# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.
if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \
dh_shlibdeps -l$(CURDIR)/debian/ros-kinetic-homer-mapping//opt/ros/kinetic/lib/
if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \
dh_shlibdeps -l$(CURDIR)/debian/@(Package)/@(InstallationPrefix)/lib/
override_dh_auto_install:
# In case we're installing to a non-standard location, look for a setup.sh
# in the install tree that was dropped by catkin, and source it. It will
# set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH.
if [ -f "/opt/ros/kinetic/setup.sh" ]; then . "/opt/ros/kinetic/setup.sh"; fi && \
if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \
dh_auto_install
......@@ -13,6 +13,7 @@
#include <homer_nav_libs/Math/Box2D.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
......@@ -91,7 +92,7 @@ class OccupancyMap {
/**
* Constructor for a loaded map.
*/
OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int pixelSize, Box2D<int> exploredRegion);
OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion);
/**
* Copy constructor, copies all members inclusive the arrays that lie behind the pointers.
......@@ -170,7 +171,7 @@ class OccupancyMap {
float computeScore( Pose robotPose, std::vector<MeasurePoint> measurePoints );
/**
* @return QImage of size m_PixelSize, m_PixelSize with values of m_OccupancyProbability scaled to 0-254
* @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254
*/
QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
......@@ -187,9 +188,9 @@ class OccupancyMap {
* @param[out] data vector containing occupancy probabilities. 0 = free, 100 = occupied, -1 = NOT_KNOWN
* @param[out] width Width of data array
* @param[out] height Height of data array
* @param[out] resolution Resolution of the map (m_Resolution)
* @param[out] resolution Resolution of the map (m_metaData.resolution)
*/
void getOccupancyProbabilityImage(vector<int8_t> &data, int& width, int& height, float &resolution);
void getOccupancyProbabilityImage(vector<int8_t> &data, nav_msgs::MapMetaData& metaData);
/**
* This method marks free the position of the robot (according to its dimensions).
......@@ -219,6 +220,8 @@ class OccupancyMap {
*/
void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down );
protected:
......@@ -297,22 +300,14 @@ class OccupancyMap {
*/
void cleanUp();
/**
* Stores the size of one map pixel in m.
*/
float m_Resolution;
/**
* Stores the metadata of the map
*/
nav_msgs::MapMetaData m_metaData;
/**
* Stores the origin of the map
*/
geometry_msgs::Pose m_Origin;
/**
* Stores the width of the map in cell numbers.
*/
int m_PixelSize;
/**
* Stores the size of the map arrays, i.e. m_PixelSize * m_PixelSize
* Stores the size of the map arrays, i.e. m_metaData.width * m_metaData.height
* for faster computation.
*/
unsigned m_ByteSize;
......@@ -329,9 +324,6 @@ class OccupancyMap {
// Counts how often a pixel is hit by a measurement that says the pixel is occupied.
unsigned short* m_OccupancyCount;
// Counts how often a cell is marked as inaccessible via markInaccessible()
unsigned char* m_InaccessibleCount;
// Used for setting flags for cells, that have been modified during the current update.
unsigned char* m_CurrentChanges;
......@@ -341,10 +333,6 @@ class OccupancyMap {
/**
* Store values from config files.
*/
// maximum valid range of one laser measurement
float m_LaserMaxRange;
//minimum valid range of one laser measurement
float m_LaserMinRange;
//minimum range classified as free in case of errorneous laser measurement
float m_FreeReadingDistance;
//enables checking to avoid matching front- and backside of an obstacle, e.g. wall
......@@ -357,9 +345,6 @@ class OccupancyMap {
//bool to reset high_sensitive Areas on the next iteration
bool m_reset_high;
//position of the laser scaner in base_link frame
geometry_msgs::Point m_LaserPos;
/**
* Defines a bounding box around the changes in the current map.
*/
......
......@@ -312,6 +312,8 @@ class SlamFilter : public ParticleFilter<SlamParticle> {
*/
ros::Time m_LastUpdateTime;
ros::Time m_LastMoveTime;
/**
* Calculates the square of given input f
* @param f input
......
<package>
<name>homer_mapping</name>
<version>0.1.10</version>
<version>0.1.11</version>
<description>
homer_mapping
......
This diff is collapsed.
......@@ -4,7 +4,7 @@
// minimum move for translation in m
const float MIN_MOVE_DISTANCE2 = 0.001 * 0.001;
// minimum turn in radiants
const float MIN_TURN_DISTANCE2 = 0.001 * 0.001;
const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
const float M_2PI = 2 * M_PI;
......@@ -50,6 +50,7 @@ SlamFilter::SlamFilter ( int particleNum ) : ParticleFilter<SlamParticle> ( part
m_EffectiveParticleNum = m_ParticleNum;
m_LastUpdateTime = ros::Time(0);
m_LastMoveTime = ros::Time::now();
}
SlamFilter::SlamFilter ( SlamFilter& slamFilter ) : ParticleFilter<SlamParticle> ( slamFilter.m_ParticleNum )
......@@ -315,24 +316,29 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD
Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
// do not resample if move to small
if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 )
// do not resample if move to small and last move is min 0.5 sec away
if ( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0 )
//if(false)
{
ROS_DEBUG_STREAM( "Move too small, will not resample." );
if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
{
resample();
ROS_INFO_STREAM( "Particles too scattered, resampling." );
// filter steps
ROS_DEBUG_STREAM( "Move too small, will not resample." );
if ( m_EffectiveParticleNum < m_ParticleNum / 10 )
{
resample();
ROS_INFO_STREAM( "Particles too scattered, resampling." );
// filter steps
drift();
measure();
normalize();
sort ( 0, m_ParticleNum - 1 );
}
}
}
else
{
if(!( sqr ( trans.x() ) + sqr ( trans.y() ) < MIN_MOVE_DISTANCE2 && sqr ( trans.theta() ) < MIN_TURN_DISTANCE2 ))
{
m_LastMoveTime = ros::Time::now();
}
resample();
// filter steps
drift();
......@@ -365,17 +371,19 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD
else
{
thetaPerSecond = trans.theta() / elapsedSeconds;
}
}
float poseVarianceX, poseVarianceY;
getPoseVariances(50, poseVarianceX, poseVarianceY);
m_LastUpdatePose = likeliestPose;
m_LastUpdateTime = measurementTime;
if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond )
if ( std::fabs(thetaPerSecond) < m_MaxRotationPerSecond && poseVarianceX < 0.05 && poseVarianceY < 0.05 )
{
updateMap();
m_LastUpdatePose = likeliestPose;
m_LastUpdateTime = measurementTime;
}
else
{
ROS_DEBUG_STREAM( "No mapping performed, rotation angle too big." );
ROS_WARN_STREAM( "No mapping performed, rotation angle too big." );
}
}
else
......
......@@ -113,34 +113,23 @@ void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
void SlamNode::sendMapDataMessage(ros::Time mapTime)
{
std::vector<int8_t> mapData;
int width, height;
float resolution;
nav_msgs::MapMetaData metaData;
OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
occMap->getOccupancyProbabilityImage (mapData, width, height, resolution);
occMap->getOccupancyProbabilityImage (mapData, metaData);
if ( width != height )
{
ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
}
else
//if ( width != height )
//{
//ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
//}
//else
{
nav_msgs::OccupancyGrid mapMsg;
std_msgs::Header header;
header.stamp = mapTime;
header.frame_id = "map";
mapMsg.header = header;
nav_msgs::MapMetaData mapMetaData;
mapMetaData.width = width;
mapMetaData.height = height;
mapMetaData.resolution = resolution;
mapMetaData.origin.position.x = -height*resolution/2;
mapMetaData.origin.position.y = -width*resolution/2;
mapMetaData.origin.orientation.w = 1.0;
mapMetaData.origin.orientation.x = 0.0;
mapMetaData.origin.orientation.y = 0.0;
mapMetaData.origin.orientation.z = 0.0;
mapMsg.info = mapMetaData;
mapMsg.info = metaData;
mapMsg.data = mapData;
m_SLAMMapPublisher.publish(mapMsg);
......@@ -323,10 +312,10 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
float res = msg->info.resolution;
int height = msg->info.height; // cell size
int width = msg->info.width; //cell size
if(height!=width) {
ROS_ERROR("Height != width in loaded map");
return;
}
//if(height!=width) {
//ROS_ERROR("Height != width in loaded map");
//return;
//}
//convert map vector from ros format to robbie probability array
float* map = new float[msg->data.size()];
......@@ -359,7 +348,7 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg)
}
}
Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY );
OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, exploredRegion);
OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, height, exploredRegion);
m_HyperSlamFilter->setOccupancyMap( occMap );
m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
ROS_INFO_STREAM( "Replacing occupancy map" );
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment