Skip to content
Snippets Groups Projects
Commit f3afb0be authored by Martí Morta Garriga's avatar Martí Morta Garriga
Browse files

Added unsafe param. Added minimum distance. Changed scale to collision time, in seconds

parent 92fc2b94
No related branches found
No related tags found
No related merge requests found
......@@ -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"))
\subsubsection usage Usage
\verbatim
<node name="SafeCmdAlgorithm" pkg="iri_safe_cmd" type="SafeCmdAlgorithm">
<param name="unsafe" type="bool" value="False" />
</node>
\endverbatim
......@@ -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
......@@ -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.
......@@ -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:
......
......@@ -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
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment