hsvColorRange::hsvColorRange(XmlRpc::XmlRpcValue _params)
{
ROS_ASSERT(_params.getType()==XmlRpc::XmlRpcValue::TypeStruct);
for (XmlRpc::XmlRpcValue::iterator i=_params.begin(); i!=_params.end(); ++i)
{
ROS_ASSERT(i->second.getType()==XmlRpc::XmlRpcValue::TypeArray);
for(int j=0; j<i->second.size(); ++j)
{
ROS_ASSERT(i->second[j].getType()==XmlRpc::XmlRpcValue::TypeInt);
}
// printf("%s %i %i\n", i->first.c_str(), static_cast<int>(i->second[0]), static_cast<int>(i->second[1]));
if (i->first == "H") H=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
if (i->first == "S") S=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
if (i->first == "V") V=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
}
}
bool GainClient::readGains(XmlRpc::XmlRpcValue& config,
sl_controller_msgs::CartesianGains& position_gains,
sl_controller_msgs::CartesianGains& force_gains)
{
if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Gains must be specified as an array");
return false;
}
if (config.size() != 6)
{
ROS_ERROR("Gains must be be an array of six characters");
return false;
}
char v[6];
for (int i=0; i<6; ++i)
{
if (config[i].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR("Gains must be be an array of six characters");
return false;
}
std::string s = config[i];
v[i] = s.at(0);
if (v[i] != 'f' && v[i] !='p' && v[i]!='0')
{
ROS_ERROR("Gains must be either 'p', 'f', or '0'");
return false;
}
}
setGainElement(v[0], position_gains.pos_x.p_gain, force_gains.pos_x.p_gain);
setGainElement(v[1], position_gains.pos_y.p_gain, force_gains.pos_y.p_gain);
setGainElement(v[2], position_gains.pos_z.p_gain, force_gains.pos_z.p_gain);
setGainElement(v[3], position_gains.rot_x.p_gain, force_gains.rot_x.p_gain);
setGainElement(v[4], position_gains.rot_y.p_gain, force_gains.rot_y.p_gain);
setGainElement(v[5], position_gains.rot_z.p_gain, force_gains.rot_z.p_gain);
setIGainsFromPGains(position_gains);
setDGainsFromPGains(position_gains);
setIGainsFromPGains(force_gains);
setDGainsFromPGains(force_gains);
return true;
}
bool NC2010TransformationSystem::initOneDimensionalTransformationSystemHelper(std::vector<dmp_lib::NC2010TSPtr>& transformation_systems,
XmlRpc::XmlRpcValue transformation_systems_parameters_xml,
ros::NodeHandle& node_handle,
dmp_lib::TransformationSystem::IntegrationMethod integration_method)
{
ROS_ASSERT(transformation_systems_parameters_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
for (int j = 0; j < transformation_systems_parameters_xml.size(); ++j)
{
ROS_DEBUG("Initializing NC2010 transformation system number >%i< from node handle.",j);
// create transformation system
dmp_lib::NC2010TSPtr transformation_system(new dmp_lib::NC2010TransformationSystem());
XmlRpc::XmlRpcValue ts_xml = transformation_systems_parameters_xml[j];
ROS_ASSERT(ts_xml.hasMember(transformation_system->getVersionString()));
XmlRpc::XmlRpcValue nc2010_ts_xml = ts_xml[transformation_system->getVersionString()];
double k_gain = 0;
if (!usc_utilities::getParam(nc2010_ts_xml, "k_gain", k_gain))
{
return false;
}
double d_gain = dmp_lib::NC2010TransformationSystem::getDGain(k_gain);
// create parameters
dmp_lib::NC2010TSParamPtr parameters(new dmp_lib::NC2010TransformationSystemParameters());
// initialize parameters
ROS_VERIFY(parameters->initialize(k_gain, d_gain));
// initialize base class
ROS_VERIFY(TransformationSystem::initFromNodeHandle(parameters, ts_xml, node_handle));
// create state
dmp_lib::NC2010TSStatePtr state(new dmp_lib::NC2010TransformationSystemState());
// initialize transformation system with parameters and state
ROS_VERIFY(transformation_system->initialize(parameters, state, integration_method));
ROS_VERIFY(transformation_system->setIntegrationMethod(integration_method));
transformation_systems.push_back(transformation_system);
}
return true;
}
void StompOptimizationTask::setFeaturesFromXml(const XmlRpc::XmlRpcValue& features_xml)
{
ROS_ASSERT (features_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
std::vector<boost::shared_ptr<StompCostFeature> > features;
for (int i=0; i<features_xml.size(); ++i)
{
XmlRpc::XmlRpcValue feature_xml = features_xml[i];
ROS_ASSERT(feature_xml.hasMember("class") &&
feature_xml["class"].getType() == XmlRpc::XmlRpcValue::TypeString);
std::string class_name = feature_xml["class"];
boost::shared_ptr<StompCostFeature> feature;
try
{
feature = feature_loader_.createInstance(class_name);
}
catch (pluginlib::PluginlibException& ex)
{
ROS_ERROR("Couldn't load feature named %s", class_name.c_str());
ROS_ERROR("Error: %s", ex.what());
ROS_BREAK();
}
STOMP_VERIFY(feature->initialize(feature_xml, num_rollouts_+1,
planning_group_name_,
kinematic_model_,
collision_robot_, collision_world_,
collision_robot_df_, collision_world_df_));
features.push_back(feature);
}
setFeatures(features);
}
inline bool parameterToColor(const ros::NodeHandle& node, const
std::string& key, float* color) {
XmlRpc::XmlRpcValue value;
node.getParam(key, value);
if (value.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (value.size() == 3) {
color[0] = static_cast<double>(value[0]);
color[1] = static_cast<double>(value[1]);
color[2] = static_cast<double>(value[2]);
}
else {
ROS_WARN("Invalid array size for color parameter %s: %d", key.c_str(),
(unsigned int)value.size());
return false;
}
}
else {
ROS_WARN("Invalid type for color parameter %s: expecting array",
key.c_str(), (unsigned int)value.size());
return false;
}
return true;
}
inline std::vector<std::string> getVectorParam(std::string name)
{
std::vector<std::string> values;
XmlRpc::XmlRpcValue list;
if (!root_nh_.getParamCached(name, list))
{
ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
}
if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Hand description: bad parameter %s", name.c_str());
}
//ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
for (int32_t i=0; i<list.size(); i++)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR("Hand description: bad parameter %s", name.c_str());
}
values.push_back( static_cast<std::string>(list[i]) );
//ROS_INFO_STREAM(" " << values.back());
}
return values;
}
TimeWarpNode::TimeWarpNode()
: m_nh("~")
, m_live(true)
, m_tfHandler(this)
{
m_srv = m_nh.advertiseService("control", &TimeWarpNode::handleTimeWarpControl, this);
m_pub_clock = m_nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
m_timer = m_nh.createTimer(ros::Duration(0.1), boost::bind(&TimeWarpNode::update, this));
m_timer.start();
XmlRpc::XmlRpcValue list;
if(m_nh.getParam("extra_topics", list))
{
ROS_INFO("Using extra_topics parameter");
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int i = 0; i < list.size(); ++i)
{
std::string name = static_cast<std::string>(list[i]);
ROS_INFO("Warping extra topic '%s'", name.c_str());
m_handlers.push_back(
new TopicHandler(&m_nh, name)
);
}
}
m_handlers.push_back(
new SmartTopicHandler<sensor_msgs::JointState>(&m_nh, "/joint_states")
);
m_topicThread = TopicThread::launch(m_nh, boost::bind(&TimeWarpNode::updateTopics, this, _1));
}
YAML::Node XmlToYaml( XmlRpc::XmlRpcValue& xml )
{
YAML::Node yaml;
if( xml.getType() != XmlRpc::XmlRpcValue::TypeStruct ) { return yaml; }
XmlRpc::XmlRpcValue::iterator iter;
for( iter = xml.begin(); iter != xml.end(); iter++ )
{
std::string name = iter->first;
XmlRpc::XmlRpcValue payload = iter->second;
if( payload.getType() == XmlRpc::XmlRpcValue::TypeStruct )
{
yaml[name] = XmlToYaml( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeArray )
{
if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeBoolean ) )
{
std::vector<bool> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<bool>( payload[i]) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeInt ) )
{
std::vector<int> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<int>( payload[i]) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeDouble ) )
{
std::vector<double> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( ParseDouble( payload[i] ) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeString ) )
{
std::vector<std::string> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<std::string>( payload[i]) );
}
yaml[name] = s;
}
else
{
std::cerr << "Invalid array type." << std::endl;
}
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeBoolean )
{
yaml[name] = static_cast<bool>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeInt )
{
yaml[name] = static_cast<int>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeDouble )
{
yaml[name] = static_cast<double>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeString )
{
yaml[name] = static_cast<std::string>( payload );
}
else
{
std::cerr << "Unsupported conversion type." << std::endl;
continue;
}
}
return yaml;
}
MidemUserInteraction::MidemUserInteraction(QMainWindow *parent) :
QMainWindow(parent),
ui(new Ui::MidemUserInteraction),
quit_thread_(false),
nhp_("~"),
working_frame_("/base_link"),
gesture_detector_loader_("midem_user_interaction","hg_gesture_detector::GestureDetector")
{
ui->setupUi(this);
dynamic_reconfigure::Server<midem_user_interaction::MidemUserInteractionConfig>::CallbackType f;
f = boost::bind(&MidemUserInteraction::callbackConfig, this, _1, _2);
reconfigure_server_.setCallback(f);
arms_sub_ = nh_.subscribe("arms_msg", 1, &MidemUserInteraction::callbackArms, this);
arm_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("arm_gestures", 1);
skeletons_sub_ = nh_.subscribe("skeletons_msg", 1, &MidemUserInteraction::callbackSkeletons, this);
skeleton_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("skeleton_gestures", 1);
arms_syn_sub_.subscribe(nh_, "arms_msg", 10);
skeletons_syn_sub_.subscribe(nh_, "skeletons_msg", 10);
arm_skeleton_sync_.reset(new message_filters::Synchronizer<ArmBodyAppoxSyncPolicy>(ArmBodyAppoxSyncPolicy(10), arms_syn_sub_, skeletons_syn_sub_));
arm_skeleton_sync_->registerCallback(boost::bind(&MidemUserInteraction::callbackArmsSkelentons, this, _1, _2));
arm_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("arm_gesture_markers", 1);
skeleton_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("skeleton_gesture_markers", 1);
XmlRpc::XmlRpcValue gesture_detector;
nhp_.getParam("gesture_detector", gesture_detector);
if(gesture_detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("invalid YAML structure");
return;
}
ROS_INFO_STREAM("load " << gesture_detector.size() << " gesture detector(s)");
for(XmlRpc::XmlRpcValue::iterator it = gesture_detector.begin(); it != gesture_detector.end(); it++)
{
ROS_INFO_STREAM("detector name: " << it->first);
XmlRpc::XmlRpcValue detector;
nhp_.getParam("gesture_detector/" + it->first, detector);
if(detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("invalid YAML structure");
return;
}
if(detector.begin()->first != "type")
{
ROS_ERROR("invalid YAML structure");
return;
}
ROS_INFO_STREAM(" type: " << detector.begin()->second);
try
{
gesture_detector_map_[it->first] = gesture_detector_loader_.createInstance(detector.begin()->second);
gesture_detector_map_[it->first]->setName(it->first);
if(!gesture_detector_map_[it->first]->initialize())
{
ROS_ERROR_STREAM("Cannot initialize detector " << it->first);
gesture_detector_map_.erase(it->first);
}
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}
}
}
void JointStateTorqueSensorController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
{
// Preconditions
XmlRpc::XmlRpcValue list;
if (!nh.getParam("extra_joints", list))
{
ROS_DEBUG("No extra joints specification found.");
return;
}
if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Extra joints specification is not an array. Ignoring.");
return;
}
for(std::size_t i = 0; i < list.size(); ++i)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
"'. Ignoring.");
continue;
}
if (!list[i].hasMember("name"))
{
ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
continue;
}
const std::string name = list[i]["name"];
if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
{
ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
continue;
}
const bool has_pos = list[i].hasMember("position");
const bool has_vel = list[i].hasMember("velocity");
const bool has_eff = list[i].hasMember("effort");
const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
if (has_pos && list[i]["position"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
continue;
}
if (has_vel && list[i]["velocity"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
continue;
}
if (has_eff && list[i]["effort"].getType() != typeDouble)
{
ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
continue;
}
// State of extra joint
const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
const double eff = has_eff ? static_cast<double>(list[i]["effort"]) : 0.0;
// Add extra joints to message
msg.name.push_back(name);
msg.position.push_back(pos);
msg.velocity.push_back(vel);
msg.effort.push_back(eff);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "standalone_complexed_nodelet");
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
nodelet::Loader manager(false); // Don't bring up the manager ROS API
nodelet::V_string my_argv;
std::vector<std::string> candidate_root;
candidate_root.push_back("nodelets");
int candidate_num;
private_nh.param("candidate_num", candidate_num, 100);
for (size_t i = 0; i < candidate_num; i++) {
candidate_root.push_back((boost::format("nodelets_%lu") % i).str());
}
for (size_t i_candidate = 0; i_candidate < candidate_root.size(); i_candidate++) {
std::string root_name = candidate_root[i_candidate];
if (private_nh.hasParam(root_name)) {
XmlRpc::XmlRpcValue nodelets_values;
private_nh.param(root_name, nodelets_values, nodelets_values);
if (nodelets_values.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t i_nodelet = 0; i_nodelet < nodelets_values.size(); i_nodelet++) {
ROS_INFO("i_nodelet %lu", i_nodelet);
XmlRpc::XmlRpcValue onenodelet_param = nodelets_values[i_nodelet];
if (onenodelet_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
std::string name, type;
nodelet::M_string remappings;
if (onenodelet_param.hasMember("if") && !(bool)onenodelet_param["if"]) {
continue;
}
if (onenodelet_param.hasMember("unless") && (bool)onenodelet_param["unless"]) {
continue;
}
if (onenodelet_param.hasMember("name")) {
name = nh.resolveName((std::string)onenodelet_param["name"]);
}
else {
ROS_FATAL("element ~nodelets should have name field");
return 1;
}
if (onenodelet_param.hasMember("type")) {
type = (std::string)onenodelet_param["type"];
}
else {
ROS_FATAL("element ~nodelets should have type field");
return 1;
}
if (onenodelet_param.hasMember("remappings")) {
XmlRpc::XmlRpcValue remappings_params
= onenodelet_param["remappings"];
if (remappings_params.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t remappings_i = 0; remappings_i < remappings_params.size(); remappings_i++) {
XmlRpc::XmlRpcValue remapping_element_param = remappings_params[remappings_i];
if (remapping_element_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
if (remapping_element_param.hasMember("from") && remapping_element_param.hasMember("to")) {
std::string from = (std::string)remapping_element_param["from"];
std::string to = (std::string)remapping_element_param["to"];
if (from.size() > 0 && from[0] == '~') {
ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
from = nodelet_private_nh.resolveName(from.substr(1, from.size() - 1));
}
else {
ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
from = nodelet_nh.resolveName(from);
}
if (to.size() > 0 && to[0] == '~') {
ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
to = nodelet_private_nh.resolveName(to.substr(1, to.size() - 1));
}
else {
ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
to = nodelet_nh.resolveName(to);
}
ROS_INFO("remapping: %s => %s", from.c_str(), to.c_str());
remappings[from] = to;
}
else {
ROS_FATAL("remappings parameter requires from and to fields");
return 1;
}
}
else {
ROS_FATAL("remappings should be an array");
return 1;
}
}
}
else {
ROS_FATAL("remappings should be an array");
return 1;
}
}
if (onenodelet_param.hasMember("params")) {
if (onenodelet_param["params"].getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_FATAL("params parameter must be an array");
return 1;
}
XmlRpc::XmlRpcValue values;
for (int params_i = 0; params_i < onenodelet_param["params"].size(); ++params_i) {
XmlRpc::XmlRpcValue oneparam = onenodelet_param["params"][params_i];
if (!(oneparam.getType() == XmlRpc::XmlRpcValue::TypeStruct &&
oneparam.hasMember("name") && oneparam.hasMember("value"))) {
ROS_FATAL("each 'params' element must be a struct which contains 'name' and 'value' fields.");
return 1;
}
std::string key = oneparam["name"];
values[key] = oneparam["value"];
ROS_INFO_STREAM("setparam: " << name << "/" << key << " => " << values[key]);
}
ros::param::set(name, values);
}
// Done reading parmaeter for one nodelet
if (!manager.load(name, type, remappings, my_argv)) {
ROS_ERROR("Failed to load nodelet [%s -- %s]", name.c_str(), type.c_str());
}
else {
ROS_INFO("Succeeded to load nodelet [%s -- %s]", name.c_str(), type.c_str());
}
}
else {
ROS_FATAL("element ~nodelets should be a dictionay");
return 1;
}
}
}
else {
ROS_FATAL("~nodelets should be a list");
return 1;
}
}
}
ROS_INFO("done reading parmaeters");
std::vector<std::string> loaded_nodelets = manager.listLoadedNodelets();
ROS_INFO("loaded nodelets: %lu", loaded_nodelets.size());
for (size_t i = 0; i < loaded_nodelets.size(); i++) {
ROS_INFO("loaded nodelet: %s", loaded_nodelets[i].c_str());
}
ros::spin();
return 0;
}
bool CartesianComplianceController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle) {
using namespace XmlRpc;
this->nodeHandle = nodeHandle;
this->robotPtr = robotPtr;
ROS_INFO("Initializing interaction control for the youbot arm...\n");
// Gets all of the joint pointers from the RobotState to a joints vector
XmlRpc::XmlRpcValue jointNames;
if (!nodeHandle.getParam("joints", jointNames)) {
ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("Malformed joint specification. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i) {
XmlRpcValue &name = jointNames[i];
if (name.getType() != XmlRpcValue::TypeString) {
ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
if (jointStatePtr == NULL) {
ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
return false;
}
joints.push_back(jointStatePtr);
}
// Ensures that all the joints are calibrated.
for (unsigned int i = 0; i < joints.size(); ++i) {
if (!joints[i]->calibrated_) {
ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
return false;
}
}
// Initializing target efforts vector
targetEfforts.resize(joints.size());
// Subscribing for an input pose command
subscriber = nodeHandle.subscribe("command", 1, &CartesianComplianceController::positionCommand, this);
// Initializing 20Sim controller
init20SimController();
// Initializing twist publisher for the base
ROS_INFO("base_confghfhgtroller/command\n");
baseTwist.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist > (nodeHandle, "base_controller/command", 1));
baseTwist->lock();
subscriberOdometry = nodeHandle.subscribe("base_odometry/odometry", 1, &CartesianComplianceController::odometryCommand, this);
// Initializing debug output
debugInfo->init();
return true;
}
bool JointPositionController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle)
{
using namespace XmlRpc;
this->nodeHandle = nodeHandle;
this->robotPtr = robotPtr;
ROS_DEBUG("Initializing joint position control...\n");
// Gets all of the joint pointers from the RobotState to a joints vector
XmlRpc::XmlRpcValue jointNames;
if (!nodeHandle.getParam("joints", jointNames))
{
ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Malformed joint specification. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i)
{
XmlRpcValue &name = jointNames[i];
if (name.getType() != XmlRpcValue::TypeString)
{
ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", nodeHandle.getNamespace().c_str());
return false;
}
pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
if (jointStatePtr == NULL)
{
ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
return false;
}
joints.push_back(jointStatePtr);
}
// Ensures that all the joints are calibrated.
for (unsigned int i = 0; i < joints.size(); ++i)
{
if (!joints[i]->calibrated_)
{
ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
return false;
}
}
// Initializing targetVelocities vector
targetPositions.resize(joints.size());
// Sets up pid controllers for all of the joints from yaml file
std::string gainsNS;
if (!nodeHandle.getParam("gains", gainsNS))
gainsNS = nodeHandle.getNamespace() + "/gains";
ROS_DEBUG("gains: %s\n", gainsNS.c_str());
pids.resize(joints.size());
ROS_DEBUG("joints.size() = %d \n", joints.size());
for (unsigned int i = 0; i < joints.size(); ++i)
{
ROS_DEBUG("processing %s/%s \n", gainsNS.c_str(), joints[i]->joint_->name.c_str());
if (!pids[i].init(ros::NodeHandle(gainsNS + "/" + joints[i]->joint_->name)))
{
ROS_ERROR("Can't setup PID for the joint %s. (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
return false;
}
double p, i, d, i_max, i_min;
pids[i].getGains(p, i, d, i_max, i_min);
ROS_DEBUG("PID for joint %s: p=%f, i=%f, d=%f, i_max=%f, i_min=%f\n", joints[i]->joint_->name.c_str(), p, i, d, i_max, i_min);
}
subscriber = nodeHandle.subscribe("position_command", 1, &JointPositionController::positionCommand, this);
return true;
}
bool initialize()
{
XmlRpc::XmlRpcValue val;
std::vector<std::string> static_joints;
if (private_nh_.getParam("static_joints", val))
{
if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("static_joints parameter is not a list");
return false;
}
for (int i = 0; i < val.size(); ++i)
{
static_joints.push_back(static_cast<std::string>(val[i]));
}
}
std::vector<std::string> controller_names;
if (private_nh_.getParam("controllers", val))
{
if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("controllers parameter is not a list");
return false;
}
for (int i = 0; i < val.size(); ++i)
{
controller_names.push_back(static_cast<std::string>(val[i]));
}
}
int num_static = static_joints.size();
int num_controllers = controller_names.size();
int num_total = num_static + num_controllers;
msg_.name.resize(num_total);
msg_.position.resize(num_total);
msg_.velocity.resize(num_total);
msg_.effort.resize(num_total);
for (int i = 0; i < num_static; ++i)
{
msg_.name[i] = static_joints[i];
msg_.position[i] = 0.0;
msg_.velocity[i] = 0.0;
msg_.effort[i] = 0.0;
}
controller_state_subs_.resize(num_controllers);
for (int i = 0; i < num_controllers; ++i)
{
controller_state_subs_[i] =
nh_.subscribe<dynamixel_hardware_interface::JointState>(controller_names[i] + "/state", 100,
boost::bind(&JointStateAggregator::processControllerState, this, _1, i+num_static));
}
for (int i = 0; i < num_controllers; ++i)
{
ros::topic::waitForMessage<dynamixel_hardware_interface::JointState>(controller_names[i] + "/state");
}
joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
return true;
}
bool GridMapVisualization::readParameters()
{
nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map"));
double activityCheckRate;
nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0);
activityCheckDuration_.fromSec(1.0 / activityCheckRate);
ROS_ASSERT(!activityCheckDuration_.isZero());
// Configure the visualizations from a configuration stored on the parameter server.
XmlRpc::XmlRpcValue config;
if (!nodeHandle_.getParam(visualizationsParameter_, config)) {
ROS_WARN(
"Could not load the visualizations configuration from parameter %s,are you sure it"
"was pushed to the parameter server? Assuming that you meant to leave it empty.",
visualizationsParameter_.c_str());
return false;
}
// Verify proper naming and structure,
if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d",
visualizationsParameter_.c_str(), config.getType());
ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str());
return false;
}
// Iterate over all visualizations (may be just one),
for (unsigned int i = 0; i < config.size(); ++i) {
if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
visualizationsParameter_.c_str(), config[i].getType());
return false;
} else if (!config[i].hasMember("type")) {
ROS_ERROR("%s: Could not add a visualization because no type was given",
visualizationsParameter_.c_str());
return false;
} else if (!config[i].hasMember("name")) {
ROS_ERROR("%s: Could not add a visualization because no name was given",
visualizationsParameter_.c_str());
return false;
} else {
//Check for name collisions within the list itself.
for (int j = i + 1; j < config.size(); ++j) {
if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
visualizationsParameter_.c_str(), config[j].getType());
return false;
}
if (!config[j].hasMember("name")
|| config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
|| config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d",
visualizationsParameter_.c_str(), config[i].getType(), config[j].getType());
return false;
}
std::string namei = config[i]["name"];
std::string namej = config[j]["name"];
if (namei == namej) {
ROS_ERROR("%s: A visualization with the name '%s' already exists.",
visualizationsParameter_.c_str(), namei.c_str());
return false;
}
}
}
// Make sure the visualization has a valid type.
if (!factory_.isValidType(config[i]["type"])) {
ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str());
return false;
}
}
for (int i = 0; i < config.size(); ++i) {
std::string type = config[i]["type"];
std::string name = config[i]["name"];
auto visualization = factory_.getInstance(type, name);
visualization->readParameters(config[i]);
visualizations_.push_back(visualization);
ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.",
visualizationsParameter_.c_str(), type.c_str(), name.c_str());
}
return true;
}
ROS_ASSERT(camera_xml["name"].getType() == XmlRpc::XmlRpcValue::TypeString);
ROS_ASSERT(camera_xml["topic"].getType() == XmlRpc::XmlRpcValue::TypeString);
ROS_ASSERT(camera_xml["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);
camera_definitions.push_back(CameraDefinition(static_cast<std::string>(camera_xml["name"]),
static_cast<std::string>(camera_xml["topic"]),
static_cast<std::string>(camera_xml["frame_id"])));
}
ROS_INFO("Loaded cameras:");
BOOST_FOREACH(const CameraDefinition& camera, camera_definitions) {
ROS_INFO_STREAM("\t" << camera.name << ": topic: " << camera.topic << ", frame_id: " << camera.frame_id);
}
XmlRpc::XmlRpcValue configs_xml;
pnh.getParam("configs", configs_xml);
ROS_ASSERT(configs_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int i = 0; i < configs_xml.size(); ++i) {
XmlRpc::XmlRpcValue config_xml = configs_xml[i];
ROS_ASSERT(config_xml.getType() == XmlRpc::XmlRpcValue::TypeStruct);
ROS_ASSERT(config_xml["width"].getType() == XmlRpc::XmlRpcValue::TypeInt);
ROS_ASSERT(config_xml["height"].getType() == XmlRpc::XmlRpcValue::TypeInt);
ROS_ASSERT(config_xml["fps"].getType() == XmlRpc::XmlRpcValue::TypeInt);
camera_configs_.push_back(CameraConfiguration(static_cast<int>(config_xml["width"]),
static_cast<int>(config_xml["height"]),
static_cast<int>(config_xml["fps"])));
}
ROS_INFO("Loaded camera configurations:");
BOOST_FOREACH(const CameraConfiguration& config, camera_configs_) {
ROS_INFO_STREAM("\twidth: " << config.width << ", height: " << config.height << ", fps: " << config.fps);
}
void EventsGenerator::parseExploitationMatches(const ros::NodeHandle& n)
{
if (!n.hasParam("exploitation_matches")) {
return;
}
ROS_INFO("Parsing parameters for exploitation matches ...");
XmlRpc::XmlRpcValue node;
n.getParam("exploitation_matches", node);
if (node.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("'exploitation_matches' is not an array, ignoring.");
return;
}
for (int i = 0; i != node.size(); ++i) {
XmlRpc::XmlRpcValue& topic_node = node[i];
if (!topic_node.hasMember("topic_name")) {
ROS_ERROR("Topic node has no topic_name, skipping.");
continue;
}
if (!topic_node.hasMember("matches")) {
ROS_ERROR("Topic node has no matches, skipping.");
continue;
}
const std::string& topic_name = topic_node["topic_name"];
XmlRpc::XmlRpcValue& matches_node = topic_node["matches"];
if (matches_node.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("Matches for '%s' is not an array, skipping.",
topic_name.c_str());
continue;
}
std::vector<hbba_msgs::ExploitationMatch> matches;
matches.reserve(matches_node.size());
for (int j = 0; j < matches_node.size(); ++j) {
XmlRpc::XmlRpcValue& elem = matches_node[j];
if (elem.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("Element for '%s' is not a proper struct, skipping.",
topic_name.c_str());
continue;
}
if (!elem.hasMember("priority")) {
ROS_ERROR("Element for '%s' does not contain a priority, "
"skipping.",
topic_name.c_str());
continue;
}
int priority = static_cast<int>(elem["priority"]);
if (!elem.hasMember("desire_type")) {
ROS_ERROR("Element for priority %i of '%s' does not contain a "
"desire type, skipping.",
priority,
topic_name.c_str());
continue;
}
XmlRpc::XmlRpcValue& desire_types = elem["desire_type"];
if (desire_types.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("Desire type for priority %i of '%s' is not an "
"array, skipping.",
priority,
topic_name.c_str());
continue;
}
hbba_msgs::ExploitationMatch match;
match.priority = priority;
match.classes.reserve(desire_types.size());
for (int k = 0; k < desire_types.size(); ++k) {
XmlRpc::XmlRpcValue& dtype = desire_types[k];
if (dtype.getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("Desire type in '%s' is not a string, skipping.",
topic_name.c_str());
continue;
}
match.classes.push_back(dtype);
}
matches.push_back(match);
}
if (matches.size()) {
cem(topic_name, matches);
}
}
}
bool ChainParser::parseChain(XmlRpc::XmlRpcValue& chain_description, Tree tree, urdf::Model& robot_model,
std::map<std::string, unsigned int>& joint_name_to_index,
std::vector<std::string>& index_to_joint_name,
std::vector<double>& q_min, std::vector<double>& q_max) {
ros::NodeHandle n("~");
std::string ns = n.getNamespace();
std::cout << chain_description << std::endl;
if (chain_description.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("Chain description should be a struct containing 'root' and 'tip'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
XmlRpc::XmlRpcValue& v_root_link = chain_description["root"];
if (v_root_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("Chain description for does not contain 'root'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
std::string root_link_name = (std::string)v_root_link;
XmlRpc::XmlRpcValue& v_tip_link = chain_description["tip"];
if (v_tip_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("Chain description for does not contain 'tip'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
std::string tip_link_name = (std::string)v_tip_link;
Chain* chain = new Chain();
ROS_INFO("Looking for chain from %s to %s", root_link_name.c_str(), tip_link_name.c_str());
if (!tree.kdl_tree_.getChain(root_link_name, tip_link_name, chain->kdl_chain_)) {
// if (!tree.kdl_tree_.getChain("base", tip_link_name, chain->kdl_chain_)) {
ROS_FATAL("Could not initialize chain object");
return false;
}
for(unsigned int i = 0; i < chain->kdl_chain_.getNrOfSegments(); ++i) {
const KDL::Segment& segment = chain->kdl_chain_.getSegment(i);
const KDL::Joint& joint = segment.getJoint();
if (joint.getType() != KDL::Joint::None)
{
//cout << "Segment: " << segment.getName() << endl;
//cout << "Joint: " << joint.getName() << endl;
unsigned int full_joint_index = 0;
std::map<std::string, unsigned int>::iterator it_joint = joint_name_to_index.find(joint.getName());
if (it_joint == joint_name_to_index.end()) {
// joint name is not yet in map, so give it a new fresh index and add it to the map
full_joint_index = joint_name_to_index.size();
//cout << " new joint, gets index " << full_joint_index << endl;
//cout << " type: " << joint.getTypeName() << endl;
joint_name_to_index[joint.getName()] = full_joint_index;
index_to_joint_name.push_back(joint.getName());
//cout << " Lower limit: " << robot_model.getJoint(joint.getName())->limits->lower << endl;
//cout << " Upper limit: " << robot_model.getJoint(joint.getName())->limits->upper << endl;
// determine joint limits from URDF
q_min.push_back(robot_model.getJoint(joint.getName())->limits->lower);
q_max.push_back(robot_model.getJoint(joint.getName())->limits->upper);
} else {
// joint name already in the map, so look-up its index
full_joint_index = it_joint->second;
//cout << " existing joint, has index: " << full_joint_index << endl;
}
}
}
return true;
}
/*!
* \brief Grabs ft rosparams from a given node hande namespace
*
* The force/torque parameters consist of
* 6x ADC offset values
* 6x6 gain matrix as 6-elment array of 6-element arrays of doubles
*
* \return True, if there are no problems.
*/
bool FTParamsInternal::getRosParams(ros::NodeHandle nh)
{
if (!nh.hasParam("ft_params"))
{
ROS_WARN("'ft_params' not available for force/torque sensor in namespace '%s'",
nh.getNamespace().c_str());
return false;
}
XmlRpc::XmlRpcValue params;
nh.getParam("ft_params", params);
if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("expected ft_params to be struct type");
return false;
}
if (!getDoubleArray(params, "offsets", offsets_, 6))
{
return false;
}
if (!getDoubleArray(params, "gains", gains_, 6))
{
return false;
}
XmlRpc::XmlRpcValue coeff_matrix = params["calibration_coeff"];
if (coeff_matrix.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Expected FT param 'calibration_coeff' to be list type");
return false;
}
if (coeff_matrix.size() != 6)
{
ROS_ERROR("Expected FT param 'calibration_coeff' to have 6 elements");
return false;
}
for (int i=0; i<6; ++i)
{
XmlRpc::XmlRpcValue coeff_row = coeff_matrix[i];
if (coeff_row.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Expected FT param calibration_coeff[%d] to be list type", i);
return false;
}
if (coeff_row.size() != 6)
{
ROS_ERROR("Expected FT param calibration_coeff[%d] to have 6 elements", i);
return false;
}
for (int j=0; j<6; ++j)
{
if (coeff_row[j].getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
ROS_ERROR("Expected FT param calibration_coeff[%d,%d] to be floating point type", i,j);
return false;
} else {
calibration_coeff(i,j) = static_cast<double>(coeff_row[j]);
}
}
}
return true;
}
RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id)
{
// ROS_INFO("ON_ACTIVATED");
std::string config_name;
config_name = nh.resolveName("controller_configuration");
if (nh.hasParam(config_name))
{
XmlRpc::XmlRpcValue param_val;
nh.getParam(config_name, param_val);
if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
for (int i = 0; i < param_val.size(); i++)
{
if (param_val[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
{
XmlRpc::XmlRpcValue gval = param_val[i]["group_name"];
XmlRpc::XmlRpcValue cval = param_val[i]["controller_name"];
XmlRpc::XmlRpcValue lval = param_val[i]["joint_list"];
std::string gname = gval;
std::string cname = cval;
std::vector<std::string> jlst;
if (lval.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
for (int s = 0; s < lval.size(); s++)
{
jlst.push_back(lval[s]);
}
}
if (gname.length() == 0 && cname.length() > 0)
{
gname = cname;
}
if (gname.length() > 0 && cname.length() == 0)
{
cname = gname;
}
if (gname.length() > 0 && cname.length() > 0 && jlst.size() > 0)
{
std::stringstream ss;
for (size_t s = 0; s < jlst.size(); s++)
{
ss << " " << jlst[s];
}
ROS_INFO_STREAM("ADD_GROUP: " << gname << " (" << cname << ")");
ROS_INFO_STREAM(" JOINT:" << ss.str());
jointTrajectoryActionObj::Ptr tmp(new jointTrajectoryActionObj(this, cval, gval, jlst));
trajectory_actions.push_back(tmp);
}
}
}
}
else
{
ROS_WARN_STREAM("param: " << config_name << ", configuration is not an array.");
}
}
else
{
ROS_WARN_STREAM("param: " << config_name << ", param does not exist.");
}
return RTC::RTC_OK;
}
SensorConfiguration::SensorConfiguration(std::string name,
XmlRpc::XmlRpcValue& conf) :
name_(name), covariance_(NULL) {
if (conf.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_FATAL("sensor %s: bad sensor configuration, expecting structure.",
name.c_str());
ROS_BREAK();
}
// set sensor type
if (!conf.hasMember("type")
|| conf["type"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'type' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
std::string type = static_cast<std::string &>(conf["type"]);
if (type == "AbsolutePosition") {
type_ = ROAMestimation::AbsolutePosition;
} else if (type == "LinearVelocity") {
type_ = ROAMestimation::LinearVelocity;
} else if (type == "AngularVelocity") {
type_ = ROAMestimation::AngularVelocity;
} else if (type == "LinearAcceleration") {
type_ = ROAMestimation::LinearAcceleration;
} else if (type == "AckermannOdometer") {
type_ = ROAMestimation::AckermannOdometer;
} else if (type == "TriskarOdometer") {
type_ = ROAMestimation::TriskarOdometer;
} else if (type == "DifferentialDriveOdometer") {
type_ = ROAMestimation::DifferentialDriveOdometer;
} else if (type == "GenericOdometer") {
type_ = ROAMestimation::GenericOdometer;
} else if (type == "VectorField") {
type_ = ROAMestimation::VectorField;
} else if (type == "VectorFieldAsCompass") {
type_ = ROAMestimation::VectorFieldAsCompass;
} else if (type == "FixedFeaturePosition") {
type_ = ROAMestimation::FixedFeaturePosition;
} else if (type == "FixedFeaturePose") {
type_ = ROAMestimation::FixedFeaturePose;
} else if (type == "PlanarConstraint") {
type_ = ROAMestimation::PlanarConstraint;
}else if (type == "IMUHandler") {
type_ = ROAMestimation::IMUHandler;
}else {
ROS_FATAL("sensor %s: unknown sensor type '%s'", name.c_str(),
type.c_str());
ROS_BREAK();
}
// set master
if (!conf.hasMember("is_master")
|| conf["is_master"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'is_master' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
is_master_ = (bool &) conf["is_master"];
// set frame id
if (!conf.hasMember("frame_id")
|| conf["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'frame_id' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
frame_id_ = static_cast<std::string &>(conf["frame_id"]);
// set topic
if (!conf.hasMember("topic")
|| conf["topic"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'topic' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
topic_ = static_cast<std::string &>(conf["topic"]);
// set topic type
if (!conf.hasMember("topic_type")
|| conf["topic_type"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'topic_type' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
topic_type_ = static_cast<std::string &>(conf["topic_type"]);
// set use header tstamp
if (!conf.hasMember("use_header_stamp")
|| conf["use_header_stamp"].getType()
!= XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'use_header_stamp' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
use_header_stamp_ = static_cast<bool>(conf["use_header_stamp"]);
// set whatever this sensor has static covariance matrix
if (!conf.hasMember("static_covariance")
|| conf["static_covariance"].getType()
!= XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'static_covariance' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
static_covariance_ = static_cast<bool>(conf["static_covariance"]);
if (static_covariance_ == true) {
if (conf.hasMember("covariance")) {
covariance_ = new Eigen::MatrixXd();
if (conf["covariance"].getType() != XmlRpc::XmlRpcValue::TypeArray
|| !XmlRpcValueToEigenXd(conf["covariance"], covariance_)) {
ROS_FATAL("sensor %s: malformed 'covariance' field",
name.c_str());
ROS_BREAK();
}
} else {
ROS_INFO("sensor %s: ignoring static covariance", name.c_str());
}
}
// load parameters
if (conf.hasMember("parameters")) {
XmlRpc::XmlRpcValue & params = conf["parameters"];
if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_FATAL("sensor %s: malformed parameters section, expecting structure",
name.c_str());
ROS_BREAK();
}
XmlRpc::XmlRpcValue::iterator it;
for (it = params.begin(); it != params.end(); ++it) {
Eigen::VectorXd *tmp = new Eigen::VectorXd;
if (it->second.getType() != XmlRpc::XmlRpcValue::TypeArray
|| !XmlRpcValueToEigenXd(it->second, tmp)) {
ROS_FATAL("sensor %s: malformed array at parameter '%s'", name.c_str(),
it->first.c_str());
ROS_BREAK();
}
// this check is needed since at the present we decide parameter type
// depending on its size. 4 -> Quaternion, 3-> Euclidean3D, etc
if (tmp->rows() != 1 && tmp->rows() != 3 && tmp->rows() != 4
&& tmp->rows() != 9) {
ROS_FATAL("sensor %s: parameter '%s' has a wrong size: %ld",
name.c_str(), it->first.c_str(), tmp->rows());
ROS_BREAK();
}
parameters_[it->first] = tmp;
}
}
}
int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/GBS
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
argv[4]=NUMBER_OF_ROBOTS //this is only necessary to automatically define G2
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
//printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
/** Define G1 and G2 **/
double G1 = 0.1;
int NUMBER_OF_ROBOTS = atoi(argv[4]);
//default:
double G2 = 100.0;
double edge_min = 1.0;
if ( strcmp (graph_file,"maps/grid/grid.graph") == 0 ){
if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;}
if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;}
if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;}
if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;}
if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;}
if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;}
}else if (strcmp (graph_file,"maps/example/example.graph") == 0 ) {
if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;}
if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;}
if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;}
edge_min = 20.0;
}else if (strcmp (graph_file,"maps/cumberland/cumberland.graph") == 0) {
if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;}
if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;}
if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;}
edge_min = 50.0;
}
printf("G1 = %f, G2 = %f\n", G1, G2);
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_reactive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("initial position: x = %f, y = %f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("initial vertex = %d\n\n",current_vertex);
//INITIALIZE tab_intention:
int tab_intention[NUMBER_OF_ROBOTS];
for (i=0; i<NUMBER_OF_ROBOTS; i++){
tab_intention[i] = -1;
}
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados de "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
/* Define Goal */
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex
}
}
interference = false;
ResendGoal = false;
goal_complete = true;
double now;
while(1){
if(goal_complete){
if(next_vertex>-1){
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
} */
}
//devolver proximo vertex tendo em conta apenas as idlenesses;
next_vertex = (int) state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min);
//printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
/** SEND INTENTION **/
send_intention(next_vertex);
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat;
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.waitForResult();
goal_complete = false;
}else{
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if (arrived && NUMBER_OF_ROBOTS>1){ //a different robot arrived at a vertex: update idleness table and keep track of last vertices positions of other robots.
//printf("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == vertex_arrived){
//actualizar last_visit[dimension]
last_visit[vertex_arrived] = now;
}
//actualizar instantaneous_idleness[dimension]
instantaneous_idleness[i]= now - last_visit[i];
}
//Show Idleness Table:
// for (i=0; i<dimension; i++){
// printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
// }
arrived = false;
}
if (intention && NUMBER_OF_ROBOTS>1){
tab_intention[robot_intention] = vertex_intention;
//printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
intention = false;
}
if(end_simulation){
return 0;
}
}
}
return 0;
}
int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1);
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
uint j;
/* Output Graph Data */
for (i=0;i<dimension;i++){
printf ("ID= %u\n", vertex_web[i].id);
printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
for (j=0;j<vertex_web[i].num_neigh; j++){
printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
}
printf("\n");
}
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_cognitive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("x=%f, y=%f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("v=%d\n",current_vertex);
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados da propria "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
// Define Goal
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base"
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string do move base = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
//instantaneous idleness and last visit initialized with zeros:
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away
}
}
bool inpath = false;
uint path [dimension];
uint elem_s_path=0, i_path=0;
interference = false;
ResendGoal = false;
goal_complete = true;
// printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
while(1){
if (goal_complete){
if (i_path>0){ //nao faz update no inicio
//Update Idleness Table:
double now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
}
*/
}
if(inpath){
//The robot is on its way to a global objective -> get NEXT_VERTEX from its path:
i_path++; //desde que nao passe o tamanho do path
if (i_path<elem_s_path){
next_vertex=path[i_path];
}else{
inpath = false;
}
}
if (!inpath){
elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path);
/* printf("Path: ");
for (i=0;i<elem_s_path;i++){
if (i==elem_s_path-1){
printf("%u.\n",path[i]);
}else{
printf("%u, ",path[i]);
}
}
*/
//we have the path and the number of elements in the path
i_path=1;
next_vertex = path[i_path];
inpath = true;
// printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
}
if (inpath){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.sendGoal(goal);
//ac.waitForResult();
}
goal_complete = false; //garantir q n volta a entrar a seguir aqui
// printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}else{ //goal not complete (active)
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if(end_simulation){
return 0;
}
// printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
}
// return 0;
}
void init( ros::NodeHandle &nh) {
/*********************
** Ros Comms
**********************/
server_add_listener = nh.advertiseService("add_listener", &ZeroconfNode::add_listener, this);
server_remove_listener = nh.advertiseService("remove_listener", &ZeroconfNode::remove_listener, this);
server_add_service = nh.advertiseService("add_service", &ZeroconfNode::add_service, this);
server_remove_service = nh.advertiseService("remove_service", &ZeroconfNode::remove_service, this);
server_list_discovered_services = nh.advertiseService("list_discovered_services", &ZeroconfNode::list_discovered_services, this);
server_list_published_services = nh.advertiseService("list_published_services", &ZeroconfNode::list_published_services, this);
pub_new_connections = nh.advertise<DiscoveredService>("new_connections",10);
pub_lost_connections = nh.advertise<DiscoveredService>("lost_connections",10);
/*********************
** Signals
**********************/
// The callback functions use the publishers, so they must come after.
connection_signal_cb new_connection_signal = std::bind1st(std::mem_fun(&ZeroconfNode::new_connections), this);
connection_signal_cb old_connection_signal = std::bind1st(std::mem_fun(&ZeroconfNode::lost_connections), this);
zeroconf.connect_signal_callbacks(new_connection_signal, old_connection_signal);
/*********************
** Parameters
**********************/
ros::NodeHandle local_nh("~");
// listeners
XmlRpc::XmlRpcValue value;
XmlRpc::XmlRpcValue::iterator iter;
if ( local_nh.getParam("listeners", value) ) {
if ( value.getType() != XmlRpc::XmlRpcValue::TypeArray ) {
ROS_ERROR_STREAM("Zeroconf: param variable 'listeners' has malformed type, should be of type array");
} else {
for ( int i = 0; i < value.size(); ++i ) {
zeroconf.add_listener(value[i]);
}
}
}
// services
XmlRpc::XmlRpcValue value_services;
if ( local_nh.getParam("services", value_services) ) {
if ( value_services.getType() != XmlRpc::XmlRpcValue::TypeArray ) {
ROS_ERROR_STREAM("Zeroconf: param variable 'services' has malformed type, should be of type array");
} else {
for ( int i = 0; i < value_services.size(); ++i ) {
XmlRpc::XmlRpcValue value_service = value_services[i];
if ( value_service.getType() != XmlRpc::XmlRpcValue::TypeStruct ) {
ROS_ERROR_STREAM("Zeroconf: " << i << "th element of param variable 'services' has malformed type, should be of type struct");
break;
}
zeroconf_comms::PublishedService service;
XmlRpc::XmlRpcValue value_name = value_service["name"];
if ( value_name.getType() != XmlRpc::XmlRpcValue::TypeString ) {
ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['name'] has malformed type, should be of type string");
break;
}
service.name = std::string(value_name);
if ( service.name == "Ros Master" ) {
// add some special ros magic to easily identify master's.
service.name = service.name + " (" + ros::master::getHost() + ")";
}
XmlRpc::XmlRpcValue value_type = value_service["type"];
if ( value_type.getType() != XmlRpc::XmlRpcValue::TypeString ) {
ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['type'] has malformed type, should be of type string");
break;
}
service.type = std::string(value_type);
XmlRpc::XmlRpcValue value_domain = value_service["domain"];
if ( value_domain.getType() != XmlRpc::XmlRpcValue::TypeString ) {
ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['domain'] has malformed type, should be of type string");
break;
}
service.domain = std::string(value_domain);
XmlRpc::XmlRpcValue value_port = value_service["port"];
if ( value_port.getType() == XmlRpc::XmlRpcValue::TypeInvalid ) {
service.port = ros::master::getPort();
// wasn't set, so default to ros master's port
ROS_WARN_STREAM("Zeroconf: services[" << service.name << "]['port'] wasn't set, default to ros master's port [" << service.port << "]");
} else if ( value_port.getType() != XmlRpc::XmlRpcValue::TypeInt ) {
ROS_ERROR_STREAM("Zeroconf: services[" << service.name << "]['port'] has malformed type, should be of type int");
break;
} else {
service.port = value_port;
}
XmlRpc::XmlRpcValue value_description = value_service["description"];
if ( value_description.getType() != XmlRpc::XmlRpcValue::TypeString ) {
service.description = "";
} else {
service.description = std::string(value_description);
}
zeroconf.add_service(service);
}
}
}
}
void loadStaticScene(ros::NodeHandle& node_handle,
planning_scene::PlanningScenePtr& planning_scene,
robot_model::RobotModelPtr& robot_model)
{
std::string environment_file;
std::vector<double> environment_position;
node_handle.param<std::string>("/itomp_planner/environment_model",
environment_file, "");
if (!environment_file.empty())
{
environment_position.resize(3, 0);
if (node_handle.hasParam("/itomp_planner/environment_model_position"))
{
XmlRpc::XmlRpcValue segment;
node_handle.getParam("/itomp_planner/environment_model_position",
segment);
if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
int size = segment.size();
for (int i = 0; i < size; ++i)
{
double value = segment[i];
environment_position[i] = value;
}
}
}
// Collision object
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = robot_model->getModelFrame();
collision_object.id = "environment";
geometry_msgs::Pose pose;
pose.position.x = environment_position[0];
pose.position.y = environment_position[1];
pose.position.z = environment_position[2];
ROS_INFO(
"Env col pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
shapes::Mesh* shape = shapes::createMeshFromResource(environment_file);
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(shape, mesh_msg);
shape_msgs::Mesh mesh = boost::get<shape_msgs::Mesh>(mesh_msg);
collision_object.meshes.push_back(mesh);
collision_object.mesh_poses.push_back(pose);
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 2.0;
primitive.dimensions[1] = 1.0;
primitive.dimensions[2] = 1.0;
pose.position.x = 0;
pose.position.y = 6.0;
pose.position.z = -0.45;
collision_object.primitive_poses.push_back(pose);
collision_object.primitives.push_back(primitive);
collision_object.operation = collision_object.ADD;
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(collision_object);
planning_scene_msg.is_diff = true;
planning_scene->setPlanningSceneDiffMsg(planning_scene_msg);
}
}
int main(int argc, char** argv)
{
bool desired_mode_full=false; //if false go in safe_mode
bool ir_warning_;
bool bumper_warning_;
ros::init(argc, argv, "roomba560_node");
ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
double last_x, last_y, last_yaw;
double vel_x, vel_y, vel_yaw;
double dt;
float last_charge = 0.0;
int time_remaining = -1;
ros::NodeHandle n;
ros::NodeHandle pn("~");
std::string base_name;
int id;
pn.param<std::string>("base_name_", base_name, "iRobot_");
pn.param<int>("id_", id, 0);
std_msgs::String my_namespace;
my_namespace.data = base_name + std::to_string(id);
bool publish_name;
ros::Publisher listpub;
pn.param<bool>("publish_name_", publish_name, false);
if(publish_name)
{
listpub = n.advertise<std_msgs::String>("/agent_list", 50);
}
int name_count=0;
pn.param<std::string>("port_", port, "/dev/ttyUSB0");
std::string base_frame_id;
std::string odom_frame_id;
pn.param<std::string>("base_frame_id", base_frame_id, my_namespace.data + "/base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, my_namespace.data + "/odom");
bool publishTf;
pn.param<bool>("publishTf", publishTf, true);
Eigen::MatrixXd poseCovariance(6,6);
Eigen::MatrixXd twistCovariance(6,6);
bool pose_cov_mat = false;
bool twist_cov_mat = false;
std::vector<double> pose_covariance_matrix;
std::vector<double> twist_covariance_matrix;
XmlRpc::XmlRpcValue poseCovarConfig;
XmlRpc::XmlRpcValue twistCovarConfig;
if (pn.hasParam("poseCovariance"))
{
try
{
pn.getParam("poseCovariance", poseCovarConfig);
ROS_ASSERT(poseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
int matSize = poseCovariance.rows();
if (poseCovarConfig.size() != matSize * matSize)
{
ROS_WARN_STREAM("Pose_covariance matrix should have " << matSize * matSize << " values.");
}
else
{
for (int i = 0; i < matSize; i++)
{
for (int j = 0; j < matSize; j++)
{
std::ostringstream ostr;
ostr << poseCovarConfig[matSize * i + j];
std::istringstream istr(ostr.str());
istr >> poseCovariance(i, j);
pose_covariance_matrix.push_back(poseCovariance(i,j));
}
}
pose_cov_mat = true;
}
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for pose_covariance (type: " << poseCovarConfig.getType() << ")");
}
}
if (pn.hasParam("twistCovariance"))
{
try
{
pn.getParam("twistCovariance", twistCovarConfig);
ROS_ASSERT(twistCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
int matSize = twistCovariance.rows();
if (twistCovarConfig.size() != matSize * matSize)
{
ROS_WARN_STREAM("Twist_covariance matrix should have " << matSize * matSize << " values.");
}
else
{
for (int i = 0; i < matSize; i++)
{
for (int j = 0; j < matSize; j++)
{
std::ostringstream ostr;
ostr << twistCovarConfig[matSize * i + j];
std::istringstream istr(ostr.str());
istr >> twistCovariance(i, j);
twist_covariance_matrix.push_back(twistCovariance(i,j));
}
}
twist_cov_mat = true;
}
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for twist_covariance (type: " << poseCovarConfig.getType() << ")");
}
}
roomba = new irobot::OpenInterface(port.c_str());
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("battery", 50);
ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("bumper", 50);
ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("buttons", 50);
ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("cliff", 50);
ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("ir_bumper", 50);
ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("ir_character", 50);
ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("wheel_drop", 50);
tf::TransformBroadcaster tf_broadcaster;
ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmdVelReceived);
ros::Subscriber leds_sub = n.subscribe<irobotcreate2::Leds>("leds", 1, ledsReceived);
ros::Subscriber digitleds_sub = n.subscribe<irobotcreate2::DigitLeds>("digit_leds", 1, digitLedsReceived);
ros::Subscriber song_sub = n.subscribe<irobotcreate2::Song>("song", 1, songReceived);
ros::Subscriber playsong_sub = n.subscribe<irobotcreate2::PlaySong>("play_song", 1, playSongReceived);
ros::Subscriber mode_sub = n.subscribe<std_msgs::String>("mode", 1, cmdModeReceived);
ros::Subscriber special_sub = n.subscribe<std_msgs::String>("special", 1, cmdSpecialReceived);
irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100};
roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE);
if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
else
{
ROS_FATAL("Could not connect to Roomba.");
ROS_BREAK();
}
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
bool first_loop=true;
while(roomba->getSensorPackets(100) == -1 && ros::ok())
{
usleep(100);
ROS_INFO_STREAM("Waiting for roomba sensors");
}
roomba->calculateOdometry();
roomba->resetOdometry();
ros::Rate r(50.0);
while(n.ok())
{
if(publish_name)
{
name_count++;
if(name_count >= 10)
{
listpub.publish(my_namespace);
name_count=0;
}
}
ir_warning_ = false;
bumper_warning_ = false;
current_time = ros::Time::now();
last_x = roomba->odometry_x_;
last_y = roomba->odometry_y_;
last_yaw = roomba->odometry_yaw_;
if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets.");
else roomba->calculateOdometry();
/* OLD CODE
* dt = (current_time - last_time).toSec();
vel_x = (roomba->odometry_x_ - last_x)/dt;
vel_y = (roomba->odometry_y_ - last_y)/dt;
vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;*/
vel_x = roomba->new_odometry_.getLinear();
vel_y = 0.0;
vel_yaw = roomba->new_odometry_.getAngular();
// ******************************************************************************************
//first, we'll publish the transforms over tf
if(publishTf)
{
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = roomba->odometry_x_;
odom_trans.transform.translation.y = roomba->odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
tf_broadcaster.sendTransform(odom_trans);
}
//TODO: Finish brodcasting the tf for all the ir sensors on the Roomba
/*geometry_msgs::TransformStamped cliff_left_trans;
cliff_left_trans.header.stamp = current_time;
cliff_left_trans.header.frame_id = "base_link";
cliff_left_trans.child_frame_id = "base_cliff_left";
cliff_left_trans.transform.translation.x = 0.0;
cliff_left_trans.transform.translation.y = 0.0;
cliff_left_trans.transform.translation.z = 0.0;
cliff_left_trans.transform.rotation = ;
tf_broadcaster.sendTransform(cliff_left_trans); */
// ******************************************************************************************
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = odom_frame_id;
//set the position
odom.pose.pose.position.x = roomba->odometry_x_;
odom.pose.pose.position.y = roomba->odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
if(pose_cov_mat)
for(int i = 0; i < pose_covariance_matrix.size(); i++)
odom.pose.covariance[i] = pose_covariance_matrix[i];
//set the velocity
odom.child_frame_id = base_frame_id;
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
if(twist_cov_mat)
for(int i = 0; i < pose_covariance_matrix.size(); i++)
odom.twist.covariance[i] = twist_covariance_matrix[i];
//publish the message
odom_pub.publish(odom);
// ******************************************************************************************
//publish battery
irobotcreate2::Battery battery;
battery.header.stamp = current_time;
battery.power_cord = roomba->power_cord_;
battery.dock = roomba->dock_;
battery.level = 100.0*(roomba->charge_/roomba->capacity_);
if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60;
last_charge = roomba->charge_;
battery.time_remaining = time_remaining;
battery_pub.publish(battery);
// ******************************************************************************************
//publish bumpers
irobotcreate2::Bumper bumper;
bumper.left.header.stamp = current_time;
bumper.left.state = roomba->bumper_[LEFT];
bumper.right.header.stamp = current_time;
bumper.right.state = roomba->bumper_[RIGHT];
bumper_pub.publish(bumper);
bumper_warning_ = bumper.left.state || bumper.right.state;
bumper_warning.store(bumper_warning_);
// ******************************************************************************************
//publish buttons
irobotcreate2::Buttons buttons;
buttons.header.stamp = current_time;
buttons.clean = roomba->buttons_[BUTTON_CLEAN];
buttons.spot = roomba->buttons_[BUTTON_SPOT];
buttons.dock = roomba->buttons_[BUTTON_DOCK];
buttons.day = roomba->buttons_[BUTTON_DAY];
buttons.hour = roomba->buttons_[BUTTON_HOUR];
buttons.minute = roomba->buttons_[BUTTON_MINUTE];
buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE];
buttons.clock = roomba->buttons_[BUTTON_CLOCK];
buttons_pub.publish(buttons);
// ******************************************************************************************
//publish cliff
irobotcreate2::RoombaIR cliff;
cliff.header.stamp = current_time;
cliff.header.frame_id = "base_cliff_left";
cliff.state = roomba->cliff_[LEFT];
cliff.signal = roomba->cliff_signal_[LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_left";
cliff.state = roomba->cliff_[FRONT_LEFT];
cliff.signal = roomba->cliff_signal_[FRONT_LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_right";
cliff.state = roomba->cliff_[FRONT_RIGHT];
cliff.signal = roomba->cliff_signal_[FRONT_RIGHT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_right";
cliff.state = roomba->cliff_[RIGHT];
cliff.signal = roomba->cliff_signal_[RIGHT];
cliff_pub.publish(cliff);
// ******************************************************************************************
//publish irbumper
irobotcreate2::RoombaIR irbumper;
irbumper.header.stamp = current_time;
irbumper.header.frame_id = "base_irbumper_left";
irbumper.state = roomba->ir_bumper_[LEFT];
irbumper.signal = roomba->ir_bumper_signal_[LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_front_left";
irbumper.state = roomba->ir_bumper_[FRONT_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_center_left";
irbumper.state = roomba->ir_bumper_[CENTER_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_center_right";
irbumper.state = roomba->ir_bumper_[CENTER_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_front_right";
irbumper.state = roomba->ir_bumper_[FRONT_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
irbumper.header.frame_id = "base_irbumper_right";
irbumper.state = roomba->ir_bumper_[RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[RIGHT];
irbumper_pub.publish(irbumper);
ir_warning_ = ir_warning_ || irbumper.state;
ir_warning.store(ir_warning_);
// ******************************************************************************************
//publish irchar
irobotcreate2::IRCharacter irchar;
irchar.header.stamp = current_time;
irchar.omni = roomba->ir_char_[OMNI];
irchar.left = roomba->ir_char_[LEFT];
irchar.right = roomba->ir_char_[RIGHT];
irchar_pub.publish(irchar);
// ******************************************************************************************
//publish wheeldrop
irobotcreate2::WheelDrop wheeldrop;
wheeldrop.left.header.stamp = current_time;
wheeldrop.left.state = roomba->wheel_drop_[LEFT];
wheeldrop.right.header.stamp = current_time;
wheeldrop.right.state = roomba->wheel_drop_[RIGHT];
wheeldrop_pub.publish(wheeldrop);
ros::spinOnce();
r.sleep();
if(first_loop)
{
roomba->startOI(true);
if(!desired_mode_full) roomba->Safe();
first_loop=false;
}
}
roomba->powerDown();
roomba->closeSerialPort();
}
bool FollowJointTrajectoryController::init(ros::NodeHandle& nh, ControllerManager* manager)
{
/* We absolutely need access to the controller manager */
if (!manager)
{
initialized_ = false;
return false;
}
Controller::init(nh, manager);
/* No initial sampler */
boost::mutex::scoped_lock lock(sampler_mutex_);
sampler_.reset();
preempted_ = false;
/* Get Joint Names */
joint_names_.clear();
XmlRpc::XmlRpcValue names;
if (!nh.getParam("joints", names))
{
ROS_ERROR_STREAM("No joints given for " << nh.getNamespace());
return false;
}
if (names.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR_STREAM("Joints not in a list for " << nh.getNamespace());
return false;
}
for (int i = 0; i < names.size(); ++i)
{
XmlRpc::XmlRpcValue &name_value = names[i];
if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM("Not all joint names are strings for " << nh.getNamespace());
return false;
}
joint_names_.push_back(static_cast<std::string>(name_value));
}
/* Get parameters */
nh.param<bool>("stop_with_action", stop_with_action_, false);
/* Get Joint Handles, setup feedback */
joints_.clear();
for (size_t i = 0; i < joint_names_.size(); ++i)
{
JointHandle* j = manager_->getJointHandle(joint_names_[i]);
feedback_.joint_names.push_back(j->getName());
joints_.push_back(j);
}
/* Update feedback */
feedback_.desired.positions.resize(joints_.size());
feedback_.desired.velocities.resize(joints_.size());
feedback_.desired.accelerations.resize(joints_.size());
feedback_.actual.positions.resize(joints_.size());
feedback_.actual.velocities.resize(joints_.size());
feedback_.actual.effort.resize(joints_.size());
feedback_.error.positions.resize(joints_.size());
feedback_.error.velocities.resize(joints_.size());
/* Update tolerances */
path_tolerance_.q.resize(joints_.size());
path_tolerance_.qd.resize(joints_.size());
path_tolerance_.qdd.resize(joints_.size());
goal_tolerance_.q.resize(joints_.size());
goal_tolerance_.qd.resize(joints_.size());
goal_tolerance_.qdd.resize(joints_.size());
/* Setup ROS interfaces */
server_.reset(new server_t(nh, "",
boost::bind(&FollowJointTrajectoryController::executeCb, this, _1),
false));
server_->start();
initialized_ = true;
return true;
}
void renderStaticScene(ros::NodeHandle& node_handle,
planning_scene::PlanningScenePtr& planning_scene,
robot_model::RobotModelPtr& robot_model)
{
std::string environment_file;
std::vector<double> environment_position;
static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
visualization_msgs::MarkerArray>("visualization_marker_array", 10);
ros::WallDuration(1.0).sleep();
node_handle.param<std::string>("/itomp_planner/environment_model",
environment_file, "");
if (!environment_file.empty())
{
environment_position.resize(3, 0);
if (node_handle.hasParam("/itomp_planner/environment_model_position"))
{
XmlRpc::XmlRpcValue segment;
node_handle.getParam("/itomp_planner/environment_model_position",
segment);
if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
int size = segment.size();
for (int i = 0; i < size; ++i)
{
double value = segment[i];
environment_position[i] = value;
}
}
}
visualization_msgs::MarkerArray ma;
visualization_msgs::Marker msg;
msg.header.frame_id = robot_model->getModelFrame();
msg.header.stamp = ros::Time::now();
msg.ns = "environment";
msg.type = visualization_msgs::Marker::MESH_RESOURCE;
msg.action = visualization_msgs::Marker::ADD;
msg.scale.x = 1.0;
msg.scale.y = 1.0;
msg.scale.z = 1.0;
msg.id = 0;
msg.pose.position.x = environment_position[0];
msg.pose.position.y = environment_position[1];
msg.pose.position.z = environment_position[2];
ROS_INFO(
"Env render pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.color.a = 1.0;
msg.color.r = 0.5;
msg.color.g = 0.5;
msg.color.b = 0.5;
msg.mesh_resource = environment_file;
ma.markers.push_back(msg);
msg.ns = "environment2";
msg.type = visualization_msgs::Marker::CUBE;
msg.id = 0;
msg.pose.position.x = 0;
msg.pose.position.y = 6.0;
msg.pose.position.z = -0.45;
msg.scale.x = 2.0;
msg.scale.y = 1.0;
msg.scale.z = 1.0;
ma.markers.push_back(msg);
ros::WallDuration(1.0).sleep();
vis_marker_array_publisher_.publish(ma);
}
}
void NavSatTransform::run()
{
ros::Time::init();
double frequency = 10.0;
double delay = 0.0;
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
// Load the parameters we need
nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
nhPriv.param("yaw_offset", yawOffset_, 0.0);
nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
nhPriv.param("zero_altitude", zeroAltitude_, false);
nhPriv.param("publish_filtered_gps", publishGps_, false);
nhPriv.param("use_odometry_yaw", useOdometryYaw_, false);
nhPriv.param("wait_for_datum", useManualDatum_, false);
nhPriv.param("frequency", frequency, 10.0);
nhPriv.param("delay", delay, 0.0);
// Subscribe to the messages and services we need
ros::ServiceServer datumServ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);
if(useManualDatum_ && nhPriv.hasParam("datum"))
{
XmlRpc::XmlRpcValue datumConfig;
try
{
double datumLat;
double datumLon;
double datumYaw;
nhPriv.getParam("datum", datumConfig);
ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(datumConfig.size() == 4);
useManualDatum_ = true;
std::ostringstream ostr;
ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
std::istringstream istr(ostr.str());
istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;
robot_localization::SetDatum::Request request;
request.geo_pose.position.latitude = datumLat;
request.geo_pose.position.longitude = datumLon;
request.geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datumYaw);
request.geo_pose.orientation = tf2::toMsg(quat);
robot_localization::SetDatum::Response response;
datumCallback(request, response);
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
" for process_noise_covariance (type: " << datumConfig.getType() << ")");
}
}
bool getImpl(const std::string& key, XmlRpc::XmlRpcValue& v, bool use_cache)
{
std::string mapped_key = names::resolve(key);
if (mapped_key.empty()) mapped_key = "/";
if (use_cache)
{
boost::mutex::scoped_lock lock(g_params_mutex);
if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
{
M_Param::iterator it = g_params.find(mapped_key);
if (it != g_params.end())
{
if (it->second.valid())
{
ROS_DEBUG_NAMED("cached_parameters", "Using cached parameter value for key [%s]", mapped_key.c_str());
v = it->second;
return true;
}
else
{
ROS_DEBUG_NAMED("cached_parameters", "Cached parameter is invalid for key [%s]", mapped_key.c_str());
return false;
}
}
}
else
{
// parameter we've never seen before, register for update from the master
if (g_subscribed_params.insert(mapped_key).second)
{
XmlRpc::XmlRpcValue params, result, payload;
params[0] = this_node::getName();
params[1] = XMLRPCManager::instance()->getServerURI();
params[2] = mapped_key;
if (!master::execute("subscribeParam", params, result, payload, false))
{
ROS_DEBUG_NAMED("cached_parameters", "Subscribe to parameter [%s]: call to the master failed", mapped_key.c_str());
g_subscribed_params.erase(mapped_key);
use_cache = false;
}
else
{
ROS_DEBUG_NAMED("cached_parameters", "Subscribed to parameter [%s]", mapped_key.c_str());
}
}
}
}
XmlRpc::XmlRpcValue params, result;
params[0] = this_node::getName();
params[1] = mapped_key;
// We don't loop here, because validateXmlrpcResponse() returns false
// both when we can't contact the master and when the master says, "I
// don't have that param."
bool ret = master::execute("getParam", params, result, v, false);
if (use_cache)
{
boost::mutex::scoped_lock lock(g_params_mutex);
ROS_DEBUG_NAMED("cached_parameters", "Caching parameter [%s] with value type [%d]", mapped_key.c_str(), v.getType());
g_params[mapped_key] = v;
}
return ret;
}
