Skip to content
Snippets Groups Projects
Commit aab13a63 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

iri_safe_cmd: minor updates, simplified cmd_vel copy in callback

parent 75412a73
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/safe_cmd_alg_node.cpp
+ 32
− 58
View file @ aab13a63
......@@ -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;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment