From 47d4f42ac62aea9dc026f52bc183176de7f69642 Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Thu, 19 Jan 2017 22:43:10 +0100
Subject: [PATCH] navigation with dynamic maps
---
homer_mapping/config/homer_mapping.yaml | 2 +-
.../homer_mapping/ParticleFilter/SlamFilter.h | 2 +
.../src/OccupancyMap/OccupancyMap.cpp | 44 +++++++++----------
.../src/ParticleFilter/SlamFilter.cpp | 12 +++--
homer_navigation/CMakeLists.txt | 3 +-
.../launch/homer_navigation.launch | 2 +-
.../src/homer_navigation_node.cpp | 8 ++--
7 files changed, 40 insertions(+), 33 deletions(-)
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index 751ffb0b..cf75bf30 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -17,7 +17,7 @@
/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
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index a712fb39..89026615 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/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
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index a1f8b468..515128a2 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -277,16 +277,16 @@ void OccupancyMap::computeOccupancyProbabilities()
int i = x + yOffset;
if ( m_MeasurementCount[i] > 0 )
{
- int maxCount = 100; //TODO param
- if(m_MeasurementCount[i] > maxCount * 2 -1)
- {
- int scalingFactor = m_MeasurementCount[i] / maxCount;
- if ( scalingFactor != 0 )
- {
- m_MeasurementCount[i] /= scalingFactor;
- m_OccupancyCount[i] /= scalingFactor;
- }
- }
+ //int maxCount = 100; //TODO param
+ //if(m_MeasurementCount[i] > maxCount * 2 -1)
+ //{
+ //int scalingFactor = m_MeasurementCount[i] / maxCount;
+ //if ( scalingFactor != 0 )
+ //{
+ //m_MeasurementCount[i] /= scalingFactor;
+ //m_OccupancyCount[i] /= scalingFactor;
+ //}
+ //}
m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float> ( m_MeasurementCount[i] );
if (m_HighSensitive[i] == 1)
{
@@ -321,7 +321,7 @@ void OccupancyMap::computeOccupancyProbabilities()
void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform)
{
m_latestMapTransform = transform;
- //markRobotPositionFree();
+ markRobotPositionFree();
std::vector<RangeMeasurement> ranges;
ranges.reserve ( laserData->ranges.size() );
@@ -818,15 +818,15 @@ void OccupancyMap::applyChanges()
if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
{
- //if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
- //m_MeasurementCount[x + m_metaData.width * (y+1)]++;
- //if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
- //m_MeasurementCount[x + m_metaData.width * (y-1)]++;
- //if(m_MeasurementCount[i-1] > 1)
- //m_MeasurementCount[i-1]++;
- //if(m_MeasurementCount[i+1] > 1)
- //m_MeasurementCount[i+1]++;
- m_OccupancyCount[i] ++;
+ //if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
+ //m_MeasurementCount[x + m_metaData.width * (y+1)]++;
+ //if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
+ //m_MeasurementCount[x + m_metaData.width * (y-1)]++;
+ //if(m_MeasurementCount[i-1] > 1)
+ //m_MeasurementCount[i-1]++;
+ //if(m_MeasurementCount[i+1] > 1)
+ //m_MeasurementCount[i+1]++;
+ m_OccupancyCount[i]++ ;
}
}
}
@@ -921,12 +921,12 @@ void OccupancyMap::markRobotPositionFree()
geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform);
Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
- int width = 0.3 / m_metaData.resolution;
+ int width = 0.35 / m_metaData.resolution;
for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ )
{
for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ )
{
- incrementMeasurementCount ( Eigen::Vector2i ( i, j ) );
+ incrementMeasurementCount ( Eigen::Vector2i ( j, i ) );
}
}
Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width );
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index cb37b877..1e58a727 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/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,8 +316,9 @@ 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 )
@@ -333,6 +335,10 @@ void SlamFilter::filter (Pose currentPose, sensor_msgs::LaserScanConstPtr laserD
}
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();
diff --git a/homer_navigation/CMakeLists.txt b/homer_navigation/CMakeLists.txt
index 0d5bda2d..a763a402 100644
--- a/homer_navigation/CMakeLists.txt
+++ b/homer_navigation/CMakeLists.txt
@@ -16,7 +16,8 @@ find_package(catkin REQUIRED COMPONENTS
find_package(Eigen3 REQUIRED)
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_BUILD_TYPE Debug)
catkin_package(
INCLUDE_DIRS include
diff --git a/homer_navigation/launch/homer_navigation.launch b/homer_navigation/launch/homer_navigation.launch
index db217818..baa57eec 100644
--- a/homer_navigation/launch/homer_navigation.launch
+++ b/homer_navigation/launch/homer_navigation.launch
@@ -1,4 +1,4 @@
<launch>
<rosparam command="load" file="$(find homer_navigation)/config/homer_navigation.yaml"/>
- <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen"/>
+ <node ns="/homer_navigation" name="homer_navigation" pkg="homer_navigation" type="homer_navigation" output="screen" launch-prefix="gdb"/>
</launch>
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 3c5d45c5..c5de4ecc 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -208,8 +208,6 @@ void HomerNavigationNode::calculatePath() {
m_path_reaches_target = true;
return;
}
- m_explorer->setOccupancyMap(m_width, m_height, m_origin,
- &(*m_last_map_data)[0]);
m_explorer->setStart(
map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
@@ -271,8 +269,6 @@ void HomerNavigationNode::startNavigation() {
maskMap();
}
- m_explorer->setOccupancyMap(m_width, m_height, m_origin,
- &(*m_last_map_data)[0]);
// check if there still exists a path to the original target
if (m_avoided_collision && m_initial_path_reaches_target &&
@@ -865,7 +861,7 @@ void HomerNavigationNode::maskMap() {
ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
safe_planning_box.max(), m_origin, m_resolution));
for (size_t x = 0; x < m_width; x++) {
- for (size_t y = 0; y < m_width; y++) {
+ for (size_t y = 0; y < m_height; y++){
if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
m_last_map_data->at(y * m_width + x) = -1;
}
@@ -905,6 +901,8 @@ void HomerNavigationNode::mapCallback(
m_width = msg->info.width;
m_height = msg->info.height;
m_resolution = msg->info.resolution;
+ m_explorer->setOccupancyMap(m_width, m_height, m_origin,
+ &(*m_last_map_data)[0]);
switch (m_MainMachine.state()) {
case AWAITING_PATHPLANNING_MAP:
--
GitLab