From 7e6bf6dc9c107b8eab74a5167d99d086efa74668 Mon Sep 17 00:00:00 2001
From: Malte Roosen <mroosen@uni-koblenz.de>
Date: Thu, 17 Nov 2016 13:13:29 +0100
Subject: [PATCH] Introduce parameter to change behaviour after collision
avoidance
---
.../include/homer_navigation/homer_navigation_node.h | 4 ++++
homer_navigation/src/homer_navigation_node.cpp | 9 +++++++++
2 files changed, 13 insertions(+)
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index 45b7d394..8239a574 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -271,6 +271,10 @@ class HomerNavigationNode {
* check_path_max_distance to robot */
float m_check_path_max_distance;
+ /** do not replan if lisa avoids an obstacle, instead send target
+ * unreachable*/
+ bool m_no_replanning_on_collision;
+
bool m_avoided_collision;
float m_min_turn_angle;
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index bc4f5a18..e56dca63 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -97,6 +97,8 @@ void HomerNavigationNode::loadParameters() {
m_collision_distance_near_target, (float)0.2);
ros::param::param("/homer_navigation/backward_collision_distance",
m_backward_collision_distance, (float)0.5);
+ ros::param::param("/homer_navigation/no_replanning_on_collision",
+ m_no_replanning_on_collision, (bool)false);
// cmd_vel config values
ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
@@ -266,6 +268,13 @@ void HomerNavigationNode::startNavigation() {
targetPositionReached();
return;
}
+ if (m_initial_path_reaches_target && m_no_replanning_on_collision) {
+ ROS_INFO_STREAM(
+ "Collision avoided and option no_replanning_on_collision is set.");
+ ROS_INFO_STREAM("Sending target unreachable.");
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
+ }
ROS_INFO_STREAM("Distance to target still too large ("
<< m_distance_to_target
<< "m; requested: " << m_desired_distance << "m)");
--
GitLab