diff --git a/cfg/safe_cmd_alg_config.cfg b/cfg/safe_cmd_alg_config.cfg
index 6af3940f16b2d7120a3658795ef703e6e9450b30..0857f1062ee0620abaa6ecd5e43f0dbe901fb876 100755
--- a/cfg/safe_cmd_alg_config.cfg
+++ b/cfg/safe_cmd_alg_config.cfg
@@ -39,7 +39,7 @@ from dynamic_reconfigure.parameter_generator import *
gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
-#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("unsafe", bool_t, 0, "Not use the safe module", False)
#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 d3ad1ec2726202df4dddee468f1064cd93bf9740..54880943522fb84f9f4f0edd53ba97befce076e0 100644
--- a/docs/SafeCmdConfig-usage.dox
+++ b/docs/SafeCmdConfig-usage.dox
@@ -1,6 +1,7 @@
\subsubsection usage Usage
\verbatim
<node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
+ <param name="unsafe" type="bool" value="False" />
</node>
\endverbatim
diff --git a/docs/SafeCmdConfig.dox b/docs/SafeCmdConfig.dox
index 1d79be4baa8875cbba7d87f8edabe30170cfa577..e6dea0c51d7388a975f003b5a7f25ebfd1fdebb3 100644
--- a/docs/SafeCmdConfig.dox
+++ b/docs/SafeCmdConfig.dox
@@ -2,4 +2,5 @@
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
diff --git a/docs/SafeCmdConfig.wikidoc b/docs/SafeCmdConfig.wikidoc
index a3817931a2ddd66531ee745eccfb603e6aa4a9a0..8650c3c51ef72acfcff35a919076748df782c788 100644
--- a/docs/SafeCmdConfig.wikidoc
+++ b/docs/SafeCmdConfig.wikidoc
@@ -3,6 +3,10 @@ param {
group.0 {
name=Dynamically Reconfigurable Parameters
desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
+0.name= ~unsafe
+0.default= False
+0.type= bool
+0.desc=Not use the safe module
}
}
# 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 0460d93a8d073dbcee626961f819222ed6a33047..f079f8e97500f5ea91c863583d23667f19808e65 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -68,7 +68,8 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [action client attributes]
bool front_obstacle_;
bool rear_obstacle_;
- float scale_;
+ float collision_time_;
+ float min_dist_;
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 e1d1342f3304e051f26cc50895d2d21169372de9..806229ac21b8bf86f886fa681b42da4502f9ad99 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,18 +4,19 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
front_obstacle_(false),
rear_obstacle_(false),
- scale_(1)
+ collision_time_(1),
+ min_dist_(0.2)
{
//init class attributes if necessary
- //this->loop_rate_ = 2;//in [Hz]
+ loop_rate_ = 20;//in [Hz]
// [init publishers]
- this->cmd_vel_safe_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
+ cmd_vel_safe_publisher_ = public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel_safe", 100);
// [init subscribers]
- this->cmd_vel_subscriber_ = this->public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback, this);
- this->rear_laser_subscriber_ = this->public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
- this->front_laser_subscriber_ = this->public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
+ cmd_vel_subscriber_ = public_node_handle_.subscribe("cmd_vel", 100, &SafeCmdAlgNode::cmd_vel_callback,this);
+ rear_laser_subscriber_ = public_node_handle_.subscribe("rear_laser", 100, &SafeCmdAlgNode::rear_laser_callback, this);
+ front_laser_subscriber_ = public_node_handle_.subscribe("front_laser", 100, &SafeCmdAlgNode::front_laser_callback, this);
// [init services]
@@ -36,17 +37,23 @@ void SafeCmdAlgNode::mainNodeThread(void)
// [fill msg structures]
//this->Twist_msg_.data = my_var;
Twist_msg_ = last_twist_;
- if(front_obstacle_ && last_twist_.linear.x > 0.0)
- {
- Twist_msg_.linear.x = 0.0;
- ROS_WARN("heading to front obstacle avoided");
- }
- if(rear_obstacle_ && last_twist_.linear.x < 0.0)
+ if(!alg_.config_.unsafe)
{
- Twist_msg_.linear.x = 0.0;
- ROS_WARN("heading to rear obstacle avoided");
- }
+
+ if(front_obstacle_ && last_twist_.linear.x > 0.0)
+ {
+ Twist_msg_.linear.x = 0.0;
+ ROS_WARN("heading to front obstacle avoided");
+ }
+
+ if(rear_obstacle_ && last_twist_.linear.x < 0.0)
+ {
+ Twist_msg_.linear.x = 0.0;
+ ROS_WARN("heading to rear obstacle avoided");
+ }
+
+ }
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
@@ -113,9 +120,9 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level)
{
- this->alg_.lock();
+ alg_.lock();
- this->alg_.unlock();
+ alg_.unlock();
}
void SafeCmdAlgNode::addNodeDiagnostics(void)
@@ -134,9 +141,9 @@ bool SafeCmdAlgNode::is_point_in_safe_area_(const sensor_msgs::LaserScan::ConstP
uint i=0;
while (i < scan->ranges.size())
{
- if(scan->ranges[i] < fabs(last_twist_.linear.x)*scale_ && scan->ranges[i] > scan->range_min)
+ if((scan->ranges[i] < fabs(last_twist_.linear.x)*collision_time_ || scan->ranges[i] < min_dist_) && scan->ranges[i] > scan->range_min)
return true;
i++;
}
return false;
-}
\ No newline at end of file
+}