Esempio n. 1
1
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());
	}
}
Esempio n. 2
0
  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));
      }
    }
  }
Esempio n. 3
0
  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;
}
Esempio n. 7
0
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);
				}
			}
		}
	}
}
Esempio n. 8
0
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 &params)
{
  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 &params)
 {
   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;
 }
Esempio n. 11
0
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;
}
Esempio n. 12
0
/*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;
}
Esempio n. 16
0
/**
 * 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;
}
Esempio n. 18
0
    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);
    }
Esempio n. 19
0
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);
        }
    }
}
Esempio n. 20
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;
}
Esempio n. 21
0
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;
}
Esempio n. 22
0
// 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 );
}
Esempio n. 23
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;
}
Esempio n. 24
0
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);
}
Esempio n. 28
0
// 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);
  }
}
Esempio n. 30
-1
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;
}