Path::Path() { ros::NodeHandle nh; XmlRpc::XmlRpcValue pathCfg; nh.getParam("/path", pathCfg); try { int i = 0; do { char pointName[256]; sprintf(pointName, "point%d", i); if (!pathCfg.hasMember(pointName)) break; XmlRpc::XmlRpcValue pointCfg = pathCfg[std::string(pointName)]; geometry_msgs::Point p; if (!(pointCfg.hasMember("x") && pointCfg.hasMember("y"))) continue; p.x = FLOAT_PARAM(pointCfg["x"]); p.y = FLOAT_PARAM(pointCfg["y"]); p.z = 0; points.push_back(p); } while ((++i) != 0); } catch (XmlRpc::XmlRpcException& e) { ROS_ERROR("Unable to parse path parameter. (%s)", e.getMessage().c_str()); } }
void checkAndSetFeature(CamData& cd, string param_name, dc1394feature_t feature) { string p = cd.name + string("/") + param_name; if (has_param(p)) { XmlRpc::XmlRpcValue val; get_param(p, val); if (val.getType() == XmlRpc::XmlRpcValue::TypeString) if (val == string("auto")) cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_AUTO); else cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL); if (val.getType() == XmlRpc::XmlRpcValue::TypeInt) { cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL); cd.cam->setFeature(feature, (int)(val)); } if (val.getType() == XmlRpc::XmlRpcValue::TypeDouble) { cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL); cd.cam->setFeatureAbsolute(feature, (double)(val)); } } }
void checkAndSetWhitebalance(CamData& cd, string param_name, dc1394feature_t feature) { string p = cd.name + string("/") + param_name; string p_b = cd.name + string("/") + param_name + string("_b"); string p_r = cd.name + string("/") + param_name + string("_r"); if (has_param(p_b) && has_param(p_r)) { XmlRpc::XmlRpcValue val; XmlRpc::XmlRpcValue val_b; XmlRpc::XmlRpcValue val_r; get_param(p, val); get_param(p_b, val_b); get_param(p_r, val_r); if (val.getType() == XmlRpc::XmlRpcValue::TypeString && val == string("auto")) cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_AUTO); else if (val_b.getType() == XmlRpc::XmlRpcValue::TypeInt && val_r.getType() == XmlRpc::XmlRpcValue::TypeInt) { cd.cam->setFeatureMode(feature, DC1394_FEATURE_MODE_MANUAL); cd.cam->setFeature(feature, (int)(val_b), (int)(val_r)); } } }
bool set(std::string key, XmlRpc::XmlRpcValue val) { try { if (val.getType() == XmlRpc::XmlRpcValue::TypeBoolean){ bool value = static_cast<bool>(val); return (set(key,value)); } if (val.getType() == XmlRpc::XmlRpcValue::TypeDouble){ double value = static_cast<double>(val); return (set(key,value)); } if (val.getType() == XmlRpc::XmlRpcValue::TypeString){ std::string value = static_cast<std::string>(val); return (set(key,value)); } if (val.getType() == XmlRpc::XmlRpcValue::TypeInt){ int value = static_cast<int>(val); return (set(key,value)); } } catch (...) { return false; } return false; }
bool get(std::string key, XmlRpc::XmlRpcValue& val) { try { bool v; if (get(key,v)){ XmlRpc::XmlRpcValue vb(v); val = vb; return (val.valid()); } double vd; if (get(key,vd)){ val = vd; return (val.valid()); } int vi; if (get(key,vi)){ val=vi; return (val.valid()); } std::string vs; if (get(key, vs)){ val=vs.c_str(); return (val.valid()); } } catch (...) { return false; } return false; }
void applySetupCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result) { if(params.valid()) { if(params.size() != 1) { result = -1; return; } XmlRpc::XmlRpcValue p = params[0]; if(p.size() % 2 != 0) { result = -1; fflush(stdout); return; } for(int i=0; i<p.size(); i+= 2) { if(0 > ctrl_xmlrpc->addModule(p[i], p[i+1])) { result = i+1; fflush(stdout); return; } } } result = 0; fflush(stdout); return; }
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); } } } } }
bool getImpl(const std::string& key, int& i, bool use_cache) { XmlRpc::XmlRpcValue v; if (!getImpl(key, v, use_cache)) { return false; } if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble) { double d = v; if (fmod(d, 1.0) < 0.5) { d = floor(d); } else { d = ceil(d); } i = d; } else if (v.getType() != XmlRpc::XmlRpcValue::TypeInt) { return false; } else { i = v; } return true; }
bool DepthImageOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue ¶ms) { try { sensor_type_ = (std::string) params["sensor_type"]; if (params.hasMember("image_topic")) image_topic_ = (std::string) params["image_topic"]; if (params.hasMember("queue_size")) queue_size_ = (int)params["queue_size"]; readXmlParam(params, "near_clipping_plane_distance", &near_clipping_plane_distance_); readXmlParam(params, "far_clipping_plane_distance", &far_clipping_plane_distance_); readXmlParam(params, "shadow_threshold", &shadow_threshold_); readXmlParam(params, "padding_scale", &padding_scale_); readXmlParam(params, "padding_offset", &padding_offset_); readXmlParam(params, "skip_vertical_pixels", &skip_vertical_pixels_); readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_); } catch (XmlRpc::XmlRpcException &ex) { ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); return false; } return true; }
bool PointCloudMoveitFilter::setParams(XmlRpc::XmlRpcValue ¶ms) { try { if (!params.hasMember("point_cloud_topic")) return false; point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]); readXmlParam(params, "max_range", &max_range_); readXmlParam(params, "padding_offset", &padding_); readXmlParam(params, "padding_scale", &scale_); readXmlParam(params, "point_subsample", &point_subsample_); if (!params.hasMember("filtered_cloud_topic")) { ROS_ERROR("filtered_cloud_topic is required"); return false; } else { filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]); } if (params.hasMember("filtered_cloud_use_color")) { use_color_ = (bool)params["filtered_cloud_use_color"]; } if (params.hasMember("filtered_cloud_keep_organized")) { keep_organized_ = (bool)params["filtered_cloud_keep_organized"]; } } catch (XmlRpc::XmlRpcException& ex) { ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str()); return false; } return true; }
bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len) { if(!params.hasMember(name)) { ROS_ERROR("Expected ft_param to have '%s' element", name); return false; } XmlRpc::XmlRpcValue values = params[name]; if (values.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("Expected FT param '%s' to be list type", name); return false; } if (values.size() != int(len)) { ROS_ERROR("Expected FT param '%s' to have %d elements", name, len); return false; } for (unsigned i=0; i<len; ++i) { if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i); return false; } else { results[i] = static_cast<double>(values[i]); } } return true; }
/*generateConfigVector stores the pinging sensors, to which pinging sensor is a listening * sensor bound to in a cycle and which sensors are not used at all. * In the output, the pinging sensors are represented by PINGING_SENSOR, location of * each listening sensor holds the address of its corresponding pinging sensor * and the sensors not used at all are represented by SENSOR_NOT_USED. */ std::vector <std::vector<int> > generateConfigVector(XmlRpc::XmlRpcValue config_list) { std::vector<std::vector<int> > config; XmlRpc::XmlRpcValue current_cycle; ROS_ASSERT(config_list.getType() == XmlRpc::XmlRpcValue::TypeArray); for(int i = 0; i < config_list.size(); i++) { ROS_ASSERT(config_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct); std::vector<int> cycle_vector; //For holding configuration for each cycle. cycle_vector.resize(MAX_SENSORS); for (int j=0; j<MAX_SENSORS; j++) cycle_vector[j]=SENSOR_NOT_USED; //better to keep it dynamic for (int j = 0; j<MAX_SENSORS; j++){ char str_index[3] = {'\0','\0', '\0'}; // for storing int to hex conv. sprintf(str_index,"%d", j); // Unsure if yamlcpp parser reads hexadecimal values if(config_list[i].hasMember(str_index)) { cycle_vector[j]=PINGING_SENSOR; //Set -1 for pinging sensor current_cycle = (config_list[i]).operator[](str_index); ROS_ASSERT(current_cycle.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_ASSERT(current_cycle.size()>0); //Assign address of pinging sensor to each listening sensor for (int k = 0; k<current_cycle.size(); k++){ cycle_vector[static_cast<int>(current_cycle[k])]=((static_cast<int>(current_cycle[k])!=j)?j:PINGING_AND_LISTENING_SENSOR); } } } config.push_back(cycle_vector); } return config; }
void CRTLXmlrpc::getProcState(XmlRpc::XmlRpcValue& result) { result.setSize(3); result[1] = 0.0; result[2] = 0.0; if(eegmanager == NULL) { result[0] = std::string("IDLE"); return; } // update local state variable if(!eegmanager->isRunning()) started = false; if(eegmanager->isRunning()) { result.setSize(eegmanager->bandwidth.size()*2 + 1); result[0] = std::string("RUNNING"); result.setSize(eegmanager->bandwidth.size()); for(uint32_t i=0; i<eegmanager->bandwidth.size(); i++) { result[(i*2)+1] = eegmanager->bandwidth[i]; result[(i*2)+2] = eegmanager->fill[i]; } return; } if(eegmanager->current_setup().size() > 0) { result[0] = std::string("CONFIGURED"); return; } result[0] = std::string("IDLE"); return; }
void startWorkingCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result) { // if no arguments were received if (!params.valid()) { result = ctrl_xmlrpc->startProcessing(); } else { switch(params.size()) { // start the server in online mode with port and blocksize case 2: ctrl_xmlrpc->startEegServer((int) params[0], (int) params[1]); result = 0; break; case 3: ctrl_xmlrpc->startEegServer((std::string) params[0], (int) params[1], (int) params[2]); result = 0; break; default: OMG("WARNING! Server got wrong number of arguments!"); break; } } fflush(stdout); }
bool TestCollisionWorld::readDoubleArray(XmlRpc::XmlRpcValue& list, std::vector<double>& array) { if (list.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("Parameter needs to be an *array* of doubles"); return false; } array.clear(); for (int32_t i = 0; i < list.size(); ++i) { if (list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && list[i].getType() != XmlRpc::XmlRpcValue::TypeInt) { ROS_ERROR("Parameter needs to be an array of *doubles*"); return false; } double value=0.0; if (list[i].getType() == XmlRpc::XmlRpcValue::TypeInt) value = static_cast<double>(static_cast<int>(list[i])); else value = static_cast<double>(list[i]); array.push_back(value); } return true; }
/** * 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; }
bool PTPFollowJointTrajectoryController::init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh) { nh_ = controller_nh; // get all joint states from the hardware interface const std::vector<std::string>& joint_names = hw->getNames(); for (unsigned i = 0; i < joint_names.size(); i++) ROS_DEBUG_NAMED("PTPFollowJointController", "Got joint %s", joint_names[i].c_str()); XmlRpc::XmlRpcValue joints; if (!controller_nh.getParam("joints", joints)) { ROS_ERROR("No joints array setting"); return false; } if (joints.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("Joints array setting setting type mismatched"); return false; } for (size_t i = 0; i < joints.size(); i++) { std::string joint_name; ROS_ASSERT(joints[i].getType() == XmlRpc::XmlRpcValue::TypeString); joint_name = static_cast<std::string>(joints[i]); for(size_t j = 0; j < joint_names.size(); j++) { if(joint_name == joint_names[i]) { ROS_DEBUG("Add joint %s to PTPFollowJointController", joint_name.c_str()); joints_.push_back(hw->getHandle(joint_name)); break; } } } q.resize(joints_.size()); qd.resize(joints_.size()); qdd.resize(joints_.size()); // Creates a dummy trajectory boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1)); SpecifiedTrajectory &traj = *traj_ptr; traj[0].duration = 0.0; traj[0].splines.resize(joints_.size()); for (size_t j = 0; j < joints_.size(); ++j) traj[0].splines[j].coef[0] = 0.0; current_trajectory_box_.set(traj_ptr); action_server_follow_.reset(new FJTAS(controller_nh, "ptp_follow_joint_trajectory", boost::bind(&PTPFollowJointTrajectoryController::goalCBFollow, this, _1), boost::bind(&PTPFollowJointTrajectoryController::cancelCBFollow, this, _1), false)); action_server_follow_->start(); return true; }
void onInit() { n_ = getNodeHandle(); ros::NodeHandle pn = getPrivateNodeHandle(); pn.getParam("target_frame_id", target_frame_id_); pn.getParam("table_frame_id", table_frame_id_); pn.getParam("height_min", height_min_); pn.getParam("height_max", height_max_); XmlRpc::XmlRpcValue v; pn.getParam("table_dimensions", v); if( v.size() < 3) { ROS_ERROR("Hull points not set correctly, nodelet will not work"); return; } double x ,y, z; x = (double)v[0]; y = (double)v[1]; z = (double)v[2]; Eigen::Vector3d p; p << -x/2, -y/2, z; hull_points_[0] = p; p << -x/2, y/2, z; hull_points_[1] = p; p << x/2, y/2, z; hull_points_[2] = p; p << x/2, -y/2, z; hull_points_[3] = p; tf::StampedTransform trf_table; try { tf_listener_.waitForTransform(target_frame_id_, table_frame_id_, ros::Time(), ros::Duration(2)); tf_listener_.lookupTransform(target_frame_id_, table_frame_id_, ros::Time(), trf_table); } catch (tf::TransformException& ex) { ROS_ERROR("[transform region crop] : %s",ex.what()); return; } Eigen::Affine3d ad; tf::transformTFToEigen(trf_table, ad); for(int i = 0; i < hull_points_.size(); i++) { Point p; p.getVector3fMap() = (ad*hull_points_[i]).cast<float>(); hull_->points[i] = p; } pc_sub_ = n_.subscribe<PointCloud>("point_cloud_in", 1, boost::bind(&TableRegionCropNodelet::topicCallback, this, _1)); pc_pub_ = n_.advertise<PointCloud>("point_cloud_out", 1); eppd_.setInputPlanarHull(hull_); eppd_.setHeightLimits(height_min_, height_max_); eppd_.setViewPoint(0,0,5); }
int main(int argc, char* argv[]) { std::vector<ros::Subscriber> subs; ros::init(argc, argv, "Navi2"); ros::NodeHandle nh; XmlRpc::XmlRpcValue topicList; ros::param::get("~inputTopics", topicList); ROS_ASSERT(topicList.size() > 0); ROS_ASSERT(topicList.getType() == XmlRpc::XmlRpcValue::TypeArray); for (int i = 0; i < topicList.size(); ++i) { ROS_ASSERT(topicList[i].getType() == XmlRpc::XmlRpcValue::TypeString); std::string topic = static_cast<std::string>(topicList[i]); maps.push_back(nav_msgs::OccupancyGrid()); flags.push_back(0); subs.push_back(nh.subscribe<nav_msgs::OccupancyGrid>(topic, 1, boost::bind(mapCallback, _1, i))); } ros::Publisher mapPub = nh.advertise<nav_msgs::OccupancyGrid>("output", 1); ros::Rate rate(20); while (ros::ok()) { rate.sleep(); ros::spinOnce(); //Everyone ready? if ((unsigned int)std::count(flags.begin(), flags.end(), 1) == flags.size()) { //merge nav_msgs::OccupancyGrid map; map.info = maps.front().info; for (unsigned int i = 0; i < map.info.height * map.info.width; ++i) { map.data.push_back(-1); for (std::vector<nav_msgs::OccupancyGrid>::iterator j = maps.begin(); j != maps.end(); ++j) { map.data[i] = std::max(map.data[i], j->data[i]); } } mapPub.publish(map); //Make not ready std::fill(flags.begin(), flags.end(), 0); } } }
bool getImpl(const std::string& key, bool& b, bool use_cache) { XmlRpc::XmlRpcValue v; if (!getImpl(key, v, use_cache)) return false; if (v.getType() != XmlRpc::XmlRpcValue::TypeBoolean) return false; b = v; return true; }
bool getImpl(const std::string& key, std::string& s, bool use_cache) { XmlRpc::XmlRpcValue v; if (!getImpl(key, v, use_cache)) return false; if (v.getType() != XmlRpc::XmlRpcValue::TypeString) return false; s = std::string(v); return true; }
// Replacement "shutdown" XMLRPC callback void shutdownCallback( XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result ) { if( (params.getType() == XmlRpc::XmlRpcValue::TypeArray) && (params.size() > 1) ) { std::string reason = params[1]; ROS_WARN( "Shutdown request received. Reason: [%s]", reason.c_str() ); g_request_shutdown = 1; } result = ros::xmlrpc::responseInt( 1, "", 0 ); }
/** * 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; }
QLearner::QLearner(ros::NodeHandle nh) { n_ = nh; ros::NodeHandle n_private("~"); n_private.param("num_states", num_states_, 8); n_private.param("num_actions", num_actions_, 3); n_private.param("learning_rate", learning_rate_, 0.3); n_private.param("discount_factor", discount_factor_, 0.5); n_private.param("temp_const", temp_const_, 5.0); n_private.param("temp_alpha", temp_alpha_, 0.85); temp_cnt_ = 0; temp_ = temp_const_ * pow(temp_alpha_, temp_cnt_); size_array_ = num_actions_ * num_states_; srand ( time(NULL) ); Init(); if (n_private.hasParam("qarray")) { learn_ = false; // Get the qtable XmlRpc::XmlRpcValue qtable; n_private.getParam("qarray", qtable); if (qtable.getType() != XmlRpc::XmlRpcValue::TypeArray) ROS_ERROR("Error reading footsteps/x from config file."); int size; try { size = qtable.size(); if (size != (num_states_ * num_actions_)) { ROS_ERROR("Size of array does not match num_states * num_actions = %d,\ exiting.", num_states_ * num_actions_); exit(0); } } catch (const XmlRpc::XmlRpcException e) { ROS_ERROR("No table available, exiting."); exit(0); } // create qarray set for(int i=0; i < size; i++) { q_array_.push_back((double)qtable[i]); } }
/* parameter format is: polygon_array: [[[0, 0, 0], [0, 0, 1], [1, 0, 0]], ...] */ bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name) { if (pnh_->hasParam(param_name)) { XmlRpc::XmlRpcValue v; pnh_->param(param_name, v, v); if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) { for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) { // polygons XmlRpc::XmlRpcValue polygon_v = v[toplevel_i]; geometry_msgs::PolygonStamped polygon; if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray && polygon_v.size() >= 3) { for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) { // each polygon, vertices XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i]; if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray && vertex_v.size() == 3 ) { // vertex_v := [x, y, z] double x = getXMLDoubleValue(vertex_v[0]); double y = getXMLDoubleValue(vertex_v[1]); double z = getXMLDoubleValue(vertex_v[2]); geometry_msgs::Point32 point; point.x = x; point.y = y; point.z = z; polygon.polygon.points.push_back(point); } else { JSK_NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3", param_name.c_str(), toplevel_i, secondlevel_i); return false; } } polygons_.polygons.push_back(polygon); // estimate model coefficients coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon)); } else { JSK_NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i); return false; } } return true; } else { JSK_NODELET_FATAL("%s is not array", param_name.c_str()); return false; } } else { JSK_NODELET_FATAL("no %s is available on parameter server", param_name.c_str()); return false; } return true; }
void PoseTracker::parseParameters(const XmlRpc::XmlRpcValue &leds) { ROS_ASSERT(leds.getType() == XmlRpc::XmlRpcValue::TypeArray); // if parameters is good, resize the matrix to account for number of leds used to track // 4 = id + 3Dposition local_points_=Eigen::MatrixXd::Zero(leds.size(),4); Nleds_ = leds.size(); for( int i = 0; i < Nleds_; ++i) { XmlRpc::XmlRpcValue current_led = leds[i]; // parse ID ROS_ASSERT(current_led.getType() == XmlRpc::XmlRpcValue::TypeStruct); if( current_led.hasMember("id") ) { ROS_ASSERT(current_led["id"].getType() == XmlRpc::XmlRpcValue::TypeInt); int id = current_led["id"]; local_points_(i, 0) = (double) id; } else { ROS_ERROR("No id value for the current led. Check the yaml configuration for this object"); return; } // parse position std::vector<double> position; if( current_led.hasMember("position") ) { for (int j = 0; j < current_led["position"].size(); ++j) { ROS_ASSERT(current_led["position"][j].getType() == XmlRpc::XmlRpcValue::TypeDouble); position.push_back( current_led["position"][j] ); } local_points_(i, 1) = position[0]; local_points_(i, 2) = position[1]; local_points_(i, 3) = position[2]; } else { ROS_ERROR("No double value for the position current led. Check the yaml configuration for this object, remember to use . dots to ensure double format"); return; } } ROS_INFO("Succesfully parsed all LED parameters!"); // std::cout << local_points_ << std::endl; return; }
double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) { // Make sure that the value we're looking at is either a double or an int. if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && value.getType() != XmlRpc::XmlRpcValue::TypeDouble) { std::string& value_string = value; ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", full_param_name.c_str(), value_string.c_str()); throw std::runtime_error("Values in the footprint specification must be numbers"); } return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); }
// Class constructor PanosContactPoints::PanosContactPoints(ModelPlane * parent) : GroundReaction(parent) { XmlRpc::XmlRpcValue list; int i, j, points; char paramMsg[50]; double temp[6]; // Read contact points number from parameter server if(!ros::param::getCached("airframe/contactPtsNo", contactPtsNo)) {ROS_FATAL("Invalid parameters for -/airframe/contactPtsNo- in param server!"); ros::shutdown();} // Create an appropriately sized matrix to contain contact point information pointCoords = (double*)malloc(sizeof(double) * contactPtsNo*3); // contact points coordinates in the body frame materialIndex = (double*)malloc(sizeof(double) * contactPtsNo*1); // contact points material type index springIndex = (double*)malloc(sizeof(double) * contactPtsNo*2); // contact points spring characteristics len=0.2; // Set coefficient of friction for each material frictForw[0] = 0.7; frictSide[0] = 0.7; frictForw[1] = 0.4; frictSide[1] = 0.4; frictForw[2] = 0.1; frictSide[2] = 1.0; frictForw[3] = 0.4; frictSide[3] = 0.4; //To update composite to ground firction coefficients! // Read contact points location and material from parameter server for (j = 0; j<contactPtsNo; j++) { //Distribute the data sprintf(paramMsg, "airframe/contactPoint%i", j+1); if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -/airframe/contactPoint- in param server!"); ros::shutdown();} for (i = 0; i < list.size(); ++i) { ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); temp[i]=list[i]; } pointCoords[j] = temp[0]; // Save body frame contact point coordinates pointCoords[j + contactPtsNo] = temp[1]; pointCoords[j + contactPtsNo*2] = temp[2]; materialIndex[j] = temp[3]; // A separate contact point material index array springIndex[j] = temp[4]; // And the spring constants springIndex[j + contactPtsNo] = temp[5]; } // Create and initialize spring contraction container spp = (double*)malloc(sizeof(double) * contactPtsNo); memset(spp, 0, sizeof(spp)); // Create and initialize previous spring contraction container sppprev = (double*)malloc(sizeof(double) * contactPtsNo); memset(sppprev, 0, sizeof(sppprev)); contact = false; // Create other matrices needed for calculations cpi_up = (double*)malloc(sizeof(double) * contactPtsNo*3); // upper spring end matrix cpi_down = (double*)malloc(sizeof(double) * contactPtsNo*3); // lower spring end matrix spd = (double*)malloc(sizeof(double) * contactPtsNo); // spring contraction speed }
void FootstepMarker::readPoseParam(ros::NodeHandle& pnh, const std::string param, tf::Transform& offset) { XmlRpc::XmlRpcValue v; geometry_msgs::Pose pose; if (pnh.hasParam(param)) { pnh.param(param, v, v); // check if v is 7 length Array if (v.getType() == XmlRpc::XmlRpcValue::TypeArray && v.size() == 7) { // safe parameter access by getXMLDoubleValue pose.position.x = getXMLDoubleValue(v[0]); pose.position.y = getXMLDoubleValue(v[1]); pose.position.z = getXMLDoubleValue(v[2]); pose.orientation.x = getXMLDoubleValue(v[3]); pose.orientation.y = getXMLDoubleValue(v[4]); pose.orientation.z = getXMLDoubleValue(v[5]); pose.orientation.w = getXMLDoubleValue(v[6]); // converst the message as following: msg -> eigen -> tf //void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e); Eigen::Affine3d e; tf::poseMsgToEigen(pose, e); // msg -> eigen tf::transformEigenToTF(e, offset); // eigen -> tf } else { ROS_ERROR_STREAM(param << " is malformed, which should be 7 length array"); } } else { ROS_WARN_STREAM("there is no parameter on " << param); } }
bool getNodes(V_string& nodes) { XmlRpc::XmlRpcValue args, result, payload; args[0] = this_node::getName(); if (!execute("getSystemState", args, result, payload, true)) { return false; } S_string node_set; for (int i = 0; i < payload.size(); ++i) { for (int j = 0; j < payload[i].size(); ++j) { XmlRpc::XmlRpcValue val = payload[i][j][1]; for (int k = 0; k < val.size(); ++k) { std::string name = payload[i][j][1][k]; node_set.insert(name); } } } nodes.insert(nodes.end(), node_set.begin(), node_set.end()); return true; }