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());
	}
}
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;
 }
PulseMotorController::PulseMotorController(
    XmlRpc::XmlRpcValue &pulse_motor_info)
    : motor_regs_(NULL), start_count_(0), last_target_(0) {
    ROS_ASSERT(pulse_motor_info.getType() == XmlRpc::XmlRpcValue::TypeStruct);
    ROS_ASSERT(pulse_motor_info.hasMember("name"));
    ROS_ASSERT(pulse_motor_info.hasMember("dev_filename"));
    ROS_ASSERT(pulse_motor_info.hasMember("gpio_filename"));
    ROS_ASSERT(pulse_motor_info.hasMember("to_meter_fraction"));
    ROS_ASSERT(pulse_motor_info.hasMember("speed"));

    // Open device files
    name_ = (string)pulse_motor_info["name"];
    string dev_file = pulse_motor_info["dev_filename"];
    legal_checker_ =
        SwitchGPIOPtr(new SwitchGPIO(pulse_motor_info["gpio_filename"]));
    dev_fd_ = open(dev_file.c_str(), O_RDWR, 0);
    if (dev_fd_ < 0) {
        ROS_ERROR("Failed to open file %s for motor %s", dev_file.c_str(),
                  name_.c_str());
        return;
    }
    ROS_ASSERT(sizeof(unsigned) == 4);
    motor_regs_ =
        (unsigned *)mmap(NULL, sizeof(unsigned) * 4, PROT_READ | PROT_WRITE,
                         MAP_SHARED, dev_fd_, 0);
    if ((long long)motor_regs_ == -1) {
        close(dev_fd_);
        ROS_ERROR("Failed to create memory map for motor %s", name_.c_str());
        motor_regs_ = NULL;
        return;
    }

    // Check initial position
    printf("f**k 1\n");
    ROS_INFO("Motor %s suceessfully mapped file %s", name_.c_str(),
             dev_file.c_str());
    can_move_ = !legal_checker_->Get();
    printf("f**k 2\n");

    // Setup
    to_meter_fraction_ = pulse_motor_info["to_meter_fraction"];
    printf("f**k 3\n");
    ROS_INFO("get to_meter_fraction");
    if (!can_move_) ROS_WARN("Motor %s not at initial position", name_.c_str());
    speed_ = (unsigned)((int)pulse_motor_info["speed"]);
    printf("f**k 4\n");
    motor_regs_[kCTRL] = CTRL_DIR & ~CTRL_ENABLE;
    motor_regs_[kVEL] = speed_;
    start_count_ = motor_regs_[kFEEDBACK];
    printf("f**k 5\n");
    ROS_INFO("Pulse motor %s starts with count %u", name_.c_str(), start_count_);
}
Exemple #5
0
Servo::Servo(XmlRpc::XmlRpcValue& servo_info)
      : channel_(-1)
      , type_(RCSERVO_SERVO_DEFAULT)
      , joint_name_("")
      , trim_pwm_(0)
      , max_pwm_(2300)
      , min_pwm_(700)     
      , max_rot_(HALFPI)
      , min_rot_(-HALFPI)
      , bus_(PWM)
{
  if (servo_info.hasMember("joint_name"))
    joint_name_ = (std::string)servo_info["joint_name"];
  
	if (servo_info.hasMember("channel"))
  	channel_ = servo_info["channel"];

  if (servo_info.hasMember("type"))
  {
    std::string type_string = servo_info["type"];
    if (type_string == "RCSERVO_KONDO_KRS78X")
      type_ = RCSERVO_KONDO_KRS78X;
    else if (type_string == "RCSERVO_KONDO_KRS788")
      type_ = RCSERVO_KONDO_KRS788;
    else if (type_string == "RCSERVO_KONDO_KRS4014")
      type_ = RCSERVO_KONDO_KRS4014;      
    else if (type_string == "RCSERVO_HITEC_HSR8498")
      type_ = RCSERVO_HITEC_HSR8498;
    else if (type_string == "KONDO_ROS3030")
    	type_ = KONDO_ROS3030;
    else
      type_ = RCSERVO_SERVO_DEFAULT; 
  }   

  if (servo_info.hasMember("bus")) 
  {
    std::string bus_string = servo_info["bus"];
    if (bus_string == "COM4")
      bus_ = COM4;
  }

  if (servo_info.hasMember("min_rotation"))
    min_rot_ = (double)servo_info["min_rotation"];
  if (servo_info.hasMember("max_rotation"))
    max_rot_ = (double)servo_info["max_rotation"];  
    
  if (servo_info.hasMember("min_pwm"))
    min_pwm_ = (int)servo_info["min_pwm"];
  if (servo_info.hasMember("max_pwm"))
    max_pwm_ = (int)servo_info["max_pwm"];    
    
  if (servo_info.hasMember("trim"))
    trim_pwm_ = servo_info["trim"];  
}
Exemple #6
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;
}
Exemple #7
0
void CServo::setParams(XmlRpc::XmlRpcValue params)
{
    if(params.hasMember("id"))
    {
      id_=params["id"];
    }
    if(params.hasMember("max_angle_degrees"))
    {
      max_angle_=(double)radians(params["max_angle_degrees"]);
    }
    if(params.hasMember("min_angle_degrees"))
    {
      min_angle_=(double)radians(params["min_angle_degrees"]);
    }
    if(params.hasMember("max_angle_radians"))
    {
      max_angle_=(double)(params["max_angle_radians"]);
    }
    if(params.hasMember("min_angle_radians"))
    {
      min_angle_=(double)(params["min_angle_radians"]);
    }
    if(params.hasMember("max_speed"))
    {
      max_speed_=(double)params["max_speed"];
    }
    if(params.hasMember("range"))
    {
      range_=(double)radians(params["range"]);
    }
    if(params.hasMember("ticks"))
    {
      ticks_=params["ticks"];
    }
    if(params.hasMember("neutral"))
    {
      neutral_=params["neutral"];
    }
    if(params.hasMember("invert"))
    {
      invert_=params["invert"];
    }
    recalculateAngleParams();
}
void OccupancyMapUpdater::readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, double *value)
{
  if (params.hasMember(param_name))
  {
    if (params[param_name].getType() == XmlRpc::XmlRpcValue::TypeInt)
      *value = (int) params[param_name];
    else
      *value = (double) params[param_name];
  }
}
Exemple #9
0
bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, double& value)
{
  if (!config.hasMember(key))
  {
    return false;
  }
  XmlRpc::XmlRpcValue param = config[key];
  if (param.getType() != XmlRpc::XmlRpcValue::TypeDouble)
  {
    return false;
  }
  value = param;
  return true;
}
bool PublishedRigidBody::validateParam(XmlRpc::XmlRpcValue &config_node, const std::string &name)
{
  if (!config_node.hasMember(name))
  {
    return false;
  }

  if (config_node[name].getType() != XmlRpc::XmlRpcValue::TypeString)
  {
    return false;
  }

  return true;
}
bool PointCloudOctomapUpdater::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"))
      filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
  }
  catch (XmlRpc::XmlRpcException &ex)
  {
    ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
    return false;
  }

  return true;
}
Exemple #12
0
bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, std::vector<double>& double_array)
{
  if (!config.hasMember(key))
  {
    ROS_ERROR("XmlRpcValue does not contain key %s.", key.c_str());
    return false;
  }

  XmlRpc::XmlRpcValue d_array_xml = config[key];

  if (d_array_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("XmlRpcValue is not of type array.");
    return false;
  }

  double_array.clear();
  for (int i=0; i<d_array_xml.size(); ++i)
  {
    if (d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
        d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
    {
      ROS_ERROR("XmlRpcValue is neither a double nor a integer array.");
      return false;
    }
    double value = 0.0;
    if (d_array_xml[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
    {
      value = static_cast<double>(static_cast<int>(d_array_xml[i]));
    }
    else
    {
      value = static_cast<double>(d_array_xml[i]);
    }
    double_array.push_back(value);
  }

  return true;
}
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;
}
static void readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, unsigned int *value)
{
  if (params.hasMember(param_name))
    *value = (int) params[param_name];
}
void OccupancyMapUpdater::readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, unsigned int *value)
{
  if (params.hasMember(param_name))
    *value = (int) params[param_name];
}
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;
}
SolverConfiguration::SolverConfiguration(XmlRpc::XmlRpcValue& conf) {

  if (conf.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
    ROS_FATAL("solver: bad configuration, expecting structure.");
    ROS_BREAK();
  }

  if (!conf.hasMember("base_link_frame_id")
      || conf["base_link_frame_id"].getType()
          != XmlRpc::XmlRpcValue::TypeString) {

    std::cerr << conf["type"].getType() << std::endl;
    ROS_FATAL("solver: 'base_link_frame_id' field undefined or not a string");
    ROS_BREAK();
  }

  base_link_frame_id_ = static_cast<std::string &>(conf["base_link_frame_id"]);

  if (!conf.hasMember("global_frame_id")
      || conf["global_frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) {
    ROS_FATAL("solver: 'global_frame' field undefined or not a string");
    ROS_BREAK();
  }

  global_frame_ = static_cast<std::string &>(conf["global_frame_id"]);

  if (!conf.hasMember("pose_window_length")
      || conf["pose_window_length"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
    ROS_FATAL("solver: 'pose_window_length' field undefined or not a double");
    ROS_BREAK();
  }

  pose_window_length_ = static_cast<double>(conf["pose_window_length"]);

  if (pose_window_length_ < 0) {
    ROS_FATAL("solver: 'pose_window_length' field must be greater than zero");
    ROS_BREAK();
  }

  if (!conf.hasMember("dead_reckoning")
      || conf["dead_reckoning"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
    ROS_FATAL("solver: 'dead_reckoning' field undefined or not a boolean");
    ROS_BREAK();
  }

  dead_reckoning_ = static_cast<bool>(conf["dead_reckoning"]);

  if (!conf.hasMember("frequency")
      || conf["frequency"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
    ROS_FATAL("solver: 'frequency' field undefined or not a double");
    ROS_BREAK();
  }

  frequency_ = static_cast<double>(conf["frequency"]);

  if (!conf.hasMember("n_gauss_newton_iterations")
      || conf["n_gauss_newton_iterations"].getType()
          != XmlRpc::XmlRpcValue::TypeInt) {
    ROS_FATAL(
        "solver: 'n_gauss_newton_iterations' field undefined or not an integer");
    ROS_BREAK();
  }

  n_gauss_newton_iterations_ =
      static_cast<int>(conf["n_gauss_newton_iterations"]);

  if (!conf.hasMember("low_level_logging")
      || conf["low_level_logging"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
    ROS_FATAL("solver: 'low_level_logging' field undefined or not a boolean");
    ROS_BREAK();
  }

  low_level_logging_ = static_cast<bool>(conf["low_level_logging"]);
}
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;
    }
  }
}
bool createLinksFromParams(XmlRpc::XmlRpcValue &params, std::vector<LinkInfo> &links)
{
  double default_padding, default_scale;

  if(params.hasMember("self_see_default_padding"))
    default_padding = double(params["self_see_default_padding"]);
  else
    default_padding = 0.01;

  if(params.hasMember("self_see_default_scale"))
    default_scale = double(params["self_see_default_scale"]);
  else
    default_scale = 1.0;

  std::string link_names;

  if (!params.hasMember("self_see_links"))
  {
    ROS_WARN ("No links specified for self filtering.");
  }
  else
  {
    XmlRpc::XmlRpcValue ssl_vals = params["self_see_links"];

    if (ssl_vals.getType () != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_WARN ("Self see links need to be an array");
    }
    else
    {
      if (ssl_vals.size () == 0)
      {
        ROS_WARN ("No values in self see links array");
      }
      else
      {
        for (int i = 0; i < ssl_vals.size (); ++i)
        {
          robot_self_filter::LinkInfo li;

          if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
          {
            ROS_WARN ("Self see links entry %d is not a structure.  Stopping processing of self see links", i);
            return false;
          }
          if (!ssl_vals[i].hasMember ("name"))
          {
            ROS_WARN ("Self see links entry %d has no name.  Stopping processing of self see links", i);
            return false;
          }
          li.name = std::string (ssl_vals[i]["name"]);
          if (!ssl_vals[i].hasMember ("padding"))
          {
            ROS_DEBUG ("Self see links entry %d has no padding.  Assuming default padding of %g", i, default_padding);
            li.padding = default_padding;
          }
          else
          {
            li.padding = ssl_vals[i]["padding"];
          }
          if (!ssl_vals[i].hasMember ("scale"))
          {
            ROS_DEBUG ("Self see links entry %d has no scale.  Assuming default scale of %g", i, default_scale);
            li.scale = default_scale;
          }
          else
          {
            li.scale = ssl_vals[i]["scale"];
          }
          links.push_back (li);
        }
      }
    }
  }
  return true;
}
/**
 Constructor
 */
CButGraspingControls::CButGraspingControls(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi )
    : wxPanel( parent, wxID_ANY, wxDefaultPosition, wxSize(280, 180), wxTAB_TRAVERSAL, title)
    , m_wmi( wmi )
{

    parent_ = parent;

    ros::NodeHandle nh("~");

    ros::param::param<double>("~abs_max_force", abs_max_force_ , ABS_MAX_FORCE);
    ros::param::param<bool>("~wait_for_allow", wait_for_allow_ , WAIT_FOR_ALLOW);

    XmlRpc::XmlRpcValue pres;

    if (nh.getParam("presets",pres)) {

        ROS_ASSERT(pres.getType() == XmlRpc::XmlRpcValue::TypeArray);

        ROS_INFO("%d presets for assisted grasping plugin.",pres.size());

        for (int i=0; i < pres.size(); i++) {

            Preset pr;

            XmlRpc::XmlRpcValue xpr = pres[i];

            if (xpr.getType() != XmlRpc::XmlRpcValue::TypeStruct) {

                ROS_ERROR("Wrong syntax in YAML config.");
                continue;

            }

            // read the name
            if (!xpr.hasMember("name")) {

                ROS_ERROR("Preset doesn't have 'name' property defined.");
                continue;

            } else {

                XmlRpc::XmlRpcValue name = xpr["name"];

                std::string tmp = static_cast<std::string>(name);

                pr.name = tmp;

                std::cout << tmp << " ";

            }

            // read the time
            if (!xpr.hasMember("time")) {

                ROS_ERROR("Preset doesn't have 'time' property defined.");
                continue;

            } else {

                XmlRpc::XmlRpcValue time = xpr["time"];

                double tmp = static_cast<double>(time);

                pr.time = ros::Duration(tmp);

                std::cout << tmp << " ";

            }

            // read the forces
            if (!xpr.hasMember("forces")) {

                ROS_ERROR("Preset doesn't have 'forces' property defined.");
                continue;

            } else {

                XmlRpc::XmlRpcValue forces = xpr["forces"];

                if (forces.getType() == XmlRpc::XmlRpcValue::TypeArray) {

                    for (int i=0; i < forces.size(); i++) {

                        XmlRpc::XmlRpcValue f = forces[i];

                        double tmp = static_cast<double>(f);

                        pr.forces.push_back(tmp);

                    }


                } else {

                    ROS_ERROR("Property 'forces' is defined in bad way.");
                    continue;

                }

            } // else forces


            // read the positions
            if (!xpr.hasMember("target_pos")) {

                ROS_ERROR("Preset doesn't have 'target_pos' property defined.");
                continue;

            } else {

                XmlRpc::XmlRpcValue target_pos = xpr["target_pos"];

                if (target_pos.getType() == XmlRpc::XmlRpcValue::TypeArray) {

                    for (int i=0; i < target_pos.size(); i++) {

                        XmlRpc::XmlRpcValue p = target_pos[i];

                        double tmp = static_cast<double>(p);

                        pr.target_pos.push_back(tmp);

                    }


                } else {

                    ROS_ERROR("Property 'target_pos' is defined in bad way.");
                    continue;

                }

            } // else target_pos


            // TODO test number of forces / positions !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

            presets_.push_back(pr);

        } // for around all presets



    } else {

        ROS_ERROR("Could not get presets for grasping, using defaults.");

        Preset pr;
        pr.name = "Empty bottle";
        pr.forces.resize(6);

        for(int i=0; i < 6; i++) pr.forces[i] = 300.0;

        pr.target_pos.resize(7);

        pr.target_pos[0] = 0.0;
        pr.target_pos[1] = 0.0;
        pr.target_pos[2] = 1.0472;
        pr.target_pos[3] = 0.0;
        pr.target_pos[4] = 1.0472;
        pr.target_pos[5] = 0.0;
        pr.target_pos[6] = 1.0472;

        pr.time = ros::Duration(5.0);

        presets_.push_back(pr);

    }


    wxArrayString prt;

    for(unsigned int i=0; i < presets_.size(); i++)
        prt.Add(wxString::FromAscii(presets_[i].name.c_str()));

    presets_choice_ = new wxChoice(this, ID_CHOICE,wxDefaultPosition,wxDefaultSize,prt);

    buttons_["grasp"] = new wxButton(this, ID_BUTTON_GRASP, wxT("Grasp"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
    buttons_["stop"] = new wxButton(this, ID_BUTTON_STOP, wxT("Stop"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);

    buttons_["stop"]->SetForegroundColour (wxColour (255, 255, 255));
    buttons_["stop"]->SetBackgroundColour (wxColour (255, 108, 108));

    m_text_status_ = new wxStaticText(this, -1, wxT("status: idle"));

    stringstream ss (stringstream::in | stringstream::out);



    std::vector<double> f = presets_[presets_choice_->GetSelection()].forces;

    double max_f = 0.0;

    for(unsigned int i=0; i < f.size(); i++) {

        if (f[i] > max_f) max_f = f[i];

    }

    m_text_max_force_ = new wxStaticText(this, -1, wxT("Maximum force"));

    max_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE,(int)max_f,0,abs_max_force_,wxDefaultPosition,wxDefaultSize,wxSL_LABELS);
    max_force_slider_->Enable(false);


    finger1_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_1,0.0,0,abs_max_force_);
    finger2_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_2,0.0,0,abs_max_force_);
    finger3_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_3,0.0,0,abs_max_force_);

    tactile_data_.resize(6);

    wxSizer *hsizer_buttons = new wxBoxSizer(wxHORIZONTAL);

    wxSizer *vsizer_glob = new wxBoxSizer(wxVERTICAL);

    hsizer_buttons->Add(buttons_["grasp"], ID_BUTTON_GRASP);
    hsizer_buttons->Add(buttons_["stop"], ID_BUTTON_STOP);

    //wxSizer *vsizer = new wxBoxSizer(wxVERTICAL); // top sizer
    wxSizer *vsizer_top = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Object grasping"));

    vsizer_top->Add(hsizer_buttons,1,wxEXPAND);


    vsizer_top->Add(m_text_status_);
    vsizer_top->Add(presets_choice_);

    vsizer_top->Add(m_text_max_force_);
    vsizer_top->Add(max_force_slider_,1,wxEXPAND);



    wxSizer *vsizer_sliders = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Force feedback"));


    vsizer_sliders->Add(finger1_force_slider_,1,wxEXPAND);
    vsizer_sliders->Add(finger2_force_slider_,1,wxEXPAND);
    vsizer_sliders->Add(finger3_force_slider_,1,wxEXPAND);

    //vsizer->Add(vsizer_sliders,1,wxEXPAND);


    finger1_force_slider_->Enable(false);
    finger2_force_slider_->Enable(false);
    finger3_force_slider_->Enable(false);

    setButton("stop",false);

    grasping_allowed_ = false;

    if (wait_for_allow_) {

        DisableControls();

    } else {

        EnableControls();
    }

    /*Connect(ID_SLIDER_FORCE, wxEVT_SCROLL_THUMBRELEASE,
              wxCommandEventHandler(CButGraspingControls::OnMaxForceSlider));*/

    Connect(ID_CHOICE, wxEVT_COMMAND_CHOICE_SELECTED,
            wxCommandEventHandler(CButGraspingControls::OnChoice));

    vsizer_glob->Add(vsizer_top,1,wxEXPAND|wxHORIZONTAL);
    vsizer_glob->Add(vsizer_sliders,1,wxEXPAND|wxHORIZONTAL);


    //this->SetSizerAndFit(vsizer_top);

    vsizer_glob->SetSizeHints(this);
    this->SetSizerAndFit(vsizer_glob);

    as_client_ = new grasping_action_client(ACT_GRASP,true);

    //ros::NodeHandle nh;

    tactile_data_received_ = false;

    stop_gui_thread_ = false;

    t_gui_update = boost::thread(&CButGraspingControls::GuiUpdateThread,this);

    //gui_update_timer_ = nh.createTimer(ros::Duration(0.5),&CButGraspingControls::timerCallback,this);

    tact_sub_  = nh.subscribe("/sdh_controller/tactile_data_filtered", 10, &CButGraspingControls::TactileDataCallback,this);

    service_grasping_allow_ = nh.advertiseService(SRV_ALLOW,&CButGraspingControls::GraspingAllow,this);


}