From f3afb0be7a6f2ad4af07284c6ef44adf368ac177 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 13:13:50 +0000 Subject: [PATCH] Added unsafe param. Added minimum distance. Changed scale to collision time, in seconds --- cfg/safe_cmd_alg_config.cfg | 2 +- docs/SafeCmdConfig-usage.dox | 1 + docs/SafeCmdConfig.dox | 1 + docs/SafeCmdConfig.wikidoc | 4 ++++ include/safe_cmd_alg_node.h | 3 ++- src/safe_cmd_alg_node.cpp | 45 +++++++++++++++++++++--------------- 6 files changed, 35 insertions(+), 21 deletions(-) diff --git a/cfg/safe_cmd_alg_config.cfg b/cfg/safe_cmd_alg_config.cfg index 6af3940..0857f10 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 d3ad1ec..5488094 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 1d79be4..e6dea0c 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 a381793..8650c3c 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 0460d93..f079f8e 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 e1d1342..806229a 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 +} -- GitLab