C++ NodeHandle::getNamespace方法代码示例

	void IkJointControllerClass::getParams(ros::NodeHandle& n) {
		ROS_INFO("getting gains params and join name params");

		XmlRpc::XmlRpcValue joint_names;  //array of 7 joint names

		if (!n.getParam("joints", joint_names)){
		    ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
		}

		if ((joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) || (joint_names.size() != 7)){
		    ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
		}

		std::string joint_param_ns;
		for (int i = 0; i < 7; i++){
			XmlRpc::XmlRpcValue &name_value = joint_names[i];
		    if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString){
		    	ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", n.getNamespace().c_str());
		    }
		    joint_names_[i] = ((std::string)name_value).c_str();

		    joint_param_ns = std::string("gains/") + joint_names_[i];
			n.getParam(joint_param_ns + "/p", p_[i]);
			n.getParam(joint_param_ns + "/d", d_[i]);

			ROS_INFO("NEW YAML got %d-th joint name %s with (p,d) gains parameters: (%f,%f)",
					i,
					joint_names_[i].c_str(),
					p_[i],
					d_[i]);
		}

    }

void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
  std::string urdf_string;
  urdf_model_ = new urdf::Model();

  // search and wait for robot_description on param server
  while (urdf_string.empty() && ros::ok())
  {
    std::string search_param_name;
    if (nh.searchParam(param_name, search_param_name))
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << search_param_name);
      nh.getParam(search_param_name, urdf_string);
    }
    else
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << param_name);
      nh.getParam(param_name, urdf_string);
    }

    usleep(100000);
  }

  if (!urdf_model_->initString(urdf_string))
    ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
  else
    ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}

bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
    // Get joint name from parameter server
    std::string joint_name;
    if (!n.getParam("joint", joint_name)) {
        ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    if (!n.getParam("effort_threshold", effort_threshold)) {
        ROS_ERROR("Effort threshold not given (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    // Get joint handle from hardware interface
    joint_ = robot->getHandle(joint_name);

    // Load PID Controller using gains set on parameter server
    if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
        return false;

    // Start realtime state publisher
    controller_state_publisher_.reset(
                new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
                (n, "state", 1));

    // Start command subscriber
    sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);

    return true;
}

/// Controller initialization in non-realtime                                                                                                                                     
bool MyCartControllerClass::init(pr2_mechanism_model::RobotState *robot,
                                 ros::NodeHandle &n)
{
  // Get the root and tip link names from parameter server.                                                                                                                       
  std::string root_name, tip_name;
  if (!n.getParam("root_name", root_name))
  {
    ROS_ERROR("No root name given in namespace: %s)",
              n.getNamespace().c_str());
    return false;
  }
  if (!n.getParam("tip_name", tip_name))
  {
    ROS_ERROR("No tip name given in namespace: %s)",
              n.getNamespace().c_str());
    return false;
  }

  // Construct a chain from the root to the tip and prepare the kinematics.                                                                                                       
  // Note the joints must be calibrated.                                                                                                                                          
  if (!chain_.init(robot, root_name, tip_name))
  {
    ROS_ERROR("MyCartController could not use the chain from '%s' to '%s'",
              root_name.c_str(), tip_name.c_str());
    return false;
  }

  // Store the robot handle for later use (to get time).                                                                                                                          
  robot_state_ = robot;

  // Construct the kdl solvers in non-realtime.                                                                                                                                   
  chain_.toKDL(kdl_chain_);
  jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
  jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));

  // Resize (pre-allocate) the variables in non-realtime.                                                                                                                         
  q_.resize(kdl_chain_.getNrOfJoints());
  q0_.resize(kdl_chain_.getNrOfJoints());
  qdot_.resize(kdl_chain_.getNrOfJoints());
  tau_.resize(kdl_chain_.getNrOfJoints());
  J_.resize(kdl_chain_.getNrOfJoints());

  // Pick the gains.                                                                                                                                                              
  Kp_.vel(0) = 100.0;  Kd_.vel(0) = 1.0;        // Translation x                                                                                                                  
  Kp_.vel(1) = 100.0;  Kd_.vel(1) = 1.0;        // Translation y                                                                                                                  
  Kp_.vel(2) = 100.0;  Kd_.vel(2) = 1.0;        // Translation z                                                                                                                  
  Kp_.rot(0) = 100.0;  Kd_.rot(0) = 1.0;        // Rotation x                                                                                                                     
  Kp_.rot(1) = 100.0;  Kd_.rot(1) = 1.0;        // Rotation y                                                                                                                     
  Kp_.rot(2) = 100.0;  Kd_.rot(2) = 1.0;        // Rotation z                                                                                                                     



  return true;
}

void getParams(const ros::NodeHandle &nh)
{
    //avg accel update period (number of msgs until next localization udpate)
    if (nh.getParam("update_period", UPDATE_PERIOD))
    {
        ROS_INFO("Set %s/update_period to %d",nh.getNamespace().c_str(), UPDATE_PERIOD);
    }
    else
    {
        if(nh.hasParam("update_period"))
            ROS_WARN("%s/update_period must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
        else
            ROS_WARN("No value set for %s/update_period. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
        UPDATE_PERIOD = DEFAULT_LOC_NODE_UPDATE_PERIOD;
    }

    //frequency this node publishes a new topic
    if (nh.getParam("publish_freq", PUBLISH_FREQ))
    {
        ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
    }
    else
    {
        if(nh.hasParam("publish_freq"))
            ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
        else
            ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
        PUBLISH_FREQ = DEFAULT_LOC_NODE_PUBLISH_FREQ;
    }

    //number of states from coax_filter this node will buffer before it begins to drop them
    if (nh.getParam("fstate_msg_buffer", FSTATE_MSG_BUFFER))
    {
        ROS_INFO("Set %s/fstate_msg_buffer to %d",nh.getNamespace().c_str(), FSTATE_MSG_BUFFER);
    }
    else
    {
        if(nh.hasParam("fstate_msg_buffer"))
            ROS_WARN("%s/fstate_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
        else
            ROS_WARN("No value set for %s/fstate_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
        FSTATE_MSG_BUFFER = DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER;
    }

    //number of messages this node will queue for publishing before droping old data
    if (nh.getParam("msg_queue", MSG_QUEUE))
    {
        ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
    }
    else
    {
        if(nh.hasParam("msg_queue"))
            ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
        else
            ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
        MSG_QUEUE = DEFAULT_LOC_NODE_MSG_QUEUE;
    }
}

    void callbackThreadFunction(SharedMemoryTransport<T>* smt, boost::function<void(T&)> callback)
    {
      T msg;
      std::string serialized_data;

      ros::Rate loop_rate(10.0);
      while(ros::ok()) //wait for the field to at least have something
      {
        if(!smt->initialized())
        {
          ROS_WARN("%s: Shared memory transport was shut down while we were waiting for connections. Stopping callback thread!", m_nh->getNamespace().c_str());
          return;
        }
        if(!smt->connected())
        {
          smt->connect();
        }
        else if(smt->hasData())
        {
          break;
        }
        ROS_WARN_STREAM_THROTTLE(1.0, m_nh->getNamespace() << ": Trying to connect to field " << smt->getFieldName() << "...");
        loop_rate.sleep();
        //boost::this_thread::interruption_point();
      }

      if(getCurrentMessage(msg))
      {
        callback(msg);
      }

      while(ros::ok())
      {
        try
        {
          if(m_use_polling)
          {
            smt->awaitNewDataPolled(msg);
          }
          else
          {
            smt->awaitNewData(msg);
          }
          callback(msg);
        }
        catch(ros::serialization::StreamOverrunException& ex)
        {
          ROS_ERROR("%s: Deserialization failed for topic %s! The string was:\n%s", m_nh->getNamespace().c_str(), m_full_topic_path.c_str(), serialized_data.c_str());
        }
        //boost::this_thread::interruption_point();
      }
    }

void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
    if(_use_dynamic_reconfig)
    {
        // classical PID init
        _pid.init(_node);
    }
    else
    {
        control_toolbox::Pid::Gains gains;

        // Load PID gains from parameter server
        if (!_node.getParam("p", gains.p_gain_))
        {
          ROS_ERROR("No p gain specified for pid.  Namespace: %s", _node.getNamespace().c_str());
          return;
        }
        // Only the P gain is required, the I and D gains are optional and default to 0:
        _node.param("i", gains.i_gain_, 0.0);
        _node.param("d", gains.d_gain_, 0.0);

        // Load integral clamp from param server or default to 0
        double i_clamp;
        _node.param("i_clamp", i_clamp, 0.0);
        gains.i_max_ = std::abs(i_clamp);
        gains.i_min_ = -std::abs(i_clamp);
        _pid.setGains(gains);
    }
}

static void require_param(const ros::NodeHandle &nh, const std::string &param_name, T &var)
{
  if(!nh.getParam(param_name, var)) {
    ROS_FATAL_STREAM("Required parameter not found! Namespace: "<<nh.getNamespace()<<" Parameter: "<<param_name);
    throw ros::InvalidParameterException("Parameter not found!");
  }
}

 void publishObstacleMarker(const pcl::PointXYZ& obstacle_location)
 {
   if (marker_pub.getNumSubscribers())
   {
     visualization_msgs::Marker obstacle;
     obstacle.header.frame_id = nh.getNamespace() + "/kinect_depth";
     obstacle.header.stamp = ros::Time::now();
     obstacle.ns = "obstacle";
     obstacle.action = visualization_msgs::Marker::ADD;
     obstacle.pose.position.x = obstacle_location.x;
     obstacle.pose.position.y = obstacle_location.y;
     obstacle.pose.position.z = obstacle_location.z;
     obstacle.pose.orientation.w = 1.0;
     obstacle.id = 0;
     obstacle.type = visualization_msgs::Marker::SPHERE;
     obstacle.scale.x = 0.1;
     obstacle.scale.y = 0.1;
     obstacle.scale.z = 0.1;
     obstacle.color.r = 1.0;
     obstacle.color.g = 0.0;
     obstacle.color.b = 0.0;
     obstacle.color.a = 0.8;
     obstacle.lifetime = ros::Duration(1.0);
     marker_pub.publish(obstacle);
   }
 }

 void callback(const giskard_msgs::ControllerFeedback::ConstPtr& msg)
 {
   visualization_msgs::MarkerArray out_msg;
   // TODO: get this from the server or a callback
   out_msg.markers = to_markers(msg->current_command, 0.1, nh_.getNamespace());
   pub_.publish(out_msg);
 }

ModelStatesWatcher::ModelStatesWatcher( ros::NodeHandle &nh, std::string topic_name )
    : valid_mspose(false), model_name(nh.getNamespace()), near_ego_vehicle(true),
      mssub(nh.subscribe( topic_name, 1, &ModelStatesWatcher::ms_cb, this ))
{
    while (model_name[0] == '/')
        model_name = std::string(model_name.c_str()+1);
}

	void publishDiagnostics()
	{
	    // publishing diagnotic messages
	    diagnostic_msgs::DiagnosticArray diagnostics;
	    diagnostics.status.resize(1);
	    diagnostics.status[0].name = nh_.getNamespace();
	    diagnostics.status[0].values.resize(1);
	    diagnostics.status[0].values[0].key = "error_count";
	    diagnostics.status[0].values[0].value = boost::lexical_cast<std::string>( error_counter_);

	    // set data to diagnostics
	    if (isDSAInitialized_)
	    {
		diagnostics.status[0].level = 0;
		diagnostics.status[0].message = "DSA tactile sensing initialized and running";
	    }
	    else if(error_counter_ == 0)
	    {
		diagnostics.status[0].level = 1;
		diagnostics.status[0].message = "DSA not initialized";
	    }
	    else
	    {
		diagnostics.status[0].level = 2;
		diagnostics.status[0].message = "DSA exceeded eror count";
	    }
	    // publish diagnostic message
	    topicPub_Diagnostics_.publish(diagnostics);
	    if(debug_) ROS_DEBUG_STREAM("publishDiagnostics " << diagnostics);

	}	 

  FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
    as_(nh, nh.getNamespace(),
        boost::bind(&FootstepPlanner::planCB, this, _1), false)
  {
    srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
    typename dynamic_reconfigure::Server<Config>::CallbackType f =
      boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
    srv_->setCallback (f);
    pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
      "text", 1, true);
    pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "close_list", 1, true);
    pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "open_list", 1, true);
    srv_project_footprint_ = nh.advertiseService(
      "project_footprint", &FootstepPlanner::projectFootPrintService, this);
    srv_project_footprint_with_local_search_ = nh.advertiseService(
      "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
    {
      boost::mutex::scoped_lock lock(mutex_);
      if (!readSuccessors(nh)) {
        return;
      }

      JSK_ROS_INFO("building graph");
      buildGraph();
      JSK_ROS_INFO("build graph done");
    }
    sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
    as_.start();
  }

		void publishError(std::string error_str) {
			diagnostic_msgs::DiagnosticArray diagnostics;
			diagnostics.status.resize(1);
			diagnostics.status[0].level = 2;
			diagnostics.status[0].name = nh.getNamespace();
			diagnostics.status[0].message = error_str;
			topicPub_Diagnostic_.publish(diagnostics);     
		}

 Rigid_Body(ros::NodeHandle& nh, std::string server_ip,
            int port) 
 {
     target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
     std::string connec_nm = server_ip + ":" + boost::lexical_cast<std::string>(port);
     connection = vrpn_get_connection_by_name(connec_nm.c_str());
     std::string target_name = nh.getNamespace().substr(1);
     tracker = new vrpn_Tracker_Remote(target_name.c_str(), connection);
     this->tracker->register_change_handler(NULL, track_target);
 }