Rebase from 'debian/kinetic/homer_mapping'

CHANGELOG.rst

@@ -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)
 -------------------
 

config/homer_mapping.yaml

-/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

debian/changelog

-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
-
-

debian/changelog.em

+@[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]

debian/compat

-9
\ No newline at end of file

debian/compat.em

+@(debhelper_version)
\ No newline at end of file

debian/control

-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

debian/control.em

+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)

debian/gbp.conf → debian/gbp.conf.em

 [git-buildpackage]
-upstream-tag=release/kinetic/homer_mapping/0.1.10-1
+upstream-tag=@(release_tag)
 upstream-tree=tag

debian/rules → debian/rules.em

@@ -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

include/homer_mapping/OccupancyMap/OccupancyMap.h

@@ -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.
      */

include/homer_mapping/ParticleFilter/SlamFilter.h

@@ -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.xml

 <package>
   <name>homer_mapping</name>
-  <version>0.1.10</version>
+  <version>0.1.11</version>
   <description>
 
      homer_mapping

src/ParticleFilter/SlamFilter.cpp

@@ -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

src/slam_node.cpp

@@ -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" );