From 92fc2b940bd7ce2e38e111e43f4408a678aba033 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mart=C3=AD=20Morta=20Garriga?= <mmorta@iri.upc.edu>
Date: Tue, 22 Oct 2013 10:31:58 +0000
Subject: [PATCH] using velocity to define the safe area
---
include/safe_cmd_alg_node.h | 2 +-
src/safe_cmd_alg_node.cpp | 4 ++--
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 69bbce4..0460d93 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -68,7 +68,7 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [action client attributes]
bool front_obstacle_;
bool rear_obstacle_;
- float radius_;
+ float scale_;
bool is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstPtr& scan);
public:
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index c88d9d0..e1d1342 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,7 +4,7 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
front_obstacle_(false),
rear_obstacle_(false),
- radius_(0.5)
+ scale_(1)
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
@@ -134,7 +134,7 @@ bool SafeCmdAlgNode::is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstP
uint i=0;
while (i < scan->ranges.size())
{
- if(scan->ranges[i] < radius_ && scan->ranges[i] > scan->range_min)
+ if(scan->ranges[i] < fabs(last_twist_.linear.x)*scale_ && scan->ranges[i] > scan->range_min)
return true;
i++;
}
--
GitLab