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;
}
}
void TimePublisher::runStalledClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
rosgraph_msgs::Clock pub_msg;
ros::WallTime t = ros::WallTime::now();
ros::WallTime done = t + duration;
while ( t < done )
{
if (t > next_pub_)
{
pub_msg.clock = current_;
time_pub_.publish(pub_msg);
next_pub_ = t + wall_step_;
}
ros::WallTime target = done;
if (target > next_pub_)
target = next_pub_;
ros::WallTime::sleepUntil(target);
t = ros::WallTime::now();
}
} else {
duration.sleep();
}
}
bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timestamp, const Eigen::Affine3d& transformationSensorToMap)
{
if (pointCloud->size() != pointCloudVariances.size()) {
ROS_ERROR("ElevationMap::add: Size of point cloud (%i) and variances (%i) do not agree.",
(int) pointCloud->size(), (int) pointCloudVariances.size());
return false;
}
// Initialization for time calculation.
const ros::WallTime methodStartTime(ros::WallTime::now());
boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
// Update initial time if it is not initialized.
if (initialTime_.toSec() == 0) {
initialTime_ = timestamp;
}
const double scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
for (unsigned int i = 0; i < pointCloud->size(); ++i) {
auto& point = pointCloud->points[i];
Index index;
Position position(point.x, point.y);
if (!rawMap_.getIndex(position, index)) continue; // Skip this point if it does not lie within the elevation map.
auto& elevation = rawMap_.at("elevation", index);
auto& variance = rawMap_.at("variance", index);
auto& horizontalVarianceX = rawMap_.at("horizontal_variance_x", index);
auto& horizontalVarianceY = rawMap_.at("horizontal_variance_y", index);
auto& horizontalVarianceXY = rawMap_.at("horizontal_variance_xy", index);
auto& color = rawMap_.at("color", index);
auto& time = rawMap_.at("time", index);
auto& lowestScanPoint = rawMap_.at("lowest_scan_point", index);
auto& sensorXatLowestScan = rawMap_.at("sensor_x_at_lowest_scan", index);
auto& sensorYatLowestScan = rawMap_.at("sensor_y_at_lowest_scan", index);
auto& sensorZatLowestScan = rawMap_.at("sensor_z_at_lowest_scan", index);
const float& pointVariance = pointCloudVariances(i);
const float scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
if (!rawMap_.isValid(index)) {
// No prior information in elevation map, use measurement.
elevation = point.z;
variance = pointVariance;
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
colorVectorToValue(point.getRGBVector3i(), color);
continue;
}
// Deal with multiple heights in one cell.
const double mahalanobisDistance = fabs(point.z - elevation) / sqrt(variance);
if (mahalanobisDistance > mahalanobisDistanceThreshold_) {
if (scanTimeSinceInitialization - time <= scanningDuration_ && elevation > point.z) {
// Ignore point if measurement is from the same point cloud (time comparison) and
// if measurement is lower then the elevation in the map.
} else if (scanTimeSinceInitialization - time <= scanningDuration_) {
// If point is higher.
elevation = point.z;
variance = pointVariance;
} else {
variance += multiHeightNoise_;
}
continue;
}
// Store lowest points from scan for visibility checking.
const float pointHeightPlusUncertainty = point.z + 3.0 * sqrt(pointVariance); // 3 sigma.
if (std::isnan(lowestScanPoint) || pointHeightPlusUncertainty < lowestScanPoint){
lowestScanPoint = pointHeightPlusUncertainty;
const Position3 sensorTranslation(transformationSensorToMap.translation());
sensorXatLowestScan = sensorTranslation.x();
sensorYatLowestScan = sensorTranslation.y();
sensorZatLowestScan = sensorTranslation.z();
}
// Fuse measurement with elevation map data.
elevation = (variance * point.z + pointVariance * elevation) / (variance + pointVariance);
variance = (pointVariance * variance) / (pointVariance + variance);
// TODO Add color fusion.
colorVectorToValue(point.getRGBVector3i(), color);
time = scanTimeSinceInitialization;
// Horizontal variances are reset.
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
}
clean();
rawMap_.setTimestamp(timestamp.toNSec()); // Point cloud stores time in microseconds.
const ros::WallDuration duration = ros::WallTime::now() - methodStartTime;
ROS_INFO("Raw map has been updated with a new point cloud in %f s.", duration.toSec());
return true;
}
bool ElevationMap::fuse(const grid_map::Index& topLeftIndex, const grid_map::Index& size)
{
ROS_DEBUG("Fusing elevation map...");
// Nothing to do.
if ((size == 0).any()) return false;
// Initializations.
const ros::WallTime methodStartTime(ros::WallTime::now());
boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
// Copy raw elevation map data for safe multi-threading.
boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
auto rawMapCopy = rawMap_;
scopedLockForRawData.unlock();
// More initializations.
const double halfResolution = fusedMap_.getResolution() / 2.0;
const float minimalWeight = std::numeric_limits<float>::epsilon() * (float) 2.0;
// Conservative cell inclusion for ellipse iterator.
const double ellipseExtension = M_SQRT2 * fusedMap_.getResolution();
// Check if there is the need to reset out-dated data.
if (fusedMap_.getTimestamp() != rawMapCopy.getTimestamp()) resetFusedData();
// Align fused map with raw map.
if (rawMapCopy.getPosition() != fusedMap_.getPosition()) fusedMap_.move(rawMapCopy.getPosition());
// For each cell in requested area.
for (SubmapIterator areaIterator(rawMapCopy, topLeftIndex, size); !areaIterator.isPastEnd(); ++areaIterator) {
// Check if fusion for this cell has already been done earlier.
if (fusedMap_.isValid(*areaIterator)) continue;
if (!rawMapCopy.isValid(*areaIterator)) {
// This is an empty cell (hole in the map).
// TODO.
continue;
}
// Get size of error ellipse.
const float& sigmaXsquare = rawMapCopy.at("horizontal_variance_x", *areaIterator);
const float& sigmaYsquare = rawMapCopy.at("horizontal_variance_y", *areaIterator);
const float& sigmaXYsquare = rawMapCopy.at("horizontal_variance_xy", *areaIterator);
Eigen::Matrix2d covarianceMatrix;
covarianceMatrix << sigmaXsquare, sigmaXYsquare, sigmaXYsquare, sigmaYsquare;
// 95.45% confidence ellipse which is 2.486-sigma for 2 dof problem.
// http://www.reid.ai/2012/09/chi-squared-distribution-table-with.html
const double uncertaintyFactor = 2.486; // sqrt(6.18)
Eigen::EigenSolver<Eigen::Matrix2d> solver(covarianceMatrix);
Eigen::Array2d eigenvalues(solver.eigenvalues().real().cwiseAbs());
Eigen::Array2d::Index maxEigenvalueIndex;
eigenvalues.maxCoeff(&maxEigenvalueIndex);
Eigen::Array2d::Index minEigenvalueIndex;
maxEigenvalueIndex == Eigen::Array2d::Index(0) ? minEigenvalueIndex = 1 : minEigenvalueIndex = 0;
const Length ellipseLength = 2.0 * uncertaintyFactor * Length(eigenvalues(maxEigenvalueIndex), eigenvalues(minEigenvalueIndex)).sqrt() + ellipseExtension;
const double ellipseRotation(atan2(solver.eigenvectors().col(maxEigenvalueIndex).real()(1), solver.eigenvectors().col(maxEigenvalueIndex).real()(0)));
// Requested length and position (center) of submap in map.
Position requestedSubmapPosition;
rawMapCopy.getPosition(*areaIterator, requestedSubmapPosition);
EllipseIterator ellipseIterator(rawMapCopy, requestedSubmapPosition, ellipseLength, ellipseRotation);
// Prepare data fusion.
Eigen::ArrayXf means, weights;
const unsigned int maxNumberOfCellsToFuse = ellipseIterator.getSubmapSize().prod();
means.resize(maxNumberOfCellsToFuse);
weights.resize(maxNumberOfCellsToFuse);
WeightedEmpiricalCumulativeDistributionFunction<float> lowerBoundDistribution;
WeightedEmpiricalCumulativeDistributionFunction<float> upperBoundDistribution;
float maxStandardDeviation = sqrt(eigenvalues(maxEigenvalueIndex));
float minStandardDeviation = sqrt(eigenvalues(minEigenvalueIndex));
Eigen::Rotation2Dd rotationMatrix(ellipseRotation);
std::string maxEigenvalueLayer, minEigenvalueLayer;
if (maxEigenvalueIndex == 0) {
maxEigenvalueLayer = "horizontal_variance_x";
minEigenvalueLayer = "horizontal_variance_y";
} else {
maxEigenvalueLayer = "horizontal_variance_y";
minEigenvalueLayer = "horizontal_variance_x";
}
// For each cell in error ellipse.
size_t i = 0;
for (; !ellipseIterator.isPastEnd(); ++ellipseIterator) {
if (!rawMapCopy.isValid(*ellipseIterator)) {
// Empty cell in submap (cannot be center cell because we checked above).
continue;
}
means[i] = rawMapCopy.at("elevation", *ellipseIterator);
// Compute weight from probability.
Position absolutePosition;
rawMapCopy.getPosition(*ellipseIterator, absolutePosition);
Eigen::Vector2d distanceToCenter = (rotationMatrix * (absolutePosition - requestedSubmapPosition)).cwiseAbs();
float probability1 =
cumulativeDistributionFunction(distanceToCenter.x() + halfResolution, 0.0, maxStandardDeviation)
- cumulativeDistributionFunction(distanceToCenter.x() - halfResolution, 0.0, maxStandardDeviation);
float probability2 =
cumulativeDistributionFunction(distanceToCenter.y() + halfResolution, 0.0, minStandardDeviation)
- cumulativeDistributionFunction(distanceToCenter.y() - halfResolution, 0.0, minStandardDeviation);
const float weight = max(minimalWeight, probability1 * probability2);
weights[i] = weight;
const float standardDeviation = sqrt(rawMapCopy.at("variance", *ellipseIterator));
lowerBoundDistribution.add(means[i] - 2.0 * standardDeviation, weight);
upperBoundDistribution.add(means[i] + 2.0 * standardDeviation, weight);
i++;
}
if (i == 0) {
// Nothing to fuse.
fusedMap_.at("elevation", *areaIterator) = rawMapCopy.at("elevation", *areaIterator);
fusedMap_.at("lower_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) - 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
fusedMap_.at("upper_bound", *areaIterator) = rawMapCopy.at("elevation", *areaIterator) + 2.0 * sqrt(rawMapCopy.at("variance", *areaIterator));
fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
continue;
}
// Fuse.
means.conservativeResize(i);
weights.conservativeResize(i);
float mean = (weights * means).sum() / weights.sum();
if (!std::isfinite(mean)) {
ROS_ERROR("Something went wrong when fusing the map: Mean = %f", mean);
continue;
}
// Add to fused map.
fusedMap_.at("elevation", *areaIterator) = mean;
lowerBoundDistribution.compute();
upperBoundDistribution.compute();
fusedMap_.at("lower_bound", *areaIterator) = lowerBoundDistribution.quantile(0.01); // TODO
fusedMap_.at("upper_bound", *areaIterator) = upperBoundDistribution.quantile(0.99); // TODO
// TODO Add fusion of colors.
fusedMap_.at("color", *areaIterator) = rawMapCopy.at("color", *areaIterator);
}
fusedMap_.setTimestamp(rawMapCopy.getTimestamp());
const ros::WallDuration duration(ros::WallTime::now() - methodStartTime);
ROS_INFO("Elevation map has been fused in %f s.", duration.toSec());
return true;
}
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;
}
void AmclNode::runFromBag(const std::string &in_bag_fn)
{
rosbag::Bag bag;
bag.open(in_bag_fn, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("tf"));
std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
topics.push_back(scan_topic_name);
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);
// Sleep for a second to let all subscribers connect
ros::WallDuration(1.0).sleep();
ros::WallTime start(ros::WallTime::now());
// Wait for map
while (ros::ok())
{
{
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if (map_)
{
ROS_INFO("Map is ready");
break;
}
}
ROS_INFO("Waiting for map...");
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
}
BOOST_FOREACH(rosbag::MessageInstance const msg, view)
{
if (!ros::ok())
{
break;
}
// Process any ros messages or callbacks at this point
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());
tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
if (tf_msg != NULL)
{
tf_pub.publish(msg);
for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
{
tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority");
}
continue;
}
sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
if (base_scan != NULL)
{
laser_pub.publish(msg);
laser_scan_filter_->add(base_scan);
if (bag_scan_period_ > ros::WallDuration(0))
{
bag_scan_period_.sleep();
}
continue;
}
ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
}
bag.close();
double runtime = (ros::WallTime::now() - start).toSec();
ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);
const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
double yaw, pitch, roll;
tf::Matrix3x3(tf::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
last_published_pose.pose.pose.position.x,
last_published_pose.pose.pose.position.y,
yaw, last_published_pose.header.stamp.toSec()
);
ros::shutdown();
}
namespace master
{
uint32_t g_port = 0;
std::string g_host;
std::string g_uri;
ros::WallDuration g_retry_timeout;
void init(const M_string& remappings)
{
M_string::const_iterator it = remappings.find("__master");
if (it != remappings.end())
{
g_uri = it->second;
}
if (g_uri.empty())
{
char *master_uri_env = NULL;
#ifdef _MSC_VER
_dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
#else
master_uri_env = getenv("ROS_MASTER_URI");
#endif
if (!master_uri_env)
{
ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
"type the following or (preferrably) add this to your " \
"~/.bashrc file in order set up your " \
"local machine as a ROS master:\n\n" \
"export ROS_MASTER_URI=http://localhost:11311\n\n" \
"then, type 'roscore' in another shell to actually launch " \
"the master program.");
ROS_BREAK();
}
g_uri = master_uri_env;
#ifdef _MSC_VER
// http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
free(master_uri_env);
#endif
}
// Split URI into
if (!network::splitURI(g_uri, g_host, g_port))
{
ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
ROS_BREAK();
}
}
const std::string& getHost()
{
return g_host;
}
uint32_t getPort()
{
return g_port;
}
const std::string& getURI()
{
return g_uri;
}
void setRetryTimeout(ros::WallDuration timeout)
{
if (timeout < ros::WallDuration(0))
{
ROS_FATAL("retry timeout must not be negative.");
ROS_BREAK();
}
g_retry_timeout = timeout;
}
bool check()
{
XmlRpc::XmlRpcValue args, result, payload;
args[0] = this_node::getName();
return execute("getPid", args, result, payload, false);
}
bool getTopics(V_TopicInfo& topics)
{
XmlRpc::XmlRpcValue args, result, payload;
args[0] = this_node::getName();
args[1] = ""; //TODO: Fix this
if (!execute("getPublishedTopics", args, result, payload, true))
{
return false;
}
topics.clear();
for (int i = 0; i < payload.size(); i++)
{
topics.push_back(TopicInfo(std::string(payload[i][0]), std::string(payload[i][1])));
}
return true;
}
bool getNodes(V_string& nodes)
{
XmlRpc::XmlRpcValue args, result, payload;
args[0] = this_node::getName();
if (!execute("getSystemState", args, result, payload, true))
{
return false;
}
S_string node_set;
for (int i = 0; i < payload.size(); ++i)
{
for (int j = 0; j < payload[i].size(); ++j)
{
XmlRpc::XmlRpcValue val = payload[i][j][1];
for (int k = 0; k < val.size(); ++k)
{
std::string name = payload[i][j][1][k];
node_set.insert(name);
}
}
}
nodes.insert(nodes.end(), node_set.begin(), node_set.end());
return true;
}
#if defined(__APPLE__)
boost::mutex g_xmlrpc_call_mutex;
#endif
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;
}
} // namespace master
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;
}