From 75412a73bf4e6979d39512929af454ed6a0d26e8 Mon Sep 17 00:00:00 2001
From: Fernando Herrero Cotarelo <fherrero@iri.upc.edu>
Date: Mon, 18 May 2015 09:46:36 +0000
Subject: [PATCH] iri_safe_cmd: publish cmd_vel messages ONLY if new input
cmd_vel msg received
---
include/safe_cmd_alg_node.h | 1 +
src/safe_cmd_alg_node.cpp | 11 ++++++++---
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 5c88505..d8ddaaf 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -63,6 +63,7 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
bool front_laser_received_;
bool rear_laser_received_;
+ bool new_cmd_vel;
// [service attributes]
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index bb62b2a..f3cdc1a 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -9,7 +9,8 @@ SafeCmdAlgNode::SafeCmdAlgNode(void) :
limit_vel_front_(1.0),
limit_vel_rear_(1.0),
front_laser_received_(false),
- rear_laser_received_(false)
+ rear_laser_received_(false),
+ new_cmd_vel(false)
{
//init class attributes if necessary
loop_rate_ = 20;//in [Hz]
@@ -88,7 +89,11 @@ void SafeCmdAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
- cmd_vel_safe_publisher_.publish(Twist_msg_);
+ if(this->new_cmd_vel)
+ {
+ cmd_vel_safe_publisher_.publish(Twist_msg_);
+ this->new_cmd_vel=false;
+ }
this->alg_.unlock();
@@ -119,7 +124,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
last_twist_= *msg;
} else
Twist_msg_ = *msg;
-
+ this->new_cmd_vel=true;
//unlock previously blocked shared variables
this->alg_.unlock();
//cmd_vel_mutex_.exit();
--
GitLab