void TeleopCOB::getConfigurationFromParameters() { //std::map<std::string,joint_module> joint_modules; //std::vector<std::string> module_names; if(n_.hasParam("modules")) { XmlRpc::XmlRpcValue modules; ROS_DEBUG("modules found "); n_.getParam("modules", modules); if(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct) { ROS_DEBUG("modules are of type struct with size %d",(int)modules.size()); for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=modules.begin();p!=modules.end();++p) { std::string mod_name = p->first; ROS_DEBUG("module name: %s",mod_name.c_str()); XmlRpc::XmlRpcValue mod_struct = p->second; if(mod_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) ROS_WARN("invalid module, name: %s",mod_name.c_str()); // search for joint_name parameter in current module struct to determine which type of module // only joint mods or wheel mods supported // which mens that is no joint names are found, then the module is a wheel module // TODO replace with build in find, but could not get it to work if(!assign_joint_module(mod_name, mod_struct)) { // add wheel module struct ROS_DEBUG("wheel module found"); assign_base_module(mod_struct); } } } } }
/** * Tries to read joint module configurations from XmlRpcValue object. * If the module is a joint module, it contains a joint name array. * A typical joint module has the following configuration structure: * struct { * joint_names: ['head_pan_joint','head_tilt_joint'], * joint_step: 0.075 * } * @param mod_struct configuration object struct * @return true if the configuration object hols a joint module config, else false */ bool TeleopCOB::assign_joint_module(std::string mod_name, XmlRpc::XmlRpcValue mod_struct) { // search for joint_name parameter in current module struct to determine which type of module // only joint mods or wheel mods supported // which mens that is no joint names are found, then the module is a wheel module // TODO replace with build in find, but could not get it to work bool is_joint_module = false; joint_module tempModule; for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps) { std::string par_name = ps->first; ROS_DEBUG("par name: %s",par_name.c_str()); if(par_name.compare("joint_names")==0) { ROS_DEBUG("joint names found"); XmlRpc::XmlRpcValue joint_names = ps->second; ROS_ASSERT(joint_names.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_DEBUG("joint_names.size: %d \n", joint_names.size()); for(int i=0;i<joint_names.size();i++) { ROS_ASSERT(joint_names[i].getType() == XmlRpc::XmlRpcValue::TypeString); std::string s((std::string)joint_names[i]); ROS_DEBUG("joint_name found = %s",s.c_str()); tempModule.joint_names.push_back(s); } // set size of other vectors according to the joint name vector tempModule.req_joint_pos_.resize(joint_names.size()); tempModule.req_joint_vel_.resize(joint_names.size()); is_joint_module = true; //break; // no need to continue searching if joint names are found }else if(par_name.compare("joint_step")==0){ ROS_DEBUG("joint steps found"); XmlRpc::XmlRpcValue joint_steps = ps->second; ROS_ASSERT(joint_steps.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_DEBUG("joint_steps.size: %d \n", joint_steps.size()); for(int i=0;i<joint_steps.size();i++) { ROS_ASSERT(joint_steps[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); double step((double)joint_steps[i]); ROS_DEBUG("joint_step found = %f",step); tempModule.steps.push_back(step); } } } if(is_joint_module) { // assign publisher tempModule.module_publisher_ = n_.advertise<trajectory_msgs::JointTrajectory>(("/"+mod_name+"_controller/command"),1); tempModule.module_publisher_brics_ = n_.advertise<brics_actuator::JointVelocities>(("/"+mod_name+"_controller/command_vel"),1); // store joint module in collection ROS_DEBUG("head module stored"); joint_modules_.insert(std::pair<std::string,joint_module>(mod_name,tempModule)); } return is_joint_module; }
/** * Tries to read base module configurations from XmlRpcValue object. * A base module is supposed to contain vectors 3 element vectors (x,y,th) * with max acceleration and velocity. Example: * struct { * max_velocity: [0.3, 0.2, 0.3], * max_acceleration: [0.5, 0.5, 0.7] * } * @param mod_struct configuration object struct * @return true no check is currently performed TODO check */ bool TeleopCOB::assign_base_module(XmlRpc::XmlRpcValue mod_struct) { for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps) { std::string par_name = ps->first; ROS_DEBUG("par name: %s",par_name.c_str()); if(par_name.compare("max_velocity")==0) { ROS_DEBUG("max vel found"); XmlRpc::XmlRpcValue max_vel = ps->second; ROS_ASSERT(max_vel.getType() == XmlRpc::XmlRpcValue::TypeArray); if(max_vel.size()!=3){ROS_WARN("invalid base parameter size");} ROS_DEBUG("max_vel.size: %d \n", max_vel.size()); for(int i=0;i<max_vel.size();i++) { ROS_ASSERT(max_vel[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); double val = (double)max_vel[i]; ROS_DEBUG("max vel value = %f",val); base_module_.max_vel_.push_back(val); } } else if(par_name.compare("max_acceleration")==0) { ROS_DEBUG("max acc found"); XmlRpc::XmlRpcValue max_acc = ps->second; ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray); if(max_acc.size()!=3){ROS_DEBUG("invalid base parameter size");} ROS_DEBUG("max_acc.size: %d \n", max_acc.size()); for(int i=0;i<max_acc.size();i++) { ROS_ASSERT(max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); double val = (double)max_acc[i]; ROS_DEBUG("max acc value = %f", val); base_module_.max_acc_.push_back(val); } } else { ROS_WARN("unsupported base module parameter read"); } } // make all the vectors the same length // the vector size is not completely safe since only warning is // issued if max value arrays has the wrong length base_module_.req_vel_.resize(base_module_.max_acc_.size()); base_module_.vel_old_.resize(base_module_.max_acc_.size()); base_module_.base_publisher_ = n_.advertise<geometry_msgs::Twist>("/base_controller/command",1); ROS_DEBUG("base module stored"); has_base_module_ = true; return true; }
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])); } }
void EmotionGenerator::getEmotionDesireRelation(std::string desireType) { XmlRpc::XmlRpcValue emotionParam; std::string namespaceNode = n_->getNamespace().substr(1,n_->getNamespace().length()-1); std::string paramServerDesire = namespaceNode + "/" + nodeName_ + "/" + desireType; n_->getParam(paramServerDesire, emotionParam); // ROS_INFO("Emo gen - param emotion desire relation :%s,%s,%s = %s", n_->getNamespace().c_str() , // nodeName_.c_str(), // desireType.c_str(), // paramServerDesire.c_str() ); std::map<std::string,double> mapEmotion; ROS_INFO_COND(emotionParam.begin() != emotionParam.end() , "new desire %s :",desireType.c_str()); for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator it = emotionParam.begin(); it != emotionParam.end(); it++) { double emotionFactor = 0; if((*it).second.getType() == XmlRpc::XmlRpcValue::TypeInt) { int value = static_cast<int>((*it).second); ROS_INFO(" %s %i",(*it).first.c_str(), value); emotionFactor = (double)value; } else if ((*it).second.getType() == XmlRpc::XmlRpcValue::TypeDouble) { emotionFactor = static_cast<double>((*it).second); ROS_INFO(" %s %5.2f",(*it).first.c_str(), emotionFactor); } mapEmotion[(*it).first] = emotionFactor; if(emotionIntensities.count((*it).first) == 0) { //initialize this emotion emotionIntensities[(*it).first] = 0; } } if(mapEmotion.size() > 0) emotionMatrix[desireType] = mapEmotion; }
bool VisualizationBase::readParameters(XmlRpc::XmlRpcValue& config) { if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { ROS_ERROR("A filter configuration must be a map with fields name, type, and params."); return false; } // Check to see if we have parameters in our list. if (config.hasMember("params")) { XmlRpc::XmlRpcValue params = config["params"]; if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) { ROS_ERROR("Params must be a map."); return false; } else { for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) { ROS_DEBUG("Loading param %s\n", it->first.c_str()); parameters_[it->first] = it->second; } } } return true; }
void ServoInterface::loadServoCommandPriorities() { ros::NodeHandle nhPvt = getPrivateNodeHandle(); XmlRpc::XmlRpcValue v; nhPvt.param("servoCommandProirities", v, v); std::map<std::string, XmlRpc::XmlRpcValue>::iterator mapIt; for(mapIt = v.begin(); mapIt != v.end(); mapIt++) { if(mapIt->second.getType() == XmlRpc::XmlRpcValue::TypeInt) { //add entry in priority queue and command map priorityEntry toAdd; toAdd.id = mapIt->first; toAdd.priority = static_cast<int>(mapIt->second); m_servoCommandPriorities.push_back(toAdd); m_servoCommandMsgs[mapIt->first] = autorally_msgs::servoMSG(); } else { NODELET_ERROR("ServoInterface: XmlRpc servo command priorities formatted incorrectly"); } } std::sort(m_servoCommandPriorities.begin(), m_servoCommandPriorities.end(), priorityComparator()); std::vector<priorityEntry>::const_iterator vecIt; for(vecIt = m_servoCommandPriorities.begin(); vecIt != m_servoCommandPriorities.end(); vecIt++) { NODELET_INFO_STREAM("ServoInterface: ServoCommand ID:Priorities:" << vecIt->id << ":" << vecIt->priority); } NODELET_INFO_STREAM("ServoInterface: Loaded " << m_servoCommandPriorities.size() << " servo commanders"); }
/** * \brief Load the configuration from file. * @param configurationPrefix The entry node inside the configuration YAML file. * @return The complete image provider configuration. */ ImageProviderConfiguration::ConstPtr getImageProviderConfiguration () { ImageProviderConfiguration::Ptr configuration = boost::make_shared<ImageProviderConfiguration>(); // private parameters ros::NodeHandle privateNodeHandle ("~"); std::string configurationPrefix; privateNodeHandle.param("cfgprefix", configurationPrefix, std::string("configuration")); privateNodeHandle.param("startpos", configuration->startPos, 0); privateNodeHandle.param("length", configuration->length, -1); privateNodeHandle.param("rate", configuration->rate, 1); privateNodeHandle.param("loop", configuration->loop, false); privateNodeHandle.param("topicprefix", configuration->topicPrefix, std::string("cameras")); ros::NodeHandle configNodeHandle (privateNodeHandle, configurationPrefix); // configuration parameters (supposed to be loaded via YAML file) if (!configNodeHandle.getParam("srcdir", configuration->sourceDir)) { configuration->sourceDir = "."; } if (!configNodeHandle.getParam("filepattern", configuration->filePattern)) { throw ImageProviderException("Parameter 'filepattern' not found"); } try { XmlRpc::XmlRpcValue cameraNames; if (configNodeHandle.getParam("cameras", cameraNames)) { std::map<std::string, XmlRpc::XmlRpcValue>::iterator nameIterator; for (nameIterator = cameraNames.begin(); nameIterator != cameraNames.end(); ++nameIterator) { std::string cameraName = nameIterator->first; ROS_INFO_STREAM("Reading camera configuration for camera: " << cameraName); CameraConfigEntry::ConstPtr cameraConfig = getCameraConfigurationEntry(configNodeHandle, cameraName); configuration->cameraConfigs.push_back(cameraConfig); } } } catch (XmlRpc::XmlRpcException& e) { throw ImageProviderException(std::string("Failed to retrieve camera names: ") + std::string(e.getMessage())); } return configuration; }
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; }
int main(int argc, char **argv) { ros::init(argc, argv, "uav_commander"); ros::NodeHandle n; ros::NodeHandle n_priv("~"); //////////////////////////// // Load object part types // //////////////////////////// std::vector<std::string> robot_ids; std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation; XmlRpc::XmlRpcValue swarm; n_priv.getParam("swarm", swarm); ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots."); std::map<std::string, XmlRpc::XmlRpcValue>::iterator i; for (i = swarm.begin(); i != swarm.end(); i++) { robot_ids.push_back(i->first); Eigen::Matrix<double,4,1, Eigen::DontAlign> pose; double x=i->second["pose"]["x"]["value"]; double y=i->second["pose"]["y"]["value"]; double z=i->second["pose"]["z"]["value"]; double yaw=i->second["pose"]["yaw"]["value"]; pose << x, y,z, yaw; desired_formation.push_back(pose); } SwarmFormationControl swarm_formation(n,robot_ids,desired_formation); ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10); geometry_msgs::PoseStamped goal_msg; goal_msg.pose.position.x=0.0; goal_msg.pose.position.y=0.0; goal_msg.pose.position.z=1.0; // static tf::TransformBroadcaster brr; // tf::Transform transform; // int increment=1; // tf::Quaternion q; // q.setRPY(0, 0, 0); // transform.setRotation(q); // transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) ); ros::AsyncSpinner spinner(4); spinner.start(); ros::Rate loop_rate(20); while (ros::ok()) { // ++increment; // transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) ); // transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) ); // q.setRPY(0, 0, 0+increment*0.001); // transform.setRotation(q); // brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal")); swarm_formation.updateCentroid(); loop_rate.sleep(); goal_msg.pose.position.x+=0.01; goal_msg.pose.position.z+=0.001; goal_pub.publish(goal_msg); } spinner.stop(); return 0; }
void WholeBodyController::loadParameterFiles() { ros::NodeHandle n("~"); std::string ns = ros::this_node::getName(); XmlRpc::XmlRpcValue groups; typedef std::map<std::string, XmlRpc::XmlRpcValue>::iterator XmlRpcIterator; try { // ROBOT n.getParam("/whole_body_controller/collision_model", groups); for(XmlRpcIterator itrGroups = groups.begin(); itrGroups != groups.end(); ++itrGroups) { // COLLISION GROUP std::vector< RobotState::CollisionBody > robot_state_group; XmlRpc::XmlRpcValue group = itrGroups->second; for(XmlRpcIterator itrBodies = group.begin(); itrBodies != group.end(); ++itrBodies) { // COLLISION BODY XmlRpc::XmlRpcValue collisionBody = itrBodies->second; //cout << collisionBody["name"] << endl; RobotState::CollisionBody robotstate_collision_body; robotstate_collision_body.fromXmlRpc(collisionBody); // add the collision bodies to the group robot_state_group.push_back(robotstate_collision_body); } // add group of collision bodies to the robot robot_state_.robot_.groups.push_back(robot_state_group); } if (robot_state_.robot_.groups.size() == 0) { ROS_WARN("No collision model loaded"); } XmlRpc::XmlRpcValue exclusion_groups; n.getParam("/whole_body_controller/exlusions_collision_calculation", exclusion_groups); for(XmlRpcIterator itrExclGr = exclusion_groups.begin(); itrExclGr != exclusion_groups.end(); ++itrExclGr) { XmlRpc::XmlRpcValue ExclGroup = itrExclGr->second; for(XmlRpcIterator itrExcl = ExclGroup.begin(); itrExcl != ExclGroup.end(); ++itrExcl) { XmlRpc::XmlRpcValue Excl = itrExcl->second; RobotState::Exclusion exclusion; exclusion.fromXmlRpc(Excl); robot_state_.exclusion_checks.checks.push_back(exclusion); } } if (robot_state_.exclusion_checks.checks.size() == 0) { ROS_WARN("No exclusions from self-collision avoindance checks"); } else if (robot_state_.exclusion_checks.checks.size() > 0) { ROS_DEBUG("Exclusions from self-collision checks are: "); for (std::vector<RobotState::Exclusion>::iterator it = robot_state_.exclusion_checks.checks.begin(); it != robot_state_.exclusion_checks.checks.end(); ++it) { RobotState::Exclusion excl = *it; ROS_DEBUG("Name body A = %s", excl.name_body_A.c_str()); ROS_DEBUG("Name body B = %s", excl.name_body_B.c_str()); } } } catch(XmlRpc::XmlRpcException& ex) { std::cout << ex.getMessage() << std::endl; } }
// WriteCameraFeaturesFromRosparam() // Read ROS parameters from this node's namespace, and see if each parameter has a similarly named & typed feature in the camera. Then set the // camera feature to that value. For example, if the parameter camnode/Gain is set to 123.0, then we'll write 123.0 to the Gain feature // in the camera. // // Note that the datatype of the parameter *must* match the datatype of the camera feature, and this can be determined by // looking at the camera's XML file. Camera enum's are string parameters, camera bools are false/true parameters (not 0/1), // integers are integers, doubles are doubles, etc. // void WriteCameraFeaturesFromRosparam(void) { XmlRpc::XmlRpcValue xmlrpcParams; XmlRpc::XmlRpcValue::iterator iter; ArvGcNode *pGcNode; GError *error=NULL; global.phNode->getParam (ros::this_node::getName(), xmlrpcParams); if (xmlrpcParams.getType() == XmlRpc::XmlRpcValue::TypeStruct) { for (iter=xmlrpcParams.begin(); iter!=xmlrpcParams.end(); iter++) { std::string key = iter->first; pGcNode = arv_device_get_feature (global.pDevice, key.c_str()); if (pGcNode && arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error)) { // unsigned long typeValue = arv_gc_feature_node_get_value_type((ArvGcFeatureNode *)pGcNode); // ROS_INFO("%s cameratype=%lu, rosparamtype=%d", key.c_str(), typeValue, static_cast<int>(iter->second.getType())); // We'd like to check the value types too, but typeValue is often given as G_TYPE_INVALID, so ignore it. switch (iter->second.getType()) { case XmlRpc::XmlRpcValue::TypeBoolean://if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeBoolean))// && (typeValue==G_TYPE_INT64)) { int value = (bool)iter->second; arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (bool) %s: %d", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeInt: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeInt))// && (typeValue==G_TYPE_INT64)) { int value = (int)iter->second; arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeDouble: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeDouble))// && (typeValue==G_TYPE_DOUBLE)) { double value = (double)iter->second; arv_device_set_float_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeString: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeString))// && (typeValue==G_TYPE_STRING)) { std::string value = (std::string)iter->second; arv_device_set_string_feature_value(global.pDevice, key.c_str(), value.c_str()); ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str()); } break; case XmlRpc::XmlRpcValue::TypeInvalid: case XmlRpc::XmlRpcValue::TypeDateTime: case XmlRpc::XmlRpcValue::TypeBase64: case XmlRpc::XmlRpcValue::TypeArray: case XmlRpc::XmlRpcValue::TypeStruct: default: ROS_WARN("Unhandled rosparam type in WriteCameraFeaturesFromRosparam()"); } } } } } // WriteCameraFeaturesFromRosparam()