From f3d1727062234c8d481308f3472add2a857de141 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 2 Dec 2021 12:54:01 +0100
Subject: [PATCH] added problem/node_rate in YAMLS params
---
include/node.h | 4 +++-
src/node.cpp | 9 ++++++---
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/node.h b/include/node.h
index 9c5b54f..44b77e4 100644
--- a/include/node.h
+++ b/include/node.h
@@ -95,8 +95,10 @@ class WolfRosNode
int print_depth_;
bool print_constr_by_, print_metric_, print_state_blocks_;
-
public:
+
+ double node_rate_;
+
WolfRosNode();
virtual ~WolfRosNode(){};
diff --git a/src/node.cpp b/src/node.cpp
index 2aab556..58f7d9e 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -56,7 +56,10 @@ WolfRosNode::WolfRosNode()
ROS_INFO("Creating solver...");
solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
- // ROS SUBSCRIBERS
+ // ROS
+ node_rate_ = server.getParam<double>("problem/node_rate");
+
+ // SUBSCRIBERS
ROS_INFO("Creating subscribers...");
for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS subscriber"))
{
@@ -67,7 +70,7 @@ WolfRosNode::WolfRosNode()
subscribers_.push_back(FactorySubscriber::create(subscriber, subscriber+" - "+topic, server, problem_ptr_->getSensor(sensor), nh_));
}
- // ROS PUBLISHERS
+ // PUBLISHERS
ROS_INFO("Creating publishers...");
for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
{
@@ -208,7 +211,7 @@ int main(int argc, char **argv)
// Wolf node
WolfRosNode wolf_node;
- ros::Rate loopRate(100);
+ ros::Rate loopRate(wolf_node.node_rate_);
// periodic stuff
ros::Time last_check = ros::Time::now();
--
GitLab