diff --git a/cfg/SafeCmd.cfg b/cfg/SafeCmd.cfg
index d9b60690c1e92bdc9ae2ddd6207e396efe6607c5..3fd54267741a80d3142204ed9a4f64de29481c0c 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 54880943522fb84f9f4f0edd53ba97befce076e0..46a2f8417174321c9b3abc85e296810a046601c4 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 e6dea0c51d7388a975f003b5a7f25ebfd1fdebb3..a6d85003f490fbd3e3c9f9c7daf7f6cd84df2d1e 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 8650c3c51ef72acfcff35a919076748df782c788..15764fac323d78499fe5a56f317e800676596642 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 c91f1708522fcfb4269e3ec818a7e5b24fd3547f..961cd3f27664f3092d3fc157804db56e17f2c095 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 a77e93a7018944b7eaeadcdaa34ed48ecc5dc09a..59b6c59a6abc088bac8c782914e256b7062e7d76 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();
}