bool QuadrotorPropulsion::processQueue(const ros::Time ×tamp, const ros::Duration &tolerance, const ros::Duration &delay, const ros::WallDuration &wait, ros::CallbackQueue *callback_queue) {
boost::mutex::scoped_lock lock(command_queue_mutex_);
bool new_command = false;
ros::Time min(timestamp), max(timestamp);
try {
min = timestamp - delay - tolerance /* - ros::Duration(dt) */;
} catch (std::runtime_error &e) {}
try {
max = timestamp - delay + tolerance;
} catch (std::runtime_error &e) {}
do {
while(!command_queue_.empty()) {
hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = command_queue_.front();
ros::Time new_time = new_motor_voltage->header.stamp;
if (new_time.isZero() || (new_time >= min && new_time <= max)) {
setVoltage(*new_motor_voltage);
command_queue_.pop();
new_command = true;
// new motor command is too old:
} else if (new_time < min) {
ROS_DEBUG_NAMED("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).toSec());
command_queue_.pop();
// new motor command is too new:
} else {
break;
}
}
if (command_queue_.empty() && !new_command) {
if (!motor_status_.on || wait.isZero()) break;
ROS_DEBUG_NAMED("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.toSec(), (timestamp - last_command_time_).toSec());
if (!callback_queue) {
if (command_condition_.timed_wait(lock, wait.toBoost())) continue;
} else {
lock.unlock();
callback_queue->callAvailable(wait);
lock.lock();
if (!command_queue_.empty()) continue;
}
ROS_ERROR_NAMED("quadrotor_propulsion", "Command timed out. Disabled motors.");
shutdown();
}
} while(false);
if (new_command) {
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)");
}
return new_command;
}
void CallbackQueue::callAvailable(ros::WallDuration timeout)
{
setupTLS();
TLS* tls = tls_.get();
{
boost::mutex::scoped_lock lock(mutex_);
if (!enabled_)
{
return;
}
if (callbacks_.empty())
{
if (!timeout.isZero())
{
condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
}
if (callbacks_.empty() || !enabled_)
{
return;
}
}
bool was_empty = tls->callbacks.empty();
tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
callbacks_.clear();
calling_ += tls->callbacks.size();
if (was_empty)
{
tls->cb_it = tls->callbacks.begin();
}
}
size_t called = 0;
while (!tls->callbacks.empty())
{
if (callOneCB(tls) != Empty)
{
++called;
}
}
{
boost::mutex::scoped_lock lock(mutex_);
calling_ -= called;
}
}
CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
{
setupTLS();
TLS* tls = tls_.get();
CallbackInfo cb_info;
{
boost::mutex::scoped_lock lock(mutex_);
if (!enabled_)
{
return Disabled;
}
if (callbacks_.empty())
{
if (!timeout.isZero())
{
condition_.timed_wait(lock, boost::posix_time::microseconds(timeout.toSec() * 1000000.0f));
}
if (callbacks_.empty())
{
return Empty;
}
if (!enabled_)
{
return Disabled;
}
}
D_CallbackInfo::iterator it = callbacks_.begin();
for (; it != callbacks_.end();)
{
CallbackInfo& info = *it;
if (info.marked_for_removal)
{
it = callbacks_.erase(it);
continue;
}
if (info.callback->ready())
{
cb_info = info;
it = callbacks_.erase(it);
break;
}
++it;
}
if (!cb_info.callback)
{
return TryAgain;
}
++calling_;
}
bool was_empty = tls->callbacks.empty();
tls->callbacks.push_back(cb_info);
if (was_empty)
{
tls->cb_it = tls->callbacks.begin();
}
CallOneResult res = callOneCB(tls);
if (res != Empty)
{
boost::mutex::scoped_lock lock(mutex_);
--calling_;
}
return res;
}
bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
{
ros::WallTime start_time = ros::WallTime::now();
std::string master_host = getHost();
uint32_t master_port = getPort();
XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
bool printed = false;
bool slept = false;
bool ok = true;
do
{
bool b = false;
{
#if defined(__APPLE__)
boost::mutex::scoped_lock lock(g_xmlrpc_call_mutex);
#endif
b = c->execute(method.c_str(), request, response);
}
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
if (!b && ok)
{
if (!printed && wait_for_master)
{
ROS_ERROR("[%s] Failed to contact master at [%s:%d]. %s", method.c_str(), master_host.c_str(), master_port, wait_for_master ? "Retrying..." : "");
printed = true;
}
if (!wait_for_master)
{
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
if (!g_retry_timeout.isZero() && (ros::WallTime::now() - start_time) >= g_retry_timeout)
{
ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
ros::WallDuration(0.05).sleep();
slept = true;
}
else
{
if (!XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload))
{
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return false;
}
break;
}
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
} while(ok);
if (ok && slept)
{
ROS_INFO("Connected to master at [%s:%d]", master_host.c_str(), master_port);
}
XMLRPCManager::instance()->releaseXMLRPCClient(c);
return true;
}