From d34fa6fde1b422f52d73eae97ecee7ab372e4a47 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 09:04:24 +0000
Subject: [PATCH] publisher in mainNodeThread since cmd_vel is not sent in a
constant rate
---
include/safe_cmd_alg_node.h | 1 +
src/safe_cmd_alg_node.cpp | 25 ++++++++++++++-----------
2 files changed, 15 insertions(+), 11 deletions(-)
diff --git a/include/safe_cmd_alg_node.h b/include/safe_cmd_alg_node.h
index 31862a2..69bbce4 100644
--- a/include/safe_cmd_alg_node.h
+++ b/include/safe_cmd_alg_node.h
@@ -46,6 +46,7 @@ class SafeCmdAlgNode : public algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>
// [publisher attributes]
ros::Publisher cmd_vel_safe_publisher_;
geometry_msgs::Twist Twist_msg_;
+ geometry_msgs::Twist last_twist_;
// [subscriber attributes]
ros::Subscriber cmd_vel_subscriber_;
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 2082258..c88d9d0 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -35,12 +35,24 @@ 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)
+ {
+ 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]
// [publish messages]
+ cmd_vel_safe_publisher_.publish(Twist_msg_);
}
@@ -52,16 +64,7 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//this->cmd_vel_mutex_.enter();
- Twist_msg_=*msg;
-
- if(front_obstacle_ && msg->linear.x > 0.0)
- Twist_msg_.linear.x = 0.0;
-
- if(rear_obstacle_ && msg->linear.x < 0.0)
- Twist_msg_.linear.x = 0.0;
-
- cmd_vel_safe_publisher_.publish(Twist_msg_);
-
+ last_twist_=*msg;
//unlock previously blocked shared variables
//this->alg_.unlock();
--
GitLab