diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp index f3cdc1abc206604c5656cac613129cd0d09fbd9a..407e125a757bd1c58759d83a6f31eecfcf9054b4 100644 --- a/src/safe_cmd_alg_node.cpp +++ b/src/safe_cmd_alg_node.cpp @@ -4,10 +4,10 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) : algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(), collision_time_(1), min_dist_(0.4), - max_vel_front_(1.0), - max_vel_rear_(1.0), - limit_vel_front_(1.0), - limit_vel_rear_(1.0), + max_vel_front_(0.0), + max_vel_rear_(0.0), + limit_vel_front_(0.0), + limit_vel_rear_(0.0), front_laser_received_(false), rear_laser_received_(false), new_cmd_vel(false) @@ -40,8 +40,6 @@ SafeCmdAlgNode::~SafeCmdAlgNode(void) void SafeCmdAlgNode::mainNodeThread(void) { this->alg_.lock(); - // [fill msg structures] - //Twist_msg_.data = my_var; if(!alg_.config_.unsafe) { @@ -49,27 +47,19 @@ void SafeCmdAlgNode::mainNodeThread(void) this->update_rear_laser_watchdog(); if(this->front_laser_watchdog_active()) { - ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!"); + ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser watchdog timeout!"); Twist_msg_.linear.x = 0.0; } else if(this->rear_laser_watchdog_active()) { - ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!"); + ROS_ERROR_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser watchdog timeout!"); Twist_msg_.linear.x = 0.0; } else { - // if(!front_laser_received_) - // ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Front laser not received"); - // if(!rear_laser_received_) - // ROS_FATAL_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Rear laser not received"); - // - // front_laser_received_ = false; - // rear_laser_received_ = false; - if(Twist_msg_.linear.x > fabs(max_vel_front_)) { - ROS_WARN_STREAM("heading to Front obstacle, reducing velocity"<<fabs(max_vel_front_)); + ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing forward velocity from "<< Twist_msg_.linear.x << " to "<<fabs(max_vel_front_)); Twist_msg_.linear.x = fabs(max_vel_front_); if(max_vel_front_==0) Twist_msg_.angular.z = 0; @@ -77,93 +67,73 @@ void SafeCmdAlgNode::mainNodeThread(void) if(Twist_msg_.linear.x < -fabs(max_vel_rear_)) { - ROS_WARN_STREAM("heading to Rear obstacle, reducing velocity"<<fabs(max_vel_rear_)); + ROS_INFO_STREAM_THROTTLE(1,"SafeCmdAlgNode::mainNodeThread: Reducing backward velocity from "<< Twist_msg_.linear.x << " to "<<-fabs(max_vel_rear_)); Twist_msg_.linear.x = -fabs(max_vel_rear_); if(max_vel_rear_==0) Twist_msg_.angular.z = 0; } } } - // [fill srv structure and make request to the server] - - // [fill action structure and make request to the action server] - // [publish messages] if(this->new_cmd_vel) { cmd_vel_safe_publisher_.publish(Twist_msg_); this->new_cmd_vel=false; } + + // [fill msg structures] + + // [fill srv structure and make request to the server] - this->alg_.unlock(); + // [fill action structure and make request to the action server] + + // [publish messages] + this->alg_.unlock(); } /* [subscriber callbacks] */ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) { //ROS_INFO("SafeCmdAlgNode::cmd_vel_callback: New Message Received"); - - //use appropiate mutex to shared variables if necessary this->alg_.lock(); //cmd_vel_mutex_.enter(); - if(!alg_.config_.unsafe) - { - if (msg->linear.x == 0 && msg->angular.z ==0) - Twist_msg_ = *msg; - else - { - Twist_msg_.linear.x += (msg->linear.x - last_twist_.linear.x ); - Twist_msg_.linear.y += (msg->linear.y - last_twist_.linear.y ); - Twist_msg_.linear.z += (msg->linear.z - last_twist_.linear.z ); - Twist_msg_.angular.x += (msg->angular.x - last_twist_.angular.x); - Twist_msg_.angular.y += (msg->angular.y - last_twist_.angular.y); - Twist_msg_.angular.z += (msg->angular.z - last_twist_.angular.z); - } - last_twist_= *msg; - } else - Twist_msg_ = *msg; + Twist_msg_=*msg; this->new_cmd_vel=true; - //unlock previously blocked shared variables - this->alg_.unlock(); + //cmd_vel_mutex_.exit(); + this->alg_.unlock(); } void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { //ROS_INFO("SafeCmdAlgNode::rear_laser_callback: New Message Received"); - - //use appropiate mutex to shared variables if necessary this->alg_.lock(); //rear_laser_mutex_.enter(); - - this->reset_rear_laser_watchdog(); + this->reset_rear_laser_watchdog(); max_vel_rear_ = std::min(compute_max_velocity_(msg),limit_vel_rear_); rear_laser_received_ = true; //ROS_INFO("Max vel r: %f",max_vel_rear_); - //unlock previously blocked shared variables - this->alg_.unlock(); //rear_laser_mutex_.exit(); + this->alg_.unlock(); + } void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { //ROS_INFO("SafeCmdAlgNode::front_laser_callback: New Message Received"); - - //use appropiate mutex to shared variables if necessary this->alg_.lock(); //front_laser_mutex_.enter(); - - this->reset_front_laser_watchdog(); + this->reset_front_laser_watchdog(); max_vel_front_ = std::min(compute_max_velocity_(msg),limit_vel_front_); front_laser_received_ = true; //ROS_INFO("Max vel f: %f",max_vel_front_); - //unlock previously blocked shared variables - this->alg_.unlock(); //front_laser_mutex_.exit(); + this->alg_.unlock(); + } /* [service callbacks] */ @@ -175,11 +145,13 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr void SafeCmdAlgNode::node_config_update(Config &config, uint32_t level) { this->alg_.lock(); + collision_time_ = config.collision_time; min_dist_ = config.min_dist; limit_vel_front_ = config.limit_vel_front; limit_vel_rear_ = config.limit_vel_rear; this->config=config; + this->alg_.unlock(); } @@ -206,13 +178,15 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP float min_range = *std::min_element( scan->ranges.begin(), scan->ranges.end(),min_test_); int min_pos = distance(scan->ranges.begin(),std::min_element( scan->ranges.begin(), scan->ranges.end() )); float max_velocity = 0; - - ROS_DEBUG_STREAM("compute_max_velocity frame: " << scan->header.frame_id << - " min range: " << min_range << " at " << min_pos << " of " << scan->ranges.size()); + //float x = scan->ranges[min_pos]*cos(scan->angle_min + min_pos*scan->angle_increment); + //float y = scan->ranges[min_pos]*sin(scan->angle_min + min_pos*scan->angle_increment); + if (min_range >= min_dist_) max_velocity = min_range / collision_time_; + //ROS_INFO_STREAM("SafeCmdAlgNode::compute_max_velocity_: min_range=" << min_range << ", max_velocity=" << max_velocity); + return max_velocity; }