From eed025e15b5182001881c3c2dc321560f347b9f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mart=C3=AD=20Morta=20Garriga?= <mmorta@iri.upc.edu>
Date: Mon, 28 Oct 2013 16:35:58 +0000
Subject: [PATCH] CRITICAL FIX after ostia to drawer
---
src/safe_cmd_alg_node.cpp | 18 +++++++++++-------
1 file changed, 11 insertions(+), 7 deletions(-)
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index fced9ff..b4d2107 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -4,8 +4,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
collision_time_(1),
min_dist_(0.3),
- max_vel_front_(1),
- max_vel_rear_(-1)
+ max_vel_front_(5),
+ max_vel_rear_(-5)
{
//init class attributes if necessary
loop_rate_ = 20;//in [Hz]
@@ -71,18 +71,19 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
if(!alg_.config_.unsafe)
{
- if(Twist_msg_.linear.x < max_vel_front_ && Twist_msg_.linear.x > max_vel_rear_ )
+ if (msg->linear.x == 0 && msg->angular.z ==0)
+ Twist_msg_ = *msg;
+ else
{
- Twist_msg_.linear.y += (msg->linear.y - last_twist_.linear.y );
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
+ } else
Twist_msg_ = *msg;
//unlock previously blocked shared variables
@@ -98,6 +99,7 @@ void SafeCmdAlgNode::rear_laser_callback(const sensor_msgs::LaserScan::ConstPtr&
//this->rear_laser_mutex_.enter();
max_vel_rear_ = - compute_max_velocity_(msg);
+ //ROS_INFO("Max vel r: %f",max_vel_rear_);
//unlock previously blocked shared variables
//this->alg_.unlock();
@@ -112,6 +114,7 @@ void SafeCmdAlgNode::front_laser_callback(const sensor_msgs::LaserScan::ConstPtr
//this->front_laser_mutex_.enter();
max_vel_front_ = compute_max_velocity_(msg);
+ //ROS_INFO("Max vel f: %f",max_vel_front_);
//unlock previously blocked shared variables
//this->alg_.unlock();
@@ -143,7 +146,7 @@ int main(int argc,char *argv[])
float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstPtr& scan) const
{
- float max_velocity = 1;
+ float max_velocity = 5;
for (uint i = 0; i < scan->ranges.size(); i++)
{
@@ -153,5 +156,6 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP
max_velocity = scan->ranges[i] / collision_time_;
}
+
return max_velocity;
}
--
GitLab