C++ Publisher::getTopic方法代码示例

void TargetAggregation::sendTUMComStringCommand(const char* cmd, ros::Publisher& tumcompub)
{
	std_msgs::String msg;
	msg.data = cmd;
	tumcompub.publish(msg);
	ROS_INFO("send TUMCom command \"%s\" on topic %s", cmd, tumcompub.getTopic().c_str());
}

 ///\brief Publishes diagnostics and status
 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
 {
   double now = ros::Time::now().toSec();
   double interval = now - lastDiagTime_;
   if (open_)
     stat.summary(0, "OK");
   else
     stat.summary(2, "Joystick not open.");
   
   stat.add("topic", pub_.getTopic());
   stat.add("device", joy_dev_);
   stat.add("dead zone", deadzone_);
   stat.add("autorepeat rate (Hz)", autorepeat_rate_);
   stat.add("coalesce interval (s)", coalesce_interval_);
   stat.add("recent joystick event rate (Hz)", event_count_ / interval);
   stat.add("recent publication rate (Hz)", pub_count_ / interval);
   stat.add("subscribers", pub_.getNumSubscribers());
   event_count_ = 0;
   pub_count_ = 0;
   lastDiagTime_ = now;
 }

//.........这里部分代码省略.........
  }
  // Fail if there was a state space but it couldn't be loaded.
  else if(hasAlternateStateSpace && !state_space->isInit())
  {
    ROS_ERROR_STREAM("Unable to load StateSpace from state_space_path \"" << state_space_path << "\"");
    exit(EXIT_FAILURE);
  }
  // Push a simple state space to the param server.
  else
  {
    shared_ptr<const SimpleFeatureSpace> simple_state_space
      = boost::dynamic_pointer_cast<const SimpleFeatureSpace>(the_mdp->getStateSpace());

    // Exists and is a SimpleFeatureSpace
    if(simple_state_space)
    {
      XmlRpc::XmlRpcValue state_space_xml;
      state_space_xml << *simple_state_space;
      nh.setParam("state_space", state_space_xml);
    }
  }

  // Push a simple action space to the param server.
  {
    shared_ptr<const SimpleFeatureSpace> simple_action_space
    = boost::dynamic_pointer_cast<const SimpleFeatureSpace>(the_mdp->getActionSpace());

    // Exists and is a SimpleFeatureSpace
    if(simple_action_space)
    {
      XmlRpc::XmlRpcValue action_space_xml;
      action_space_xml << *simple_action_space;
      nh.setParam("action_space", action_space_xml);
    }
  }

  // Debug print.
  ROS_DEBUG_STREAM("Initial state: " << the_mdp->prettyState());

  //
  // Read policy file.
  //
  the_policy = PolicyFactory::loadFromFile<Policy>(policy_paths[0],
                                                   the_mdp->getStateSpace(), the_mdp->getActionSpace());
  if(!the_policy)
  {
    ROS_ERROR("Unable to load Policy '%s'.", policy_paths[0].c_str());
    exit(EXIT_FAILURE);
  }
  the_policy->setName(name);

  //
  // Associate POMDP with Policy.
  //
  if(!the_mdp->setMyPolicy(the_policy))
  {
    ROS_ERROR("Unable to associate policy '%s' and POMDP '%s'!",
              the_policy->getName().c_str(), the_mdp->getName().c_str());
    exit(EXIT_FAILURE);
  }

  //
  // Determine initial action (set internally within POMDP).
  //
  int a = the_mdp->applyPolicy();
  if(a == -1)
  {
    ROS_ERROR("Error applying policy '%s' to initial state of '%s'.", the_policy->getName().c_str(), the_mdp->getName().c_str());
    exit(EXIT_FAILURE);
  }
  ROS_DEBUG_STREAM("Initial action: " << the_mdp->prettyAction(a));

  //
  // Advertise topics.
  //

  // Create names (default).
//  action_pub_name = ros::this_node::getName() + "/" + DEFAULT_ACTION_TOPIC;
//  obs_sub_name = ros::this_node::getName() +  "/" + DEFAULT_OBSERVATION_TOPIC;
    action_pub_name = DEFAULT_ACTION_TOPIC;
    obs_sub_name = DEFAULT_OBSERVATION_TOPIC;

  // Advertise.
  action_pub = nh.advertise<pomdp_node::action>(action_pub_name, DEFAULT_ACTION_QUEUE);
  if(!action_pub)
    ROS_ERROR_STREAM("Unable to initialize topic " << action_pub);
  else
    ROS_INFO_STREAM("Ready to publish actions on " << action_pub.getTopic());


  obs_sub = nh.subscribe(obs_sub_name, DEFAULT_OBSERVATION_QUEUE, observationCallback);
  if(!obs_sub)
    ROS_ERROR_STREAM("Unable to subscribe to " << obs_sub_name);
  else
    ROS_INFO_STREAM("Listening for observations on " << obs_sub.getTopic());

  ros::spin();

  return 0;
}