Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
148 lines (126 sloc) 4.3 KB
/***************************************************************************
* navgraph_breakout_thread.cpp - Provide navgraph-like API through ROS
*
* Created: Fri Jan 27 11:35:39 2017
* Copyright 2017 Tim Niemueller
****************************************************************************/
/* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Library General Public License for more details.
*
* Read the full text in the LICENSE.GPL file in the doc directory.
*/
#include "navgraph_breakout_thread.h"
#include <interfaces/NavigatorInterface.h>
using namespace fawkes;
/** @class RosNavgraphBreakoutThread "navigator_thread.h"
* Provide navgraph-like API through ROS.
* @author Tim Niemueller
*/
/** Contructor. */
RosNavgraphBreakoutThread::RosNavgraphBreakoutThread()
: Thread("RosNavgraphBreakoutThread", Thread::OPMODE_WAITFORWAKEUP),
BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
{
}
void
RosNavgraphBreakoutThread::init()
{
goal_active_ = false;
was_connected_ = false;
try {
pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
} catch (Exception &e) {
e.append("%s initialization failed, could not open navigator "
"interface for writing",
name());
logger->log_error(name(), e);
throw;
}
cfg_action_topic_ = config->get_string("/ros/navgraph-breakout/action-topic");
ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
}
void
RosNavgraphBreakoutThread::finalize()
{
try {
blackboard->close(pp_nav_if_);
} catch (Exception &e) {
logger->log_error(name(), "Closing interface failed!");
logger->log_error(name(), e);
}
delete ac_;
}
void
RosNavgraphBreakoutThread::loop()
{
if (!ac_->isServerConnected()) {
if (!pp_nav_if_->msgq_empty()) {
logger->log_warn(name(),
"Command received while action provider "
"not available, ignoring");
pp_nav_if_->msgq_flush();
}
if (was_connected_) {
delete ac_;
ac_ = new NavGraphGotoClient(cfg_action_topic_, false);
was_connected_ = false;
}
} else {
was_connected_ = true;
// Check for new incoming commands and process them
while (!pp_nav_if_->msgq_empty()) {
if (NavigatorInterface::StopMessage *msg = pp_nav_if_->msgq_first_safe(msg)) {
if (goal_active_) {
ac_->cancelAllGoals();
goal_active_ = false;
}
} else if (NavigatorInterface::PlaceGotoMessage *msg = pp_nav_if_->msgq_first_safe(msg)) {
logger->log_info(name(), "Relaying place goto for %s", msg->place());
pp_nav_if_->set_msgid(msg->id());
goal_.place = msg->place();
goal_.orientation = std::numeric_limits<float>::quiet_NaN();
if (goal_active_) {
ac_->cancelAllGoals();
}
ac_->sendGoal(goal_);
pp_nav_if_->set_final(false);
pp_nav_if_->set_error_code(0);
pp_nav_if_->write();
goal_active_ = true;
}
pp_nav_if_->msgq_pop();
}
// If there is an active goal, check for its completion
if (goal_active_) {
if (ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
fawkes_msgs::NavGraphGotoResult result = *(ac_->getResult());
if (result.errcode != fawkes_msgs::NavGraphGotoResult::ERROR_NONE) {
if (result.errmsg.empty()) {
logger->log_warn(name(),
"Remote navgraph goto failed without error message (code %u)",
result.errcode);
} else {
logger->log_warn(name(), "Remote navgraph goto failed: %s", result.errmsg.c_str());
}
}
pp_nav_if_->set_final(true);
pp_nav_if_->set_error_code(result.errcode);
pp_nav_if_->write();
goal_active_ = false;
} else if (ac_->getState() == actionlib::SimpleClientGoalState::ABORTED
|| ac_->getState() == actionlib::SimpleClientGoalState::REJECTED) {
pp_nav_if_->set_final(true);
pp_nav_if_->set_error_code(NavigatorInterface::ERROR_OBSTRUCTION);
pp_nav_if_->write();
goal_active_ = false;
}
}
}
}
You can’t perform that action at this time.