TEST(LogUtils, testJSKROSXXX){
JSK_ROS_DEBUG("Testing JSK_ROS_DEBUG: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO("Testing JSK_ROS_INFO: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN("Testing JSK_ROS_WARN: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR("Testing JSK_ROS_ERROR: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL("Testing JSK_ROS_FATAL: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM("Testing " << "JSK_ROS_DEBUG_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM("Testing " << "JSK_ROS_INFO_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM("Testing " << "JSK_ROS_WARN_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM("Testing " << "JSK_ROS_ERROR_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM("Testing " << "JSK_ROS_FATAL_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_DEBUG_THROTTLE(1, "Testing JSK_ROS_DEBUG_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO_THROTTLE(1, "Testing JSK_ROS_INFO_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN_THROTTLE(1, "Testing JSK_ROS_WARN_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR_THROTTLE(1, "Testing JSK_ROS_ERROR_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL_THROTTLE(1, "Testing JSK_ROS_FATAL_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_DEBUG_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_INFO_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_WARN_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_ERROR_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_FATAL_STREAM_THROTTLE: " << ros::Time::now().toNSec());
}
/**
format is
successors:
- x: 0
y: 0
theta: 0
- x: 0
y: 0
theta: 0
...
*/
bool FootstepPlanner::readSuccessors(ros::NodeHandle& nh)
{
successors_.clear();
if (!nh.hasParam("successors")) {
JSK_ROS_FATAL("no successors are specified");
return false;
}
XmlRpc::XmlRpcValue successors_xml;
nh.param("successors", successors_xml, successors_xml);
if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
JSK_ROS_FATAL("successors should be an array");
return false;
}
for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
XmlRpc::XmlRpcValue successor_xml;
successor_xml = successors_xml[i_successors];
if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
JSK_ROS_FATAL("element of successors should be an dictionary");
return false;
}
double x = 0;
double y = 0;
double theta = 0;
if (successor_xml.hasMember("x")) {
x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
}
if (successor_xml.hasMember("y")) {
y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
}
if (successor_xml.hasMember("theta")) {
theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
}
Eigen::Affine3f successor = affineFromXYYaw(x, y, theta);
successors_.push_back(successor);
}
JSK_ROS_INFO("%lu successors are defined", successors_.size());
return true;
}
void PolygonArrayColorLikelihood::readReference(const std::string& file)
{
// The yaml format should be
// bins:
// -
// min_value: xx
// max_value: xx
// count: xx
// -
// min_value: xx
// max_value: xx
// count: xx
// ...
jsk_recognition_msgs::HistogramWithRange read_msg;
#ifdef USE_OLD_YAML
YAML::Node doc;
std::ifstream fin;
fin.open(file.c_str(), std::ifstream::in);
YAML::Parser parser(fin);
while (parser.GetNextDocument(doc)) {
const YAML::Node& bins_yaml = doc["bins"];
for (size_t i = 0; i < bins_yaml.size(); i++) {
const YAML::Node& bin_yaml = bins_yaml[i];
const YAML::Node& min_value_yaml = bin_yaml["min_value"];
const YAML::Node& max_value_yaml = bin_yaml["max_value"];
const YAML::Node& count_yaml = bin_yaml["count"];
jsk_recognition_msgs::HistogramWithRangeBin bin;
min_value_yaml >> bin.min_value;
max_value_yaml >> bin.max_value;
count_yaml >> bin.count;
read_msg.bins.push_back(bin);
}
}
#else
JSK_ROS_FATAL("yaml 0.5 is not supported yet");
#endif
reference_ = boost::make_shared<jsk_recognition_msgs::HistogramWithRange>(read_msg);
}
