hsvColorRange::hsvColorRange(XmlRpc::XmlRpcValue _params)
{
    ROS_ASSERT(_params.getType()==XmlRpc::XmlRpcValue::TypeStruct);

    for (XmlRpc::XmlRpcValue::iterator i=_params.begin(); i!=_params.end(); ++i)
    {
        ROS_ASSERT(i->second.getType()==XmlRpc::XmlRpcValue::TypeArray);
        for(int j=0; j<i->second.size(); ++j)
        {
            ROS_ASSERT(i->second[j].getType()==XmlRpc::XmlRpcValue::TypeInt);
        }
        // printf("%s %i %i\n", i->first.c_str(), static_cast<int>(i->second[0]), static_cast<int>(i->second[1]));
        if (i->first == "H") H=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
        if (i->first == "S") S=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
        if (i->first == "V") V=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
    }
}
Esempio n. 2
0
bool GainClient::readGains(XmlRpc::XmlRpcValue& config,
                           sl_controller_msgs::CartesianGains& position_gains,
                           sl_controller_msgs::CartesianGains& force_gains)
{
  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Gains must be specified as an array");
    return false;
  }

  if (config.size() != 6)
  {
    ROS_ERROR("Gains must be be an array of six characters");
    return false;
  }

  char v[6];
  for (int i=0; i<6; ++i)
  {
    if (config[i].getType() != XmlRpc::XmlRpcValue::TypeString)
    {
      ROS_ERROR("Gains must be be an array of six characters");
      return false;
    }
    std::string s = config[i];
    v[i] = s.at(0);
    if (v[i] != 'f' && v[i] !='p' && v[i]!='0')
    {
      ROS_ERROR("Gains must be either 'p', 'f', or '0'");
      return false;
    }
  }

  setGainElement(v[0], position_gains.pos_x.p_gain, force_gains.pos_x.p_gain);
  setGainElement(v[1], position_gains.pos_y.p_gain, force_gains.pos_y.p_gain);
  setGainElement(v[2], position_gains.pos_z.p_gain, force_gains.pos_z.p_gain);
  setGainElement(v[3], position_gains.rot_x.p_gain, force_gains.rot_x.p_gain);
  setGainElement(v[4], position_gains.rot_y.p_gain, force_gains.rot_y.p_gain);
  setGainElement(v[5], position_gains.rot_z.p_gain, force_gains.rot_z.p_gain);

  setIGainsFromPGains(position_gains);
  setDGainsFromPGains(position_gains);
  setIGainsFromPGains(force_gains);
  setDGainsFromPGains(force_gains);
  return true;
}
bool NC2010TransformationSystem::initOneDimensionalTransformationSystemHelper(std::vector<dmp_lib::NC2010TSPtr>& transformation_systems,
                                                                                XmlRpc::XmlRpcValue transformation_systems_parameters_xml,
                                                                                ros::NodeHandle& node_handle,
                                                                                dmp_lib::TransformationSystem::IntegrationMethod integration_method)
{
  ROS_ASSERT(transformation_systems_parameters_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for (int j = 0; j < transformation_systems_parameters_xml.size(); ++j)
  {
    ROS_DEBUG("Initializing NC2010 transformation system number >%i< from node handle.",j);

    // create transformation system
    dmp_lib::NC2010TSPtr transformation_system(new dmp_lib::NC2010TransformationSystem());

    XmlRpc::XmlRpcValue ts_xml = transformation_systems_parameters_xml[j];
    ROS_ASSERT(ts_xml.hasMember(transformation_system->getVersionString()));
    XmlRpc::XmlRpcValue nc2010_ts_xml = ts_xml[transformation_system->getVersionString()];

    double k_gain = 0;
    if (!usc_utilities::getParam(nc2010_ts_xml, "k_gain", k_gain))
    {
      return false;
    }
    double d_gain = dmp_lib::NC2010TransformationSystem::getDGain(k_gain);

    // create parameters
    dmp_lib::NC2010TSParamPtr parameters(new dmp_lib::NC2010TransformationSystemParameters());

    // initialize parameters
    ROS_VERIFY(parameters->initialize(k_gain, d_gain));

    // initialize base class
    ROS_VERIFY(TransformationSystem::initFromNodeHandle(parameters, ts_xml, node_handle));

    // create state
    dmp_lib::NC2010TSStatePtr state(new dmp_lib::NC2010TransformationSystemState());

    // initialize transformation system with parameters and state
    ROS_VERIFY(transformation_system->initialize(parameters, state, integration_method));
    ROS_VERIFY(transformation_system->setIntegrationMethod(integration_method));

    transformation_systems.push_back(transformation_system);
  }
  return true;
}
Esempio n. 4
0
void StompOptimizationTask::setFeaturesFromXml(const XmlRpc::XmlRpcValue& features_xml)
{
  ROS_ASSERT (features_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);

  std::vector<boost::shared_ptr<StompCostFeature> > features;

  for (int i=0; i<features_xml.size(); ++i)
  {
    XmlRpc::XmlRpcValue feature_xml = features_xml[i];

    ROS_ASSERT(feature_xml.hasMember("class") &&
               feature_xml["class"].getType() == XmlRpc::XmlRpcValue::TypeString);

    std::string class_name = feature_xml["class"];

    boost::shared_ptr<StompCostFeature> feature;
    try
    {
      feature = feature_loader_.createInstance(class_name);
    }
    catch (pluginlib::PluginlibException& ex)
    {
      ROS_ERROR("Couldn't load feature named %s", class_name.c_str());
      ROS_ERROR("Error: %s", ex.what());
      ROS_BREAK();
    }

    STOMP_VERIFY(feature->initialize(feature_xml, num_rollouts_+1,
                                     planning_group_name_,
                                     kinematic_model_,
                                     collision_robot_, collision_world_,
                                     collision_robot_df_, collision_world_df_));
    features.push_back(feature);
  }

  setFeatures(features);

}
Esempio n. 5
0
inline bool parameterToColor(const ros::NodeHandle& node, const
    std::string& key, float* color) {
  XmlRpc::XmlRpcValue value;
  node.getParam(key, value);
  if (value.getType() == XmlRpc::XmlRpcValue::TypeArray) {
    if (value.size() == 3) {
      color[0] = static_cast<double>(value[0]);
      color[1] = static_cast<double>(value[1]);
      color[2] = static_cast<double>(value[2]);
   }
    else {
      ROS_WARN("Invalid array size for color parameter %s: %d", key.c_str(),
        (unsigned int)value.size());
      return false;
    }
  }
  else {
    ROS_WARN("Invalid type for color parameter %s: expecting array",
      key.c_str(), (unsigned int)value.size());
    return false;
  }
  
  return true;
}
  inline std::vector<std::string> getVectorParam(std::string name)
  {
    std::vector<std::string> values;
    XmlRpc::XmlRpcValue list;
    if (!root_nh_.getParamCached(name, list)) 
    {
      ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
    }
    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Hand description: bad parameter %s", name.c_str());
    }
    //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
    for (int32_t i=0; i<list.size(); i++)
    {
      if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
	ROS_ERROR("Hand description: bad parameter %s", name.c_str());
      }
      values.push_back( static_cast<std::string>(list[i]) );
      //ROS_INFO_STREAM("  " << values.back());
    }
    return values;	
  }
TimeWarpNode::TimeWarpNode()
 : m_nh("~")
 , m_live(true)
 , m_tfHandler(this)
{
	m_srv = m_nh.advertiseService("control", &TimeWarpNode::handleTimeWarpControl, this);

	m_pub_clock = m_nh.advertise<rosgraph_msgs::Clock>("/clock", 1);

	m_timer = m_nh.createTimer(ros::Duration(0.1), boost::bind(&TimeWarpNode::update, this));
	m_timer.start();

	XmlRpc::XmlRpcValue list;
	if(m_nh.getParam("extra_topics", list))
	{
		ROS_INFO("Using extra_topics parameter");
		ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);

		for(int i = 0; i < list.size(); ++i)
		{
			std::string name = static_cast<std::string>(list[i]);

			ROS_INFO("Warping extra topic '%s'", name.c_str());

			m_handlers.push_back(
				new TopicHandler(&m_nh, name)
			);
		}
	}

	m_handlers.push_back(
		new SmartTopicHandler<sensor_msgs::JointState>(&m_nh, "/joint_states")
	);

	m_topicThread = TopicThread::launch(m_nh, boost::bind(&TimeWarpNode::updateTopics, this, _1));
}
Esempio n. 8
0
YAML::Node XmlToYaml( XmlRpc::XmlRpcValue& xml )
{
	YAML::Node yaml;
	
	if( xml.getType() != XmlRpc::XmlRpcValue::TypeStruct ) { return yaml; }
	
	XmlRpc::XmlRpcValue::iterator iter;
	for( iter = xml.begin(); iter != xml.end(); iter++ )
	{
		std::string name = iter->first;
		XmlRpc::XmlRpcValue payload = iter->second;
		if( payload.getType() == XmlRpc::XmlRpcValue::TypeStruct )
		{
			yaml[name] = XmlToYaml( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeArray )
		{
			if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeBoolean ) )
			{
				std::vector<bool> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<bool>( payload[i]) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeInt ) )
			{
				std::vector<int> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<int>( payload[i]) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeDouble ) )
			{
				std::vector<double> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					
					s.push_back( ParseDouble( payload[i] ) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeString ) )
			{
				std::vector<std::string> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<std::string>( payload[i]) );
				}
				yaml[name] = s;
			}
			else
			{
				std::cerr << "Invalid array type." << std::endl;
			}
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeBoolean )
		{
			yaml[name] = static_cast<bool>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeInt )
		{
			yaml[name] = static_cast<int>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeDouble )
		{
			yaml[name] = static_cast<double>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeString )
		{
			yaml[name] = static_cast<std::string>( payload );
		}
		else
		{
			std::cerr << "Unsupported conversion type." << std::endl;
			continue;
		}
	}
	return yaml;
}
MidemUserInteraction::MidemUserInteraction(QMainWindow *parent) :
  QMainWindow(parent),
  ui(new Ui::MidemUserInteraction),
  quit_thread_(false),
  nhp_("~"),
  working_frame_("/base_link"),
  gesture_detector_loader_("midem_user_interaction","hg_gesture_detector::GestureDetector")
{
  ui->setupUi(this);
  dynamic_reconfigure::Server<midem_user_interaction::MidemUserInteractionConfig>::CallbackType f;
  f = boost::bind(&MidemUserInteraction::callbackConfig, this, _1, _2);
  reconfigure_server_.setCallback(f);

  arms_sub_ = nh_.subscribe("arms_msg", 1, &MidemUserInteraction::callbackArms, this);
  arm_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("arm_gestures", 1);

  skeletons_sub_ = nh_.subscribe("skeletons_msg", 1, &MidemUserInteraction::callbackSkeletons, this);
  skeleton_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("skeleton_gestures", 1);

  arms_syn_sub_.subscribe(nh_, "arms_msg", 10);
  skeletons_syn_sub_.subscribe(nh_, "skeletons_msg", 10);
  arm_skeleton_sync_.reset(new message_filters::Synchronizer<ArmBodyAppoxSyncPolicy>(ArmBodyAppoxSyncPolicy(10), arms_syn_sub_, skeletons_syn_sub_));
  arm_skeleton_sync_->registerCallback(boost::bind(&MidemUserInteraction::callbackArmsSkelentons, this, _1, _2));

  arm_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("arm_gesture_markers", 1);
  skeleton_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("skeleton_gesture_markers", 1);

  XmlRpc::XmlRpcValue gesture_detector;
  nhp_.getParam("gesture_detector", gesture_detector);
  if(gesture_detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
  {
    ROS_ERROR("invalid YAML structure");
    return;
  }

  ROS_INFO_STREAM("load " << gesture_detector.size() << " gesture detector(s)");
  for(XmlRpc::XmlRpcValue::iterator it = gesture_detector.begin(); it != gesture_detector.end(); it++)
  {
    ROS_INFO_STREAM("detector name: " << it->first);
    XmlRpc::XmlRpcValue detector;
    nhp_.getParam("gesture_detector/" + it->first, detector);
    if(detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_ERROR("invalid YAML structure");
      return;
    }

    if(detector.begin()->first != "type")
    {
      ROS_ERROR("invalid YAML structure");
      return;
    }

    ROS_INFO_STREAM("         type: " << detector.begin()->second);
    try
    {
      gesture_detector_map_[it->first] = gesture_detector_loader_.createInstance(detector.begin()->second);
      gesture_detector_map_[it->first]->setName(it->first);
      if(!gesture_detector_map_[it->first]->initialize())
      {
        ROS_ERROR_STREAM("Cannot initialize detector " << it->first);
        gesture_detector_map_.erase(it->first);
      }
    }
    catch(pluginlib::PluginlibException& ex)
    {
      ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
    }

  }
}
void JointStateTorqueSensorController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
{

  // Preconditions
  XmlRpc::XmlRpcValue list;
  if (!nh.getParam("extra_joints", list))
  {
    ROS_DEBUG("No extra joints specification found.");
    return;
  }

  if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Extra joints specification is not an array. Ignoring.");
    return;
  }

  for(std::size_t i = 0; i < list.size(); ++i)
  {
    if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
                       "'. Ignoring.");
      continue;
    }

    if (!list[i].hasMember("name"))
    {
      ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
      continue;
    }

    const std::string name = list[i]["name"];
    if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
    {
      ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
      continue;
    }

    const bool has_pos = list[i].hasMember("position");
    const bool has_vel = list[i].hasMember("velocity");
    const bool has_eff = list[i].hasMember("effort");

    const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
    if (has_pos && list[i]["position"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
      continue;
    }
    if (has_vel && list[i]["velocity"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
      continue;
    }
    if (has_eff && list[i]["effort"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
      continue;
    }

    // State of extra joint
    const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
    const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
    const double eff = has_eff ? static_cast<double>(list[i]["effort"])   : 0.0;

    // Add extra joints to message
    msg.name.push_back(name);
    msg.position.push_back(pos);
    msg.velocity.push_back(vel);
    msg.effort.push_back(eff);
  }
}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "standalone_complexed_nodelet");
  ros::NodeHandle private_nh("~");
  ros::NodeHandle nh;
  nodelet::Loader manager(false); // Don't bring up the manager ROS API
  nodelet::V_string my_argv;  
  std::vector<std::string> candidate_root;
  candidate_root.push_back("nodelets");
  int candidate_num;
  private_nh.param("candidate_num", candidate_num, 100);
  for (size_t i = 0; i < candidate_num; i++) {
    candidate_root.push_back((boost::format("nodelets_%lu") % i).str());
  }
  for (size_t i_candidate = 0; i_candidate < candidate_root.size(); i_candidate++) {
    std::string root_name = candidate_root[i_candidate];
    if (private_nh.hasParam(root_name)) {
      XmlRpc::XmlRpcValue nodelets_values;
      private_nh.param(root_name, nodelets_values, nodelets_values);
      if (nodelets_values.getType() == XmlRpc::XmlRpcValue::TypeArray) {
        for (size_t i_nodelet = 0; i_nodelet < nodelets_values.size(); i_nodelet++) {
          ROS_INFO("i_nodelet %lu", i_nodelet);
          XmlRpc::XmlRpcValue onenodelet_param = nodelets_values[i_nodelet];
          if (onenodelet_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
            std::string name, type;
            nodelet::M_string remappings;
            if (onenodelet_param.hasMember("if") && !(bool)onenodelet_param["if"]) {
              continue;
            }
            if (onenodelet_param.hasMember("unless") && (bool)onenodelet_param["unless"]) {
              continue;
            }
            if (onenodelet_param.hasMember("name")) {
              name = nh.resolveName((std::string)onenodelet_param["name"]);
            }
            else {
              ROS_FATAL("element ~nodelets should have name field");
              return 1;
            }
            if (onenodelet_param.hasMember("type")) {
              type = (std::string)onenodelet_param["type"];
            }
            else {
              ROS_FATAL("element ~nodelets should have type field");
              return 1;
            }
            if (onenodelet_param.hasMember("remappings")) {
              XmlRpc::XmlRpcValue remappings_params
                = onenodelet_param["remappings"];
              if (remappings_params.getType() == XmlRpc::XmlRpcValue::TypeArray) {
                for (size_t remappings_i = 0; remappings_i < remappings_params.size(); remappings_i++) {
                  XmlRpc::XmlRpcValue remapping_element_param = remappings_params[remappings_i];
                  if (remapping_element_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
                    if (remapping_element_param.hasMember("from") && remapping_element_param.hasMember("to")) {
                      std::string from = (std::string)remapping_element_param["from"];
                      std::string to = (std::string)remapping_element_param["to"];
                      if (from.size() > 0 && from[0] == '~') {
                        ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
                        from = nodelet_private_nh.resolveName(from.substr(1, from.size() - 1));
                      }
                      else {
                        ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
                        from = nodelet_nh.resolveName(from);
                      }
                      if (to.size() > 0 && to[0] == '~') {
                        ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
                        to = nodelet_private_nh.resolveName(to.substr(1, to.size() - 1));
                      }
                      else {
                        ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
                        to = nodelet_nh.resolveName(to);
                      }
                      ROS_INFO("remapping: %s => %s", from.c_str(), to.c_str());
                      remappings[from] = to;
                    }
                    else {
                      ROS_FATAL("remappings parameter requires from and to fields");
                      return 1;
                    }
                  }
                  else {
                    ROS_FATAL("remappings should be an array");
                    return 1;
                  }
                }
              }
              else {
                ROS_FATAL("remappings should be an array");
                return 1;
              }
            }
            if (onenodelet_param.hasMember("params")) {
              if (onenodelet_param["params"].getType() != XmlRpc::XmlRpcValue::TypeArray) {
                ROS_FATAL("params parameter must be an array");
                return 1;
              }
              XmlRpc::XmlRpcValue values;
              for (int params_i = 0; params_i < onenodelet_param["params"].size(); ++params_i) {
                XmlRpc::XmlRpcValue oneparam = onenodelet_param["params"][params_i];
                if (!(oneparam.getType() == XmlRpc::XmlRpcValue::TypeStruct &&
                      oneparam.hasMember("name") && oneparam.hasMember("value"))) {
                  ROS_FATAL("each 'params' element must be a struct which contains 'name' and 'value' fields.");
                  return 1;
                }
                std::string key = oneparam["name"];
                values[key] = oneparam["value"];
                ROS_INFO_STREAM("setparam: " << name << "/" << key << " => " << values[key]);
              }
              ros::param::set(name, values);
            }
            // Done reading parmaeter for one nodelet
          
            if (!manager.load(name, type, remappings, my_argv)) {
              ROS_ERROR("Failed to load nodelet [%s -- %s]", name.c_str(), type.c_str());
            }
            else {
              ROS_INFO("Succeeded to load nodelet [%s -- %s]", name.c_str(), type.c_str());
            }
          }
          else {
            ROS_FATAL("element ~nodelets should be a dictionay");
            return 1;
          }
        }
      }
      else {
        ROS_FATAL("~nodelets should be a list");
        return 1;
      }
    }
  }
  ROS_INFO("done reading parmaeters");
  std::vector<std::string> loaded_nodelets = manager.listLoadedNodelets();
  ROS_INFO("loaded nodelets: %lu", loaded_nodelets.size());
  for (size_t i = 0; i < loaded_nodelets.size(); i++) {
    ROS_INFO("loaded nodelet: %s", loaded_nodelets[i].c_str());
  }
  ros::spin();
  return 0;
}
bool CartesianComplianceController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle) {
	using namespace XmlRpc;
	this->nodeHandle = nodeHandle;
	this->robotPtr = robotPtr;

	ROS_INFO("Initializing interaction control for the youbot arm...\n");

	// Gets all of the joint pointers from the RobotState to a joints vector
	XmlRpc::XmlRpcValue jointNames;
	if (!nodeHandle.getParam("joints", jointNames)) {
		ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray) {
		ROS_ERROR("Malformed joint specification.  (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i) {
		XmlRpcValue &name = jointNames[i];
		if (name.getType() != XmlRpcValue::TypeString) {
			ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", nodeHandle.getNamespace().c_str());
			return false;
		}

		pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
		if (jointStatePtr == NULL) {
			ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}

		joints.push_back(jointStatePtr);
	}

	// Ensures that all the joints are calibrated.
	for (unsigned int i = 0; i < joints.size(); ++i) {
		if (!joints[i]->calibrated_) {
			ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}
	}

	// Initializing target efforts vector
	targetEfforts.resize(joints.size());

	// Subscribing for an input pose command
	subscriber = nodeHandle.subscribe("command", 1, &CartesianComplianceController::positionCommand, this);

	// Initializing 20Sim controller
	init20SimController();

	// Initializing twist publisher for the base
	ROS_INFO("base_confghfhgtroller/command\n");
	baseTwist.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist > (nodeHandle, "base_controller/command", 1));
	baseTwist->lock();

	subscriberOdometry = nodeHandle.subscribe("base_odometry/odometry", 1, &CartesianComplianceController::odometryCommand, this);


	// Initializing debug output
	debugInfo->init();

	return true;
}
bool JointPositionController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle)
{
    using namespace XmlRpc;
    this->nodeHandle = nodeHandle;
    this->robotPtr = robotPtr;



    ROS_DEBUG("Initializing joint position control...\n");

    // Gets all of the joint pointers from the RobotState to a joints vector
    XmlRpc::XmlRpcValue jointNames;
    if (!nodeHandle.getParam("joints", jointNames))
    {
        ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
        return false;
    }

    if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
        ROS_ERROR("Malformed joint specification.  (namespace: %s)", nodeHandle.getNamespace().c_str());
        return false;
    }

    for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i)
    {
        XmlRpcValue &name = jointNames[i];
        if (name.getType() != XmlRpcValue::TypeString)
        {
            ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", nodeHandle.getNamespace().c_str());
            return false;
        }

        pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
        if (jointStatePtr == NULL)
        {
            ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }

        joints.push_back(jointStatePtr);
    }

    // Ensures that all the joints are calibrated.
    for (unsigned int i = 0; i < joints.size(); ++i)
    {
        if (!joints[i]->calibrated_)
        {
            ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }
    }

    // Initializing targetVelocities vector
    targetPositions.resize(joints.size());

    // Sets up pid controllers for all of the joints from yaml file
    std::string gainsNS;

    if (!nodeHandle.getParam("gains", gainsNS))
        gainsNS = nodeHandle.getNamespace() + "/gains";

    ROS_DEBUG("gains: %s\n", gainsNS.c_str());

    pids.resize(joints.size());

    ROS_DEBUG("joints.size() = %d \n", joints.size());

    for (unsigned int i = 0; i < joints.size(); ++i)
    {
	ROS_DEBUG("processing  %s/%s \n", gainsNS.c_str(), joints[i]->joint_->name.c_str());

        if (!pids[i].init(ros::NodeHandle(gainsNS + "/" + joints[i]->joint_->name)))
        {
            ROS_ERROR("Can't setup PID for the joint %s. (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }

        double p, i, d, i_max, i_min;
        pids[i].getGains(p, i, d, i_max, i_min);
        ROS_DEBUG("PID for joint %s: p=%f, i=%f, d=%f, i_max=%f, i_min=%f\n", joints[i]->joint_->name.c_str(), p, i, d, i_max, i_min);
    }

    subscriber = nodeHandle.subscribe("position_command", 1, &JointPositionController::positionCommand, this);

    return true;
}
Esempio n. 14
0
    bool initialize()
    {
        XmlRpc::XmlRpcValue val;
        std::vector<std::string> static_joints;
        
        if (private_nh_.getParam("static_joints", val))
        {
            if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
            {
                ROS_ERROR("static_joints parameter is not a list");
                return false;
            }
            
            for (int i = 0; i < val.size(); ++i)
            {
                static_joints.push_back(static_cast<std::string>(val[i]));
            }
        }
        
        std::vector<std::string> controller_names;
        
        if (private_nh_.getParam("controllers", val))
        {
            if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
            {
                ROS_ERROR("controllers parameter is not a list");
                return false;
            }
            
            for (int i = 0; i < val.size(); ++i)
            {
                controller_names.push_back(static_cast<std::string>(val[i]));
            }
        }
        
        int num_static = static_joints.size();
        int num_controllers = controller_names.size();
        int num_total = num_static + num_controllers;
        
        msg_.name.resize(num_total);
        msg_.position.resize(num_total);
        msg_.velocity.resize(num_total);
        msg_.effort.resize(num_total);
        
        for (int i = 0; i < num_static; ++i)
        {
            msg_.name[i] = static_joints[i];
            msg_.position[i] = 0.0;
            msg_.velocity[i] = 0.0;
            msg_.effort[i] = 0.0;
        }
        
        controller_state_subs_.resize(num_controllers);
        
        for (int i = 0; i < num_controllers; ++i)
        {
            controller_state_subs_[i] =
				nh_.subscribe<dynamixel_hardware_interface::JointState>(controller_names[i] +  "/state", 100,
                boost::bind(&JointStateAggregator::processControllerState, this, _1, i+num_static));
        }
        
        for (int i = 0; i < num_controllers; ++i)
        {
            ros::topic::waitForMessage<dynamixel_hardware_interface::JointState>(controller_names[i] +  "/state");
        }
        
        joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
        
        return true;
    }
Esempio n. 15
0
bool GridMapVisualization::readParameters()
{
  nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map"));

  double activityCheckRate;
  nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0);
  activityCheckDuration_.fromSec(1.0 / activityCheckRate);
  ROS_ASSERT(!activityCheckDuration_.isZero());

  // Configure the visualizations from a configuration stored on the parameter server.
  XmlRpc::XmlRpcValue config;
  if (!nodeHandle_.getParam(visualizationsParameter_, config)) {
    ROS_WARN(
        "Could not load the visualizations configuration from parameter %s,are you sure it"
        "was pushed to the parameter server? Assuming that you meant to leave it empty.",
        visualizationsParameter_.c_str());
    return false;
  }

  // Verify proper naming and structure,
  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) {
    ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d",
              visualizationsParameter_.c_str(), config.getType());
    ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str());
    return false;
  }

  // Iterate over all visualizations (may be just one),
  for (unsigned int i = 0; i < config.size(); ++i) {
    if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
      ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
                visualizationsParameter_.c_str(), config[i].getType());
      return false;
    } else if (!config[i].hasMember("type")) {
      ROS_ERROR("%s: Could not add a visualization because no type was given",
                visualizationsParameter_.c_str());
      return false;
    } else if (!config[i].hasMember("name")) {
      ROS_ERROR("%s: Could not add a visualization because no name was given",
                visualizationsParameter_.c_str());
      return false;
    } else {
      //Check for name collisions within the list itself.
      for (int j = i + 1; j < config.size(); ++j) {
        if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
          ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
                    visualizationsParameter_.c_str(), config[j].getType());
          return false;
        }

        if (!config[j].hasMember("name")
            || config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
            || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) {
          ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d",
                    visualizationsParameter_.c_str(), config[i].getType(), config[j].getType());
          return false;
        }

        std::string namei = config[i]["name"];
        std::string namej = config[j]["name"];
        if (namei == namej) {
          ROS_ERROR("%s: A visualization with the name '%s' already exists.",
                    visualizationsParameter_.c_str(), namei.c_str());
          return false;
        }
      }
    }

    // Make sure the visualization has a valid type.
    if (!factory_.isValidType(config[i]["type"])) {
      ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str());
      return false;
    }
  }

  for (int i = 0; i < config.size(); ++i) {
    std::string type = config[i]["type"];
    std::string name = config[i]["name"];
    auto visualization = factory_.getInstance(type, name);
    visualization->readParameters(config[i]);
    visualizations_.push_back(visualization);
    ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.",
             visualizationsParameter_.c_str(), type.c_str(), name.c_str());
  }

  return true;
}
Esempio n. 16
0
    ROS_ASSERT(camera_xml["name"].getType() == XmlRpc::XmlRpcValue::TypeString);
    ROS_ASSERT(camera_xml["topic"].getType() == XmlRpc::XmlRpcValue::TypeString);
    ROS_ASSERT(camera_xml["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);

    camera_definitions.push_back(CameraDefinition(static_cast<std::string>(camera_xml["name"]),
						  static_cast<std::string>(camera_xml["topic"]),
						  static_cast<std::string>(camera_xml["frame_id"])));
  }
  ROS_INFO("Loaded cameras:");
  BOOST_FOREACH(const CameraDefinition& camera, camera_definitions) {
    ROS_INFO_STREAM("\t" << camera.name << ": topic: " << camera.topic << ", frame_id: " << camera.frame_id);
  }

  XmlRpc::XmlRpcValue configs_xml;
  pnh.getParam("configs", configs_xml);
  ROS_ASSERT(configs_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for(int i = 0; i < configs_xml.size(); ++i) {
    XmlRpc::XmlRpcValue config_xml = configs_xml[i];
    ROS_ASSERT(config_xml.getType() == XmlRpc::XmlRpcValue::TypeStruct);
    ROS_ASSERT(config_xml["width"].getType() == XmlRpc::XmlRpcValue::TypeInt);
    ROS_ASSERT(config_xml["height"].getType() == XmlRpc::XmlRpcValue::TypeInt);
    ROS_ASSERT(config_xml["fps"].getType() == XmlRpc::XmlRpcValue::TypeInt);

    camera_configs_.push_back(CameraConfiguration(static_cast<int>(config_xml["width"]),
						  static_cast<int>(config_xml["height"]),
						  static_cast<int>(config_xml["fps"])));
  }
  ROS_INFO("Loaded camera configurations:");
  BOOST_FOREACH(const CameraConfiguration& config, camera_configs_) {
    ROS_INFO_STREAM("\twidth: " << config.width << ", height: " << config.height << ", fps: " << config.fps);
  }
void EventsGenerator::parseExploitationMatches(const ros::NodeHandle& n)
{
    if (!n.hasParam("exploitation_matches")) {
        return;
    }

    ROS_INFO("Parsing parameters for exploitation matches ...");

    XmlRpc::XmlRpcValue node;
    n.getParam("exploitation_matches", node);

    if (node.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        ROS_ERROR("'exploitation_matches' is not an array, ignoring.");
        return;
    }

    for (int i = 0; i != node.size(); ++i) {
        XmlRpc::XmlRpcValue& topic_node  = node[i];

        if (!topic_node.hasMember("topic_name")) {
            ROS_ERROR("Topic node has no topic_name, skipping.");
            continue;
        }

        if (!topic_node.hasMember("matches")) {
            ROS_ERROR("Topic node has no matches, skipping.");
            continue;
        }

        const std::string&   topic_name   = topic_node["topic_name"];
        XmlRpc::XmlRpcValue& matches_node = topic_node["matches"];

        if (matches_node.getType() != XmlRpc::XmlRpcValue::TypeArray) {
            ROS_ERROR("Matches for '%s' is not an array, skipping.",
                      topic_name.c_str());
            continue;
        }

        std::vector<hbba_msgs::ExploitationMatch> matches;
        matches.reserve(matches_node.size());
        for (int j = 0; j < matches_node.size(); ++j) {
            XmlRpc::XmlRpcValue& elem = matches_node[j];
            if (elem.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
                ROS_ERROR("Element for '%s' is not a proper struct, skipping.",
                          topic_name.c_str());
                continue;
            }

            if (!elem.hasMember("priority")) {
                ROS_ERROR("Element for '%s' does not contain a priority, "
                          "skipping.",
                          topic_name.c_str());
                continue;
            }

            int priority = static_cast<int>(elem["priority"]);

            if (!elem.hasMember("desire_type")) {
                ROS_ERROR("Element for priority %i of '%s' does not contain a "
                          "desire type, skipping.",
                          priority,
                          topic_name.c_str());
                continue;
            }

            XmlRpc::XmlRpcValue& desire_types = elem["desire_type"];
            if (desire_types.getType() != XmlRpc::XmlRpcValue::TypeArray) {
                ROS_ERROR("Desire type for priority %i of '%s' is not an "
                          "array, skipping.",
                          priority,
                          topic_name.c_str());
                continue;
            }

            hbba_msgs::ExploitationMatch match;
            match.priority = priority;
            match.classes.reserve(desire_types.size());
            for (int k = 0; k < desire_types.size(); ++k) {
                XmlRpc::XmlRpcValue& dtype = desire_types[k];
                if (dtype.getType() != XmlRpc::XmlRpcValue::TypeString) {
                    ROS_ERROR("Desire type in '%s' is not a string, skipping.",
                              topic_name.c_str());
                    continue;
                }

                match.classes.push_back(dtype);
            }

            matches.push_back(match);
        }

        if (matches.size()) {
            cem(topic_name, matches);
        }
    }
}
bool ChainParser::parseChain(XmlRpc::XmlRpcValue& chain_description, Tree tree, urdf::Model& robot_model,
                               std::map<std::string, unsigned int>& joint_name_to_index,
                               std::vector<std::string>& index_to_joint_name,
                               std::vector<double>& q_min, std::vector<double>& q_max) {
    ros::NodeHandle n("~");
    std::string ns = n.getNamespace();

    std::cout << chain_description << std::endl;

    if (chain_description.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
        ROS_ERROR("Chain description should be a struct containing 'root' and 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    XmlRpc::XmlRpcValue& v_root_link = chain_description["root"];
    if (v_root_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'root'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string root_link_name = (std::string)v_root_link;

    XmlRpc::XmlRpcValue& v_tip_link = chain_description["tip"];
    if (v_tip_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
        ROS_ERROR("Chain description for does not contain 'tip'. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    std::string tip_link_name = (std::string)v_tip_link;

    Chain* chain = new Chain();

    ROS_INFO("Looking for chain from %s to %s", root_link_name.c_str(), tip_link_name.c_str());

    if (!tree.kdl_tree_.getChain(root_link_name, tip_link_name, chain->kdl_chain_)) {
//    if (!tree.kdl_tree_.getChain("base", tip_link_name, chain->kdl_chain_)) {
        ROS_FATAL("Could not initialize chain object");
        return false;
    }

    for(unsigned int i = 0; i < chain->kdl_chain_.getNrOfSegments(); ++i) {
        const KDL::Segment& segment = chain->kdl_chain_.getSegment(i);
        const KDL::Joint& joint = segment.getJoint();

        if (joint.getType() != KDL::Joint::None)
        {
            //cout << "Segment: " << segment.getName() << endl;
            //cout << "Joint:   " << joint.getName() << endl;

            unsigned int full_joint_index = 0;

            std::map<std::string, unsigned int>::iterator it_joint = joint_name_to_index.find(joint.getName());

            if (it_joint == joint_name_to_index.end()) {
                // joint name is not yet in map, so give it a new fresh index and add it to the map
                full_joint_index = joint_name_to_index.size();

                //cout << "    new joint, gets index " << full_joint_index << endl;
                //cout << "    type: " << joint.getTypeName() << endl;

                joint_name_to_index[joint.getName()] = full_joint_index;
                index_to_joint_name.push_back(joint.getName());

                //cout << "    Lower limit: " << robot_model.getJoint(joint.getName())->limits->lower << endl;
                //cout << "    Upper limit: " << robot_model.getJoint(joint.getName())->limits->upper << endl;

                // determine joint limits from URDF
                q_min.push_back(robot_model.getJoint(joint.getName())->limits->lower);
                q_max.push_back(robot_model.getJoint(joint.getName())->limits->upper);

            } else {
                // joint name already in the map, so look-up its index
                full_joint_index = it_joint->second;

                //cout << "    existing joint, has index: " << full_joint_index << endl;
            }

        }

    }

    return true;
}
Esempio n. 19
0
/*!
 * \brief Grabs ft rosparams from a given node hande namespace
 *
 * The force/torque parameters consist of 
 *  6x ADC offset values
 *  6x6 gain matrix as 6-elment array of 6-element arrays of doubles
 *
 * \return True, if there are no problems.
 */
bool FTParamsInternal::getRosParams(ros::NodeHandle nh)
{
  if (!nh.hasParam("ft_params"))
  {
    ROS_WARN("'ft_params' not available for force/torque sensor in namespace '%s'",
             nh.getNamespace().c_str());
    return false;
  }

  XmlRpc::XmlRpcValue params;
  nh.getParam("ft_params", params);
  if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
  {
    ROS_ERROR("expected ft_params to be struct type");
    return false;
  }

  if (!getDoubleArray(params, "offsets", offsets_, 6))
  {
    return false;
  }

  if (!getDoubleArray(params, "gains", gains_, 6))
  {
    return false;
  }

  XmlRpc::XmlRpcValue coeff_matrix = params["calibration_coeff"];
  if (coeff_matrix.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Expected FT param 'calibration_coeff' to be list type");
    return false;
  }
  if (coeff_matrix.size() != 6)
  {
    ROS_ERROR("Expected FT param 'calibration_coeff' to have 6 elements");
    return false;
  }

  for (int i=0; i<6; ++i)
  {
    XmlRpc::XmlRpcValue coeff_row = coeff_matrix[i];
    if (coeff_row.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Expected FT param calibration_coeff[%d] to be list type", i);
      return false;
    }
    if (coeff_row.size() != 6)
    {
      ROS_ERROR("Expected FT param calibration_coeff[%d] to have 6 elements", i);
      return false;
    }
    
    for (int j=0; j<6; ++j)
    {
      if (coeff_row[j].getType() != XmlRpc::XmlRpcValue::TypeDouble)
      {
        ROS_ERROR("Expected FT param calibration_coeff[%d,%d] to be floating point type", i,j);
        return false;
      } else {
        calibration_coeff(i,j) = static_cast<double>(coeff_row[j]);
      }
    }
  }

  return true;
}
RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id)
{
  // ROS_INFO("ON_ACTIVATED");
  std::string config_name;
  config_name = nh.resolveName("controller_configuration");
  if (nh.hasParam(config_name))
  {
    XmlRpc::XmlRpcValue param_val;
    nh.getParam(config_name, param_val);
    if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      for (int i = 0; i < param_val.size(); i++)
      {
        if (param_val[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
        {
          XmlRpc::XmlRpcValue gval = param_val[i]["group_name"];
          XmlRpc::XmlRpcValue cval = param_val[i]["controller_name"];
          XmlRpc::XmlRpcValue lval = param_val[i]["joint_list"];

          std::string gname = gval;
          std::string cname = cval;
          std::vector<std::string> jlst;
          if (lval.getType() == XmlRpc::XmlRpcValue::TypeArray)
          {
            for (int s = 0; s < lval.size(); s++)
            {
              jlst.push_back(lval[s]);
            }
          }
          if (gname.length() == 0 && cname.length() > 0)
          {
            gname = cname;
          }
          if (gname.length() > 0 && cname.length() == 0)
          {
            cname = gname;
          }
          if (gname.length() > 0 && cname.length() > 0 && jlst.size() > 0)
          {
            std::stringstream ss;
            for (size_t s = 0; s < jlst.size(); s++)
            {
              ss << " " << jlst[s];
            }
            ROS_INFO_STREAM("ADD_GROUP: " << gname << " (" << cname << ")");
            ROS_INFO_STREAM("    JOINT:" << ss.str());
            jointTrajectoryActionObj::Ptr tmp(new jointTrajectoryActionObj(this, cval, gval, jlst));
            trajectory_actions.push_back(tmp);
          }
        }
      }
    }
    else
    {
      ROS_WARN_STREAM("param: " << config_name << ", configuration is not an array.");
    }
  }
  else
  {
    ROS_WARN_STREAM("param: " << config_name << ", param does not exist.");
  }

  return RTC::RTC_OK;
}
Esempio n. 21
0
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;
    }
  }
}
Esempio n. 22
0
int main(int argc, char** argv){	//pass the .graph file to open
  /*
  argc=3
  argv[0]=/.../patrolling_sim/bin/GBS
  argv[1]=__name:=XXXXXX
  argv[2]=maps/1r-5-map.graph
  argv[3]=ID_ROBOT
  argv[4]=NUMBER_OF_ROBOTS	//this is only necessary to automatically define G2
  */
  
  
  //More than One robot (ID between 0 and 99)
  if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
    ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots 
    return 0;
  }else{
    ID_ROBOT = atoi(argv[3]); 
    //printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
  }

  uint i = strlen( argv[2] );
  char graph_file[i];
  strcpy (graph_file,argv[2]);
  
  //Check Graph Dimension:
  uint dimension = GetGraphDimension(graph_file);
  
  //Create Structure to save the Graph Info;
  vertex vertex_web[dimension];
  
  //Get the Graph info from the Graph File
  GetGraphInfo(vertex_web, dimension, graph_file);
  
  
  
  /** Define G1 and G2 **/
  double G1 = 0.1;
  
  int NUMBER_OF_ROBOTS = atoi(argv[4]);
 
  //default:
  double G2 = 100.0;
  double edge_min = 1.0;
  
  if ( strcmp (graph_file,"maps/grid/grid.graph") == 0 ){  
    if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;}
    if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;}
    if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;}
    if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;}
    if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;}
    if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;}
    
  }else if (strcmp (graph_file,"maps/example/example.graph") == 0 ) {
    if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;}
    if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;}
    if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;}
    if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;}
    if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;}
    edge_min = 20.0;
    
  }else if (strcmp (graph_file,"maps/cumberland/cumberland.graph") == 0) {
    if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;}
    if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;}
    if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;}
    if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;}
    if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;}
    edge_min = 50.0;
    
  }
  
  printf("G1 = %f, G2 = %f\n", G1, G2);
  
  
   
  /* Define Starting Vertex/Position (Launch File Parameters) */

  ros::init(argc, argv, "c_reactive");
  ros::NodeHandle nh;
  double initial_x, initial_y;
  
  XmlRpc::XmlRpcValue list;
  nh.getParam("initial_pos", list);
  ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  
  int value = ID_ROBOT;
  if (value == -1){value = 0;}
  
  ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
  initial_x = static_cast<double>(list[2*value]);
  
  ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
  initial_y = static_cast<double>(list[2*value+1]);
 
//   printf("initial position: x = %f, y = %f\n", initial_x, initial_y);
  uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
//   printf("initial vertex = %d\n\n",current_vertex);  
  
  
  //INITIALIZE tab_intention:
  int tab_intention[NUMBER_OF_ROBOTS];
  for (i=0; i<NUMBER_OF_ROBOTS; i++){
    tab_intention[i] = -1;
  }
  
  
   //Publicar dados de "odom" para nó de posições
  odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
	
  //Subscrever posições de outros robots
  odom_sub = nh.subscribe("positions", 10, positionsCB);  
  
  char string[20];
  char string2[20];
  
  if(ID_ROBOT==-1){ 
    strcpy (string,"odom"); //string = "odom"
    strcpy (string2,"cmd_vel"); //string = "cmd_vel"
    TEAMSIZE = 1;
  }else{ 
    strcpy (string,"robot_");
    strcpy (string2,"robot_"); 
    char id[3];
    itoa(ID_ROBOT, id, 10);  
    strcat(string,id);
    strcat(string2,id);
    strcat(string,"/odom"); //string = "robot_X/odom" 
    strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
    TEAMSIZE = ID_ROBOT + 1;
  }	  
  
//   printf("string de publicação da odometria: %s\n",string);

   //Cmd_vel to backup:
   cmd_vel_pub  = nh.advertise<geometry_msgs::Twist>(string2, 1);
   
  //Subscrever para obter dados de "odom" do robot corrente
  ros::Subscriber sub;
  sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
  ros::spinOnce();    
    
  
  /* Define Goal */  
  
  if(ID_ROBOT==-1){ 
    strcpy (string,"move_base"); //string = "move_base
  }else{ 
    strcpy (string,"robot_"); 
    char id[3];
    itoa(ID_ROBOT, id, 10);  
    strcat(string,id);
    strcat(string,"/move_base"); //string = "robot_X/move_base"  
  }
  
  //printf("string = %s\n",string);
  MoveBaseClient ac(string, true); 
  
  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
     ROS_INFO("Waiting for the move_base action server to come up");
  }  

  //Define Goal:
  move_base_msgs::MoveBaseGoal goal;
  
  //Publicar dados para "results"
  results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
  results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
  
  initialize_node(); //dizer q está vivo
  ros::Rate loop_rate(1); //1 segundo
  
  /* Wait until all nodes are ready.. */
  while(initialize){
	ros::spinOnce();
	loop_rate.sleep();
  }
  
  /* Run Algorithm */
   
  double instantaneous_idleness [dimension];
  double last_visit [dimension];
  for(i=0;i<dimension;i++){ 
    instantaneous_idleness[i]= 0.0; 
    last_visit[i]= 0.0; 
    
    if(i==current_vertex){
      last_visit[i]= 0.1; //Avoids getting back at the initial vertex
    }
  }
  
  interference = false;
  ResendGoal = false;
  goal_complete = true;
  
  double now;
  
  
  while(1){
	  
    if(goal_complete){
	    
	    if(next_vertex>-1){
		//Update Idleness Table:
		now = ros::Time::now().toSec();
			
		for(i=0; i<dimension; i++){
			if (i == next_vertex){
				last_visit[i] = now;	
			}	
			instantaneous_idleness[i]= now - last_visit[i];           
		} 
		
		current_vertex = next_vertex;		
		
			
		//Show Idleness Table:
	/*	for (i=0; i<dimension; i++){
			printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);      
		} */
	    }
    
	//devolver proximo vertex tendo em conta apenas as idlenesses;
	next_vertex = (int) state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min);
	//printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
	
	/** SEND INTENTION **/
	send_intention(next_vertex);
	
	//Send the goal to the robot (Global Map)
	geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
	goal.target_pose.header.frame_id = "map"; 
	goal.target_pose.header.stamp = ros::Time::now();    
	goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
	goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
	goal.target_pose.pose.orientation = angle_quat;
	ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
	ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
	//ac.waitForResult();   
	
	goal_complete = false;
    
    }else{
	if (interference){
						
		// Stop the robot..			
 		ROS_INFO("Interference detected!\n");	
		send_interference();

		//get own "odom" positions...
		ros::spinOnce();		
					
		//Waiting until conflict is solved...
		while(interference){
			interference = check_interference();
			if (goal_complete || ResendGoal){
				interference = false;
			}
		}
			// se saiu é pq interference = false			
	}	    
	    
	if(ResendGoal){
		//Send the goal to the robot (Global Map)
		geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);	    
		goal.target_pose.header.frame_id = "map"; 
		goal.target_pose.header.stamp = ros::Time::now();    
		goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
		goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
		goal.target_pose.pose.orientation = angle_quat;	//alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
		ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// 		printf("ID_ROBOT = %d\n", ID_ROBOT);
		ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
		ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
	}
	
	if (arrived && NUMBER_OF_ROBOTS>1){	//a different robot arrived at a vertex: update idleness table and keep track of last vertices positions of other robots.

	  //printf("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);    

	  //Update Idleness Table:
	  now = ros::Time::now().toSec();
			  
	  for(i=0; i<dimension; i++){
		  if (i == vertex_arrived){
			    //actualizar last_visit[dimension]
			  last_visit[vertex_arrived] = now;	
		  }			
		  //actualizar instantaneous_idleness[dimension]
		  instantaneous_idleness[i]= now - last_visit[i];           
	  }	  
			  
	  //Show Idleness Table:	  	
// 	   for (i=0; i<dimension; i++){
// 		  printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);      
// 	  } 
	  
	  arrived = false;
	}
	
	if (intention && NUMBER_OF_ROBOTS>1){	  
	  tab_intention[robot_intention] = vertex_intention;
	  //printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
	  intention = false;	  
	}
	
	if(end_simulation){
	      return 0;
	}	
	
    }
  }
  
  return 0; 
}
int main(int argc, char** argv){	//pass the .graph file to open
  /*
  argc=3
  argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive
  argv[1]=__name:=XXXXXX
  argv[2]=maps/1r-5-map.graph
  argv[3]=ID_ROBOT
  */
  
  //More than One robot (ID between 0 and 99)
  if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
    ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1);
     return 0;
  }else{
    ID_ROBOT = atoi(argv[3]); 
  }
  

  uint i = strlen( argv[2] );
  char graph_file[i];
  strcpy (graph_file,argv[2]);
  
  //Check Graph Dimension:
  uint dimension = GetGraphDimension(graph_file);
  
  //Create Structure to save the Graph Info;
  vertex vertex_web[dimension];
  
  //Get the Graph info from the Graph File
  GetGraphInfo(vertex_web, dimension, graph_file);
  
  uint j;
  
  
  
  /* Output Graph Data */
  for (i=0;i<dimension;i++){
    printf ("ID= %u\n", vertex_web[i].id);
    printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
    printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
	
    for (j=0;j<vertex_web[i].num_neigh; j++){
      printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
    }
    
    printf("\n");	
  }
  
  
  
  /* Define Starting Vertex/Position (Launch File Parameters) */

  ros::init(argc, argv, "c_cognitive");
  ros::NodeHandle nh;	
  double initial_x, initial_y;
  
  XmlRpc::XmlRpcValue list;
  nh.getParam("initial_pos", list);
  ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  
  int value = ID_ROBOT;
  if (value == -1){value = 0;}
  
  ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
  initial_x = static_cast<double>(list[2*value]);
  
  ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
  initial_y = static_cast<double>(list[2*value+1]);
 
//   printf("x=%f, y=%f\n", initial_x, initial_y);
  uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
//   printf("v=%d\n",current_vertex);
    
  //Publicar dados de "odom" para nó de posições
  odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
	
  //Subscrever posições de outros robots
  odom_sub = nh.subscribe("positions", 10, positionsCB); 
  
  char string[20];
  char string2[20];
	
  if(ID_ROBOT==-1){ 
    strcpy (string,"odom"); //string = "odom"
    strcpy (string2,"cmd_vel"); //string = "cmd_vel"
    TEAMSIZE = 1;
  }else{ 
    strcpy (string,"robot_"); 
    strcpy (string2,"robot_"); 
    char id[3];
    itoa(ID_ROBOT, id, 10);  
    strcat(string,id);
    strcat(string2,id);
    strcat(string,"/odom"); //string = "robot_X/odom"  
    strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
    TEAMSIZE = ID_ROBOT + 1;
  }	  
  
//   printf("string de publicação da odometria: %s\n",string);

   //Cmd_vel to backup:
   cmd_vel_pub  = nh.advertise<geometry_msgs::Twist>(string2, 1);
  
  //Subscrever para obter dados da propria "odom" do robot corrente
  ros::Subscriber sub;
  sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
  ros::spinOnce(); 
  
  
  // Define Goal 

  if(ID_ROBOT==-1){ 
    strcpy (string,"move_base"); //string = "move_base"  
  }else{ 
    strcpy (string,"robot_"); 
    char id[3];
    itoa(ID_ROBOT, id, 10);  
    strcat(string,id);
    strcat(string,"/move_base"); //string = "robot_X/move_base"  
  }
  
  //printf("string do move base = %s\n",string);
  MoveBaseClient ac(string, true); 
  
  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
     ROS_INFO("Waiting for the move_base action server to come up");
  }  

  //Define Goal:
  move_base_msgs::MoveBaseGoal goal;  							
  
  //Publicar dados para "results"
  results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
  results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
  
  initialize_node(); //dizer q está vivo
  ros::Rate loop_rate(1); //1 segundo
  
  /* Wait until all nodes are ready.. */
  while(initialize){
	ros::spinOnce();
	loop_rate.sleep();
  }
  
  /* Run Algorithm */
  
  //instantaneous idleness and last visit initialized with zeros:
  double instantaneous_idleness [dimension];
  double last_visit [dimension];
  for(i=0;i<dimension;i++){ 
    instantaneous_idleness[i]= 0.0; 
    last_visit[i]= 0.0; 
    
    if(i==current_vertex){
      last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away
    }
  } 


  bool inpath = false;
  uint path [dimension];
  uint elem_s_path=0, i_path=0;
  
  interference = false;
  ResendGoal = false;
  goal_complete = true;

//   printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
  
  while(1){
	  
    if (goal_complete){
	    
    if (i_path>0){	//nao faz update no inicio
      //Update Idleness Table:
      double now = ros::Time::now().toSec();
      
      for(i=0; i<dimension; i++){
	if (i == next_vertex){
	  last_visit[i] = now;	
	}	
	instantaneous_idleness[i]= now - last_visit[i];           
      } 
	  
      current_vertex = next_vertex;
      
      //Show Idleness Table:
/*      for (i=0; i<dimension; i++){
	printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);      
       }
*/      

    }	    
    
    if(inpath){
	//The robot is on its way to a global objective -> get NEXT_VERTEX from its path:
	i_path++; //desde que nao passe o tamanho do path
	
	if (i_path<elem_s_path){
	  next_vertex=path[i_path];	  
	}else{	  
	 inpath = false;	
	}
    }
    
    if (!inpath){
	elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path);
      
/*      printf("Path: ");
      for (i=0;i<elem_s_path;i++){
	if (i==elem_s_path-1){
	  printf("%u.\n",path[i]);
	}else{
	  printf("%u, ",path[i]);
	}
      }
*/     
      //we have the path and the number of elements in the path
      i_path=1;
      next_vertex = path[i_path];
      inpath = true;
//       printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
    }
    
    if (inpath){
	    
	geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);	    
    
      //Send the goal to the robot (Global Map)
      goal.target_pose.header.frame_id = "map"; 
      goal.target_pose.header.stamp = ros::Time::now();    
      goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
      goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
      goal.target_pose.pose.orientation = angle_quat;	//alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
      ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
      ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
      //ac.sendGoal(goal);
      //ac.waitForResult();
    }    
	
    goal_complete = false; //garantir q n volta a entrar a seguir aqui
//     printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
    
    }else{ //goal not complete (active)
	    
		if (interference){
						
			// Stop the robot..			
 			ROS_INFO("Interference detected!\n");	
			send_interference();

			//get own "odom" positions...
			ros::spinOnce();		
					
			//Waiting until conflict is solved...
			while(interference){
				interference = check_interference();
				if (goal_complete || ResendGoal){
						interference = false;
				}
			}
			// se saiu é pq interference = false			
		}	    
	    
	    if(ResendGoal){
		geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);	    
    
		//Send the goal to the robot (Global Map)
		goal.target_pose.header.frame_id = "map"; 
		goal.target_pose.header.stamp = ros::Time::now();    
		goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
		goal.target_pose.pose.position.y = vertex_web[next_vertex].y;  
		goal.target_pose.pose.orientation = angle_quat;	//alpha -> orientação  (queria optimizar este parametro -> através da direcção do vizinho!)
		ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// 		printf("ID_ROBOT = %d\n", ID_ROBOT);
		ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
		ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
	    }
	    
	    if(end_simulation){
		    return 0;
	    }
	    
// 	    printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
	    
    }
    
  }
  
//   return 0; 
}
	void init( ros::NodeHandle &nh) {
		/*********************
		** Ros Comms
		**********************/
		server_add_listener = nh.advertiseService("add_listener", &ZeroconfNode::add_listener, this);
		server_remove_listener = nh.advertiseService("remove_listener", &ZeroconfNode::remove_listener, this);
		server_add_service = nh.advertiseService("add_service", &ZeroconfNode::add_service, this);
		server_remove_service = nh.advertiseService("remove_service", &ZeroconfNode::remove_service, this);
		server_list_discovered_services = nh.advertiseService("list_discovered_services", &ZeroconfNode::list_discovered_services, this);
		server_list_published_services = nh.advertiseService("list_published_services", &ZeroconfNode::list_published_services, this);
		pub_new_connections = nh.advertise<DiscoveredService>("new_connections",10);
		pub_lost_connections = nh.advertise<DiscoveredService>("lost_connections",10);

		/*********************
		** Signals
		**********************/
		// The callback functions use the publishers, so they must come after.
		connection_signal_cb new_connection_signal = std::bind1st(std::mem_fun(&ZeroconfNode::new_connections), this);
		connection_signal_cb old_connection_signal = std::bind1st(std::mem_fun(&ZeroconfNode::lost_connections), this);
		zeroconf.connect_signal_callbacks(new_connection_signal, old_connection_signal);

		/*********************
		** Parameters
		**********************/
		ros::NodeHandle local_nh("~");
		// listeners
	    XmlRpc::XmlRpcValue value;
	    XmlRpc::XmlRpcValue::iterator iter;
	    if ( local_nh.getParam("listeners", value) ) {
		    if ( value.getType() != XmlRpc::XmlRpcValue::TypeArray ) {
		        ROS_ERROR_STREAM("Zeroconf: param variable 'listeners' has malformed type, should be of type array");
		    } else {
			    for ( int i = 0; i < value.size(); ++i ) {
			    	zeroconf.add_listener(value[i]);
			    }
		    }
	    }
		// services
	    XmlRpc::XmlRpcValue value_services;
	    if ( local_nh.getParam("services", value_services) ) {
		    if ( value_services.getType() != XmlRpc::XmlRpcValue::TypeArray ) {
		        ROS_ERROR_STREAM("Zeroconf: param variable 'services' has malformed type, should be of type array");
		    } else {
			    for ( int i = 0; i < value_services.size(); ++i ) {
			    	XmlRpc::XmlRpcValue value_service = value_services[i];
			        if ( value_service.getType() != XmlRpc::XmlRpcValue::TypeStruct ) {
				        ROS_ERROR_STREAM("Zeroconf: " << i << "th element of param variable 'services' has malformed type, should be of type struct");
			            break;
			        }
					zeroconf_comms::PublishedService service;
			        XmlRpc::XmlRpcValue value_name = value_service["name"];
			        if ( value_name.getType() != XmlRpc::XmlRpcValue::TypeString ) {
				        ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['name'] has malformed type, should be of type string");
			            break;
			        }
		        	service.name = std::string(value_name);
			        if ( service.name == "Ros Master" ) {
			        	// add some special ros magic to easily identify master's.
			        	service.name = service.name + " (" + ros::master::getHost() + ")";
			        }
			        XmlRpc::XmlRpcValue value_type = value_service["type"];
			        if ( value_type.getType() != XmlRpc::XmlRpcValue::TypeString ) {
				        ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['type'] has malformed type, should be of type string");
			            break;
			        }
			        service.type = std::string(value_type);
			        XmlRpc::XmlRpcValue value_domain = value_service["domain"];
			        if ( value_domain.getType() != XmlRpc::XmlRpcValue::TypeString ) {
				        ROS_ERROR_STREAM("Zeroconf: services[" << i << "]['domain'] has malformed type, should be of type string");
			            break;
			        }
			        service.domain = std::string(value_domain);
			        XmlRpc::XmlRpcValue value_port = value_service["port"];
			        if ( value_port.getType() == XmlRpc::XmlRpcValue::TypeInvalid ) {
			        	service.port = ros::master::getPort();
			        	// wasn't set, so default to ros master's port
				        ROS_WARN_STREAM("Zeroconf: services[" << service.name << "]['port'] wasn't set, default to ros master's port [" << service.port << "]");
			        } else if ( value_port.getType() != XmlRpc::XmlRpcValue::TypeInt ) {
				        ROS_ERROR_STREAM("Zeroconf: services[" << service.name << "]['port'] has malformed type, should be of type int");
			            break;
			        } else {
			        	service.port = value_port;
			        }
			        XmlRpc::XmlRpcValue value_description = value_service["description"];
			        if ( value_description.getType() != XmlRpc::XmlRpcValue::TypeString ) {
			        	service.description = "";
			        } else {
			        	service.description = std::string(value_description);
			        }
			        zeroconf.add_service(service);
			    }
		    }
	    }
	}
Esempio n. 25
0
void loadStaticScene(ros::NodeHandle& node_handle,
		planning_scene::PlanningScenePtr& planning_scene,
		robot_model::RobotModelPtr& robot_model)
{
	std::string environment_file;
	std::vector<double> environment_position;

	node_handle.param<std::string>("/itomp_planner/environment_model",
			environment_file, "");

	if (!environment_file.empty())
	{
		environment_position.resize(3, 0);
		if (node_handle.hasParam("/itomp_planner/environment_model_position"))
		{
			XmlRpc::XmlRpcValue segment;
			node_handle.getParam("/itomp_planner/environment_model_position",
					segment);
			if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
			{
				int size = segment.size();
				for (int i = 0; i < size; ++i)
				{
					double value = segment[i];
					environment_position[i] = value;
				}
			}
		}

		// Collision object
		moveit_msgs::CollisionObject collision_object;
		collision_object.header.frame_id = robot_model->getModelFrame();
		collision_object.id = "environment";
		geometry_msgs::Pose pose;
		pose.position.x = environment_position[0];
		pose.position.y = environment_position[1];
		pose.position.z = environment_position[2];
		ROS_INFO(
				"Env col pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
		pose.orientation.x = 0.0;
		pose.orientation.y = 0.0;
		pose.orientation.z = 0.0;
		pose.orientation.w = 1.0;

		shapes::Mesh* shape = shapes::createMeshFromResource(environment_file);
		shapes::ShapeMsg mesh_msg;
		shapes::constructMsgFromShape(shape, mesh_msg);
		shape_msgs::Mesh mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

		collision_object.meshes.push_back(mesh);
		collision_object.mesh_poses.push_back(pose);

		shape_msgs::SolidPrimitive primitive;
		primitive.type = primitive.BOX;
		primitive.dimensions.resize(3);
		primitive.dimensions[0] = 2.0;
		primitive.dimensions[1] = 1.0;
		primitive.dimensions[2] = 1.0;
		pose.position.x = 0;
		pose.position.y = 6.0;
		pose.position.z = -0.45;
		collision_object.primitive_poses.push_back(pose);
		collision_object.primitives.push_back(primitive);

		collision_object.operation = collision_object.ADD;
		moveit_msgs::PlanningScene planning_scene_msg;
		planning_scene_msg.world.collision_objects.push_back(collision_object);
		planning_scene_msg.is_diff = true;
		planning_scene->setPlanningSceneDiffMsg(planning_scene_msg);
	}
}
Esempio n. 26
0
int main(int argc, char** argv)
{
	bool desired_mode_full=false; //if false go in safe_mode
	bool ir_warning_;
	bool bumper_warning_;

	ros::init(argc, argv, "roomba560_node");

	ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
	
	double last_x, last_y, last_yaw;
	double vel_x, vel_y, vel_yaw;
	double dt;
	float last_charge = 0.0;
	int time_remaining = -1;
	
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	
	std::string base_name;
	int id;
	pn.param<std::string>("base_name_", base_name, "iRobot_");
	pn.param<int>("id_", id, 0);
	std_msgs::String my_namespace;
	my_namespace.data = base_name + std::to_string(id);

	bool publish_name;
	ros::Publisher listpub;
	pn.param<bool>("publish_name_", publish_name, false);
	if(publish_name)
	{
	    listpub = n.advertise<std_msgs::String>("/agent_list", 50);
	}
	int name_count=0;

	pn.param<std::string>("port_", port, "/dev/ttyUSB0");

	std::string base_frame_id;
	std::string odom_frame_id;
	pn.param<std::string>("base_frame_id", base_frame_id, my_namespace.data + "/base_link");
	pn.param<std::string>("odom_frame_id", odom_frame_id, my_namespace.data + "/odom");
    
    bool publishTf;
    pn.param<bool>("publishTf", publishTf, true);

    Eigen::MatrixXd poseCovariance(6,6);
    Eigen::MatrixXd twistCovariance(6,6);
    bool pose_cov_mat = false;
    bool twist_cov_mat = false;
    std::vector<double> pose_covariance_matrix; 
    std::vector<double> twist_covariance_matrix;
    XmlRpc::XmlRpcValue poseCovarConfig;
    XmlRpc::XmlRpcValue twistCovarConfig;
    
    if (pn.hasParam("poseCovariance"))
    {
        try
        {
            pn.getParam("poseCovariance", poseCovarConfig);

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

            int matSize = poseCovariance.rows();

            if (poseCovarConfig.size() != matSize * matSize)
            {
                ROS_WARN_STREAM("Pose_covariance matrix should have " << matSize * matSize << " values.");
            }
            else
            {
                for (int i = 0; i < matSize; i++)
                {
                    for (int j = 0; j < matSize; j++)
                    {
                        std::ostringstream ostr;
                        ostr << poseCovarConfig[matSize * i + j];
                        std::istringstream istr(ostr.str());
                        istr >> poseCovariance(i, j);
                        pose_covariance_matrix.push_back(poseCovariance(i,j));
                    }
                }
                pose_cov_mat = true;
            }
        }
        catch (XmlRpc::XmlRpcException &e)
        {
            ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for pose_covariance (type: " << poseCovarConfig.getType() << ")");
        }
        
    }
    
    if (pn.hasParam("twistCovariance"))
    {
        try
        {
            pn.getParam("twistCovariance", twistCovarConfig);

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

            int matSize = twistCovariance.rows();

            if (twistCovarConfig.size() != matSize * matSize)
            {
                ROS_WARN_STREAM("Twist_covariance matrix should have " << matSize * matSize << " values.");
            }
            else
            {
                for (int i = 0; i < matSize; i++)
                {
                    for (int j = 0; j < matSize; j++)
                    {
                        std::ostringstream ostr;
                        ostr << twistCovarConfig[matSize * i + j];
                        std::istringstream istr(ostr.str());
                        istr >> twistCovariance(i, j);
                        twist_covariance_matrix.push_back(twistCovariance(i,j));
                    }
                }
                twist_cov_mat = true;
            }
        }
        catch (XmlRpc::XmlRpcException &e)
        {
            ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << " for twist_covariance (type: " << poseCovarConfig.getType() << ")");
        }
    }
    
	roomba = new irobot::OpenInterface(port.c_str());

	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("battery", 50);
    ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("bumper", 50);
    ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("buttons", 50);
    ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("cliff", 50);
    ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("ir_bumper", 50);
    ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("ir_character", 50);
    ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("wheel_drop", 50);

	tf::TransformBroadcaster tf_broadcaster;
	
	ros::Subscriber cmd_vel_sub  = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, cmdVelReceived);
    ros::Subscriber leds_sub  = n.subscribe<irobotcreate2::Leds>("leds", 1, ledsReceived);
    ros::Subscriber digitleds_sub  = n.subscribe<irobotcreate2::DigitLeds>("digit_leds", 1, digitLedsReceived);
    ros::Subscriber song_sub  = n.subscribe<irobotcreate2::Song>("song", 1, songReceived);
    ros::Subscriber playsong_sub  = n.subscribe<irobotcreate2::PlaySong>("play_song", 1, playSongReceived);
    ros::Subscriber mode_sub  = n.subscribe<std_msgs::String>("mode", 1, cmdModeReceived);
    ros::Subscriber special_sub  = n.subscribe<std_msgs::String>("special", 1, cmdSpecialReceived);
	
	irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100};
	roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE);

	if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
	else
	{
		ROS_FATAL("Could not connect to Roomba.");
		ROS_BREAK();
	}
	
	ros::Time current_time, last_time;
	current_time = ros::Time::now();
	last_time = ros::Time::now();

	bool first_loop=true;

    while(roomba->getSensorPackets(100) == -1 && ros::ok())
    {
        usleep(100);
        ROS_INFO_STREAM("Waiting for roomba sensors");
    }
    roomba->calculateOdometry();
    roomba->resetOdometry();
    
	ros::Rate r(50.0);
	while(n.ok())
	{
	        if(publish_name)
		{
		    name_count++;
		    if(name_count >= 10)
		    {
			listpub.publish(my_namespace);
			name_count=0;
		    }
		}

		ir_warning_ = false;
		bumper_warning_ = false;
		current_time = ros::Time::now();
		
		last_x = roomba->odometry_x_;
		last_y = roomba->odometry_y_;
		last_yaw = roomba->odometry_yaw_;
		
		if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets.");
		else roomba->calculateOdometry();
		
		/* OLD CODE
         * dt = (current_time - last_time).toSec();
		vel_x = (roomba->odometry_x_ - last_x)/dt;
		vel_y = (roomba->odometry_y_ - last_y)/dt;
		vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;*/
        
        vel_x = roomba->new_odometry_.getLinear();
        vel_y = 0.0;
        vel_yaw = roomba->new_odometry_.getAngular();
		
		// ******************************************************************************************
		//first, we'll publish the transforms over tf
        if(publishTf)
        {
            geometry_msgs::TransformStamped odom_trans;
            odom_trans.header.stamp = current_time;
            odom_trans.header.frame_id = odom_frame_id;
            odom_trans.child_frame_id = base_frame_id;
            odom_trans.transform.translation.x = roomba->odometry_x_;
            odom_trans.transform.translation.y = roomba->odometry_y_;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
            tf_broadcaster.sendTransform(odom_trans);
        }
		
		//TODO: Finish brodcasting the tf for all the ir sensors on the Roomba
		/*geometry_msgs::TransformStamped cliff_left_trans;
		cliff_left_trans.header.stamp = current_time;
		cliff_left_trans.header.frame_id = "base_link";
		cliff_left_trans.child_frame_id = "base_cliff_left";
		cliff_left_trans.transform.translation.x = 0.0;
		cliff_left_trans.transform.translation.y = 0.0;
		cliff_left_trans.transform.translation.z = 0.0;
		cliff_left_trans.transform.rotation = ;
		tf_broadcaster.sendTransform(cliff_left_trans);	*/

		// ******************************************************************************************
		//next, we'll publish the odometry message over ROS
		nav_msgs::Odometry odom;
		odom.header.stamp = current_time;
		odom.header.frame_id = odom_frame_id;
		
		//set the position
		odom.pose.pose.position.x = roomba->odometry_x_;
		odom.pose.pose.position.y = roomba->odometry_y_;
		odom.pose.pose.position.z = 0.0;
		odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
        if(pose_cov_mat)
            for(int i = 0; i < pose_covariance_matrix.size(); i++)
                odom.pose.covariance[i] = pose_covariance_matrix[i];
		
		//set the velocity
		odom.child_frame_id = base_frame_id;
		odom.twist.twist.linear.x = vel_x;
		odom.twist.twist.linear.y = vel_y;
		odom.twist.twist.angular.z = vel_yaw;
        if(twist_cov_mat)
            for(int i = 0; i < pose_covariance_matrix.size(); i++)
                odom.twist.covariance[i] = twist_covariance_matrix[i];
        
		//publish the message
		odom_pub.publish(odom);

		// ******************************************************************************************
		//publish battery
        irobotcreate2::Battery battery;
		battery.header.stamp = current_time;
		battery.power_cord = roomba->power_cord_;
		battery.dock = roomba->dock_;
		battery.level = 100.0*(roomba->charge_/roomba->capacity_);
		if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60;
		last_charge = roomba->charge_;
		battery.time_remaining = time_remaining;
		battery_pub.publish(battery);
	
		// ******************************************************************************************	
		//publish bumpers
        irobotcreate2::Bumper bumper;
		bumper.left.header.stamp = current_time;
		bumper.left.state = roomba->bumper_[LEFT];
		bumper.right.header.stamp = current_time;
		bumper.right.state = roomba->bumper_[RIGHT];
		bumper_pub.publish(bumper);
		bumper_warning_ = bumper.left.state || bumper.right.state;
		bumper_warning.store(bumper_warning_);
	
		// ******************************************************************************************	
		//publish buttons
        irobotcreate2::Buttons buttons;
		buttons.header.stamp = current_time;
		buttons.clean = roomba->buttons_[BUTTON_CLEAN];
		buttons.spot = roomba->buttons_[BUTTON_SPOT];
		buttons.dock = roomba->buttons_[BUTTON_DOCK];
		buttons.day = roomba->buttons_[BUTTON_DAY];
		buttons.hour = roomba->buttons_[BUTTON_HOUR];
		buttons.minute = roomba->buttons_[BUTTON_MINUTE];
		buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE];
		buttons.clock = roomba->buttons_[BUTTON_CLOCK];
		buttons_pub.publish(buttons);

		// ******************************************************************************************
		//publish cliff
        irobotcreate2::RoombaIR cliff;
		cliff.header.stamp = current_time;

		cliff.header.frame_id = "base_cliff_left";
		cliff.state = roomba->cliff_[LEFT];
		cliff.signal = roomba->cliff_signal_[LEFT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_front_left";
		cliff.state = roomba->cliff_[FRONT_LEFT];
		cliff.signal = roomba->cliff_signal_[FRONT_LEFT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_front_right";
		cliff.state = roomba->cliff_[FRONT_RIGHT];
		cliff.signal = roomba->cliff_signal_[FRONT_RIGHT];
		cliff_pub.publish(cliff);

		cliff.header.frame_id = "base_cliff_right";
		cliff.state = roomba->cliff_[RIGHT];
		cliff.signal = roomba->cliff_signal_[RIGHT];
		cliff_pub.publish(cliff);

		// ******************************************************************************************
		//publish irbumper
        irobotcreate2::RoombaIR irbumper;
		irbumper.header.stamp = current_time;

		irbumper.header.frame_id = "base_irbumper_left";
		irbumper.state = roomba->ir_bumper_[LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[LEFT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		irbumper.header.frame_id = "base_irbumper_front_left";
		irbumper.state = roomba->ir_bumper_[FRONT_LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		irbumper.header.frame_id = "base_irbumper_center_left";
		irbumper.state = roomba->ir_bumper_[CENTER_LEFT];
		irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		irbumper.header.frame_id = "base_irbumper_center_right";
		irbumper.state = roomba->ir_bumper_[CENTER_RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		irbumper.header.frame_id = "base_irbumper_front_right";
		irbumper.state = roomba->ir_bumper_[FRONT_RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		irbumper.header.frame_id = "base_irbumper_right";
		irbumper.state = roomba->ir_bumper_[RIGHT];
		irbumper.signal = roomba->ir_bumper_signal_[RIGHT];
		irbumper_pub.publish(irbumper);
		ir_warning_ = ir_warning_ || irbumper.state;

		ir_warning.store(ir_warning_);

		// ******************************************************************************************
		//publish irchar
        irobotcreate2::IRCharacter irchar;
		irchar.header.stamp = current_time;
		irchar.omni = roomba->ir_char_[OMNI];
		irchar.left = roomba->ir_char_[LEFT];
		irchar.right = roomba->ir_char_[RIGHT];
		irchar_pub.publish(irchar);

		// ******************************************************************************************
		//publish wheeldrop
        irobotcreate2::WheelDrop wheeldrop;
		wheeldrop.left.header.stamp = current_time;
		wheeldrop.left.state = roomba->wheel_drop_[LEFT];
		wheeldrop.right.header.stamp = current_time;
		wheeldrop.right.state = roomba->wheel_drop_[RIGHT];
		wheeldrop_pub.publish(wheeldrop);
		
		ros::spinOnce();
		r.sleep();
		
		if(first_loop)
		{
		    roomba->startOI(true);
		    if(!desired_mode_full) roomba->Safe();
		    first_loop=false;
		}
	}
	
	roomba->powerDown();
	roomba->closeSerialPort();
}
bool FollowJointTrajectoryController::init(ros::NodeHandle& nh, ControllerManager* manager)
{
    /* We absolutely need access to the controller manager */
    if (!manager)
    {
        initialized_ = false;
        return false;
    }

    Controller::init(nh, manager);

    /* No initial sampler */
    boost::mutex::scoped_lock lock(sampler_mutex_);
    sampler_.reset();
    preempted_ = false;

    /* Get Joint Names */
    joint_names_.clear();
    XmlRpc::XmlRpcValue names;
    if (!nh.getParam("joints", names))
    {
        ROS_ERROR_STREAM("No joints given for " << nh.getNamespace());
        return false;
    }
    if (names.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
        ROS_ERROR_STREAM("Joints not in a list for " << nh.getNamespace());
        return false;
    }
    for (int i = 0; i < names.size(); ++i)
    {
        XmlRpc::XmlRpcValue &name_value = names[i];
        if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
        {
            ROS_ERROR_STREAM("Not all joint names are strings for " << nh.getNamespace());
            return false;
        }

        joint_names_.push_back(static_cast<std::string>(name_value));
    }

    /* Get parameters */
    nh.param<bool>("stop_with_action", stop_with_action_, false);

    /* Get Joint Handles, setup feedback */
    joints_.clear();
    for (size_t i = 0; i < joint_names_.size(); ++i)
    {
        JointHandle* j = manager_->getJointHandle(joint_names_[i]);
        feedback_.joint_names.push_back(j->getName());
        joints_.push_back(j);
    }

    /* Update feedback */
    feedback_.desired.positions.resize(joints_.size());
    feedback_.desired.velocities.resize(joints_.size());
    feedback_.desired.accelerations.resize(joints_.size());
    feedback_.actual.positions.resize(joints_.size());
    feedback_.actual.velocities.resize(joints_.size());
    feedback_.actual.effort.resize(joints_.size());
    feedback_.error.positions.resize(joints_.size());
    feedback_.error.velocities.resize(joints_.size());

    /* Update tolerances */
    path_tolerance_.q.resize(joints_.size());
    path_tolerance_.qd.resize(joints_.size());
    path_tolerance_.qdd.resize(joints_.size());
    goal_tolerance_.q.resize(joints_.size());
    goal_tolerance_.qd.resize(joints_.size());
    goal_tolerance_.qdd.resize(joints_.size());

    /* Setup ROS interfaces */
    server_.reset(new server_t(nh, "",
                               boost::bind(&FollowJointTrajectoryController::executeCb, this, _1),
                               false));
    server_->start();

    initialized_ = true;
    return true;
}
Esempio n. 28
0
void renderStaticScene(ros::NodeHandle& node_handle,
		planning_scene::PlanningScenePtr& planning_scene,
		robot_model::RobotModelPtr& robot_model)
{
	std::string environment_file;
	std::vector<double> environment_position;

	static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
			visualization_msgs::MarkerArray>("visualization_marker_array", 10);

	ros::WallDuration(1.0).sleep();

	node_handle.param<std::string>("/itomp_planner/environment_model",
			environment_file, "");

	if (!environment_file.empty())
	{
		environment_position.resize(3, 0);
		if (node_handle.hasParam("/itomp_planner/environment_model_position"))
		{
			XmlRpc::XmlRpcValue segment;
			node_handle.getParam("/itomp_planner/environment_model_position",
					segment);
			if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
			{
				int size = segment.size();
				for (int i = 0; i < size; ++i)
				{
					double value = segment[i];
					environment_position[i] = value;
				}
			}
		}

		visualization_msgs::MarkerArray ma;
		visualization_msgs::Marker msg;
		msg.header.frame_id = robot_model->getModelFrame();
		msg.header.stamp = ros::Time::now();
		msg.ns = "environment";
		msg.type = visualization_msgs::Marker::MESH_RESOURCE;
		msg.action = visualization_msgs::Marker::ADD;
		msg.scale.x = 1.0;
		msg.scale.y = 1.0;
		msg.scale.z = 1.0;
		msg.id = 0;
		msg.pose.position.x = environment_position[0];
		msg.pose.position.y = environment_position[1];
		msg.pose.position.z = environment_position[2];
		ROS_INFO(
				"Env render pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
		msg.pose.orientation.x = 0.0;
		msg.pose.orientation.y = 0.0;
		msg.pose.orientation.z = 0.0;
		msg.pose.orientation.w = 1.0;
		msg.color.a = 1.0;
		msg.color.r = 0.5;
		msg.color.g = 0.5;
		msg.color.b = 0.5;
		msg.mesh_resource = environment_file;
		ma.markers.push_back(msg);

		msg.ns = "environment2";
		msg.type = visualization_msgs::Marker::CUBE;
		msg.id = 0;
		msg.pose.position.x = 0;
		msg.pose.position.y = 6.0;
		msg.pose.position.z = -0.45;
		msg.scale.x = 2.0;
		msg.scale.y = 1.0;
		msg.scale.z = 1.0;
		ma.markers.push_back(msg);

		ros::WallDuration(1.0).sleep();
		vis_marker_array_publisher_.publish(ma);
	}
}
  void NavSatTransform::run()
  {
    ros::Time::init();

    double frequency = 10.0;
    double delay = 0.0;

    ros::NodeHandle nh;
    ros::NodeHandle nhPriv("~");

    // Load the parameters we need
    nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
    nhPriv.param("yaw_offset", yawOffset_, 0.0);
    nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
    nhPriv.param("zero_altitude", zeroAltitude_, false);
    nhPriv.param("publish_filtered_gps", publishGps_, false);
    nhPriv.param("use_odometry_yaw", useOdometryYaw_, false);
    nhPriv.param("wait_for_datum", useManualDatum_, false);
    nhPriv.param("frequency", frequency, 10.0);
    nhPriv.param("delay", delay, 0.0);

    // Subscribe to the messages and services we need
    ros::ServiceServer datumServ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);

    if(useManualDatum_ && nhPriv.hasParam("datum"))
    {
      XmlRpc::XmlRpcValue datumConfig;

      try
      {
        double datumLat;
        double datumLon;
        double datumYaw;

        nhPriv.getParam("datum", datumConfig);

        ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
        ROS_ASSERT(datumConfig.size() == 4);

        useManualDatum_ = true;

        std::ostringstream ostr;
        ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
        std::istringstream istr(ostr.str());
        istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;

        robot_localization::SetDatum::Request request;
        request.geo_pose.position.latitude = datumLat;
        request.geo_pose.position.longitude = datumLon;
        request.geo_pose.position.altitude = 0.0;
        tf2::Quaternion quat;
        quat.setRPY(0.0, 0.0, datumYaw);
        request.geo_pose.orientation = tf2::toMsg(quat);
        robot_localization::SetDatum::Response response;
        datumCallback(request, response);
      }
      catch (XmlRpc::XmlRpcException &e)
      {
        ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
                         " for process_noise_covariance (type: " << datumConfig.getType() << ")");
      }
    }
Esempio n. 30
0
bool getImpl(const std::string& key, XmlRpc::XmlRpcValue& v, bool use_cache)
{
  std::string mapped_key = names::resolve(key);
  if (mapped_key.empty()) mapped_key = "/";

  if (use_cache)
  {
    boost::mutex::scoped_lock lock(g_params_mutex);

    if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
    {
      M_Param::iterator it = g_params.find(mapped_key);
      if (it != g_params.end())
      {
        if (it->second.valid())
        {
          ROS_DEBUG_NAMED("cached_parameters", "Using cached parameter value for key [%s]", mapped_key.c_str());

          v = it->second;
          return true;
        }
        else
        {
          ROS_DEBUG_NAMED("cached_parameters", "Cached parameter is invalid for key [%s]", mapped_key.c_str());
          return false;
        }
      }
    }
    else
    {
      // parameter we've never seen before, register for update from the master
      if (g_subscribed_params.insert(mapped_key).second)
      {
        XmlRpc::XmlRpcValue params, result, payload;
        params[0] = this_node::getName();
        params[1] = XMLRPCManager::instance()->getServerURI();
        params[2] = mapped_key;

        if (!master::execute("subscribeParam", params, result, payload, false))
        {
          ROS_DEBUG_NAMED("cached_parameters", "Subscribe to parameter [%s]: call to the master failed", mapped_key.c_str());
          g_subscribed_params.erase(mapped_key);
          use_cache = false;
        }
        else
        {
          ROS_DEBUG_NAMED("cached_parameters", "Subscribed to parameter [%s]", mapped_key.c_str());
        }
      }
    }
  }

  XmlRpc::XmlRpcValue params, result;
  params[0] = this_node::getName();
  params[1] = mapped_key;

  // We don't loop here, because validateXmlrpcResponse() returns false
  // both when we can't contact the master and when the master says, "I
  // don't have that param."
  bool ret = master::execute("getParam", params, result, v, false);

  if (use_cache)
  {
    boost::mutex::scoped_lock lock(g_params_mutex);

    ROS_DEBUG_NAMED("cached_parameters", "Caching parameter [%s] with value type [%d]", mapped_key.c_str(), v.getType());
    g_params[mapped_key] = v;
  }

  return ret;
}