From 466d0ee54735c8732598a88aba8ccc8cb390793a Mon Sep 17 00:00:00 2001
From: Fernando Herrero Cotarelo <fherrero@iri.upc.edu>
Date: Tue, 10 Jun 2014 13:49:44 +0000
Subject: [PATCH] iri_safe_cmd: - added parameters for variables - if a scan
is not received, outputs an error (but keeps working)
---
cfg/SafeCmd.cfg | 4 ++++
docs/SafeCmdConfig-usage.dox | 4 ++++
docs/SafeCmdConfig.dox | 4 ++++
docs/SafeCmdConfig.wikidoc | 16 ++++++++++++++++
include/safe_cmd_alg_node.h | 3 +++
src/safe_cmd_alg_node.cpp | 18 +++++++++++++++---
6 files changed, 46 insertions(+), 3 deletions(-)
diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index d9b6069..3fd5426 100755
--- a/cfg/SafeCmd.cfg
+++ b/cfg/SafeCmd.cfg
@@ -39,6 +39,10 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
gen.add("unsafe", bool_t, 0, "Not use the safe module", False)
+gen.add("min_dist", double_t, 0, "distance in meters where to stop", 0.4, 0, 2.0)
+gen.add("collision_time", double_t, 0, "time in seconds to compute collision", 1.0, 0.1, 5.0)
+gen.add("limit_vel_front", double_t, 0, "max speed in meters per second forward", 1.0, 0.0, 2.0)
+gen.add("limit_vel_rear", double_t, 0, "max speed in meters per second backward", 1.0, 0.0, 2.0)
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
exit(gen.generate(PACKAGE, "SafeCmdAlgorithm", "SafeCmd"))
diff --git a/docs/SafeCmdConfig-usage.dox b/docs/SafeCmdConfig-usage.dox
index 5488094..46a2f84 100644
--- a/docs/SafeCmdConfig-usage.dox
+++ b/docs/SafeCmdConfig-usage.dox
@@ -2,6 +2,10 @@
\verbatim
<node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
<param name="unsafe" type="bool" value="False" />
+ <param name="min_dist" type="double" value="0.4" />
+ <param name="collision_time" type="double" value="1.0" />
+ <param name="limit_vel_front" type="double" value="1.0" />
+ <param name="limit_vel_rear" type="double" value="1.0" />
</node>
\endverbatim
diff --git a/docs/SafeCmdConfig.dox b/docs/SafeCmdConfig.dox
index e6dea0c..a6d8500 100644
--- a/docs/SafeCmdConfig.dox
+++ b/docs/SafeCmdConfig.dox
@@ -3,4 +3,8 @@
Reads and maintains the following parameters on the ROS server
- \b "~unsafe" : \b [bool] Not use the safe module min: False, default: False, max: True
+- \b "~min_dist" : \b [double] distance in meters where to stop min: 0.0, default: 0.4, max: 2.0
+- \b "~collision_time" : \b [double] time in seconds to compute collision min: 0.1, default: 1.0, max: 5.0
+- \b "~limit_vel_front" : \b [double] max speed in meters per second forward min: 0.0, default: 1.0, max: 2.0
+- \b "~limit_vel_rear" : \b [double] max speed in meters per second backward min: 0.0, default: 1.0, max: 2.0
diff --git a/docs/SafeCmdConfig.wikidoc b/docs/SafeCmdConfig.wikidoc
index 8650c3c..15764fa 100644
--- a/docs/SafeCmdConfig.wikidoc
+++ b/docs/SafeCmdConfig.wikidoc
@@ -7,6 +7,22 @@ desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfig
0.default= False
0.type= bool
0.desc=Not use the safe module
+1.name= ~min_dist
+1.default= 0.4
+1.type= double
+1.desc=distance in meters where to stop Range: 0.0 to 2.0
+2.name= ~collision_time
+2.default= 1.0
+2.type= double
+2.desc=time in seconds to compute collision Range: 0.1 to 5.0
+3.name= ~limit_vel_front
+3.default= 1.0
+3.type= double
+3.desc=max speed in meters per second forward Range: 0.0 to 2.0
+4.name= ~limit_vel_rear
+4.default= 1.0
+4.type= double
+4.desc=max speed in meters per second backward Range: 0.0 to 2.0
}
}
# End of autogenerated section. You may edit below.
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index c91f170..961cd3f 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -61,6 +61,9 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
CMutex front_laser_mutex_;
+ bool front_laser_received;
+ bool rear_laser_received;
+
// [service attributes]
// [client attributes]
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index a77e93a..59b6c59 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -7,7 +7,9 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
max_vel_front_(7),
max_vel_rear_(7),
limit_vel_front_(7),
- limit_vel_rear_(7)
+ limit_vel_rear_(7),
+ front_laser_received(false),
+ rear_laser_received(false)
{
//init class attributes if necessary
loop_rate_ = 20;//in [Hz]
@@ -41,17 +43,22 @@ void SafeCmdAlgNode::mainNodeThread(void)
if(!alg_.config_.unsafe)
{
+ if(!this->front_laser_received || !this->rear_laser_received)
+ ROS_ERROR("SafeCmdAlgNode::mainNodeThread: laser/s not received");
+
if(Twist_msg_.linear.x > fabs(max_vel_front_))
{
Twist_msg_.linear.x = fabs(max_vel_front_);
ROS_WARN("heading to front obstacle, reducing velocity");
- }
+ }
if(Twist_msg_.linear.x < -fabs(max_vel_rear_))
{
Twist_msg_.linear.x = -fabs(max_vel_rear_);
ROS_WARN("heading to rear obstacle, reducing velocity");
}
+ this->front_laser_received = false;
+ this->rear_laser_received = false;
}
// [fill srv structure and make request to the server]
@@ -102,6 +109,7 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_);
//ROS_INFO("Max vel r: %f",max_vel_rear_);
+ this->rear_laser_received = true;
//unlock previously blocked shared variables
//this->alg_.unlock();
@@ -117,6 +125,7 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_);
//ROS_INFO("Max vel f: %f",max_vel_front_);
+ this->front_laser_received = true;
//unlock previously blocked shared variables
//this->alg_.unlock();
@@ -132,7 +141,10 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
{
alg_.lock();
-
+ this->collision_time_ = config.collision_time;
+ this->min_dist_ = config.min_dist;
+ this->limit_vel_front_ = config.limit_vel_front;
+ this->limit_vel_rear_ = config.limit_vel_rear;
alg_.unlock();
}
--
GitLab