Skip to content
Snippets Groups Projects
Commit 0fbb2d0b authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working!

parent 77d2320a
No related branches found
No related tags found
3 merge requests!11new release,!10new release,!8Resolve "publishers threads"
...@@ -83,6 +83,10 @@ class Publisher ...@@ -83,6 +83,10 @@ class Publisher
virtual ~Publisher(){}; virtual ~Publisher(){};
virtual void run() final;
virtual void stop() final;
virtual void loop() final;
virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0; virtual void initialize(ros::NodeHandle& nh, const std::string& topic) = 0;
virtual void publish() final; virtual void publish() final;
...@@ -109,6 +113,8 @@ class Publisher ...@@ -109,6 +113,8 @@ class Publisher
std::string prefix_; std::string prefix_;
std::string topic_; std::string topic_;
std::thread pub_thread_;
// PROFILING // PROFILING
unsigned int n_publish_; unsigned int n_publish_;
std::chrono::microseconds acc_duration_; std::chrono::microseconds acc_duration_;
...@@ -140,14 +146,7 @@ inline void Publisher::publish() ...@@ -140,14 +146,7 @@ inline void Publisher::publish()
inline bool Publisher::ready() inline bool Publisher::ready()
{ {
long unsigned int n_period = (long unsigned int)((ros::Time::now() - first_publish_time_).toSec() / period_); return true;
if (n_period > last_n_period_)
{
last_n_period_ = n_period;
return true;
}
return false;
} }
inline void Publisher::printProfiling(std::ostream &_stream) const inline void Publisher::printProfiling(std::ostream &_stream) const
...@@ -159,6 +158,35 @@ inline void Publisher::printProfiling(std::ostream &_stream) const ...@@ -159,6 +158,35 @@ inline void Publisher::printProfiling(std::ostream &_stream) const
<< "\n\tmax time: " << 1e-3 * max_duration_.count() << " ms" << std::endl; << "\n\tmax time: " << 1e-3 * max_duration_.count() << " ms" << std::endl;
} }
inline void Publisher::run()
{
pub_thread_ = std::thread(&Publisher::loop, this);
}
inline void Publisher::stop()
{
pub_thread_.join();
}
inline void Publisher::loop()
{
auto awake_time = std::chrono::system_clock::now();
auto period = std::chrono::duration<int,std::milli>((int)(period_*1e3));
WOLF_DEBUG("Started publisher ", name_, " loop");
while (ros::ok())
{
if (ready())
publish();
// relax to fit output rate
awake_time += period;
std::this_thread::sleep_until(awake_time);
}
WOLF_DEBUG("Publisher ", name_, " loop finished");
}
template<typename T> template<typename T>
inline T Publisher::getParamWithDefault(const ParamsServer &_server, inline T Publisher::getParamWithDefault(const ParamsServer &_server,
const std::string &_param_name, const std::string &_param_name,
......
...@@ -19,6 +19,7 @@ ...@@ -19,6 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- //--------LICENSE_END--------
#include <chrono>
#include "node.h" #include "node.h"
#include "ros/time.h" #include "ros/time.h"
#include "core/solver/factory_solver.h" #include "core/solver/factory_solver.h"
...@@ -142,7 +143,8 @@ void WolfRosNode::solve() ...@@ -142,7 +143,8 @@ void WolfRosNode::solve()
void WolfRosNode::solveLoop() void WolfRosNode::solveLoop()
{ {
ros::Rate solverRate(1/(solver_->getPeriod()+1e-9)); // 1ns added to allow pausing if rosbag paused if period==0 auto awake_time = std::chrono::system_clock::now();
auto period = std::chrono::duration<int,std::milli>((int)(solver_->getPeriod()*1e3+1)); // 1ms added to allow pausing if rosbag paused if period==0
WOLF_DEBUG("Started solver loop"); WOLF_DEBUG("Started solver loop");
while (ros::ok()) while (ros::ok())
...@@ -153,7 +155,8 @@ void WolfRosNode::solveLoop() ...@@ -153,7 +155,8 @@ void WolfRosNode::solveLoop()
break; break;
// relax to fit output rate // relax to fit output rate
solverRate.sleep(); awake_time += period;
std::this_thread::sleep_until(awake_time);
} }
WOLF_DEBUG("Solver loop finished"); WOLF_DEBUG("Solver loop finished");
} }
...@@ -225,13 +228,12 @@ int main(int argc, char **argv) ...@@ -225,13 +228,12 @@ int main(int argc, char **argv)
int policy=SCHED_FIFO; int policy=SCHED_FIFO;
pthread_setschedparam(solver_thread.native_handle(), SCHED_FIFO, &Priority_Param); pthread_setschedparam(solver_thread.native_handle(), SCHED_FIFO, &Priority_Param);
// Init publishers threads
for(auto pub : wolf_node.publishers_)
pub->run();
while (ros::ok()) while (ros::ok())
{ {
// publish periodically
for(auto pub : wolf_node.publishers_)
if (pub->ready())
pub->publish();
// check that subscribers received data (every second) // check that subscribers received data (every second)
if ((ros::Time::now() - last_check).toSec() > 1) if ((ros::Time::now() - last_check).toSec() > 1)
{ {
...@@ -249,10 +251,16 @@ int main(int argc, char **argv) ...@@ -249,10 +251,16 @@ int main(int argc, char **argv)
// relax to fit output rate // relax to fit output rate
loopRate.sleep(); loopRate.sleep();
} }
// Stop solver thread
WOLF_DEBUG("Node is shutting down outside loop... waiting for the thread to stop..."); WOLF_DEBUG("Node is shutting down outside loop... waiting for the thread to stop...");
solver_thread.join(); solver_thread.join();
WOLF_DEBUG("thread stopped."); WOLF_DEBUG("thread stopped.");
// Stop publishers threads
for(auto pub : wolf_node.publishers_)
pub->stop();
// PROFILING ======================================== // PROFILING ========================================
wolf_node.createProfilingFile(); wolf_node.createProfilingFile();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment