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());
}
Пример #2
0
 ///\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;
 }
Пример #3
0
int main(int argc, char **argv)
{
  std::string name = "pomdp_run";
  ros::init(argc, argv, name);
  ros::NodeHandle nh;

  //
  // Parse command line arguments
  //
  std::vector<std::string> mdp_paths;
  std::vector<std::string> policy_paths;
  shared_ptr<Policy> the_policy;
  if(!parseArgs(argc, argv, mdp_paths, policy_paths))
  {
    ROS_ERROR("Unable to parse command line arguments.");
    exit(EXIT_FAILURE);
  }

  // Check for single POMDP and Policy.
  if(mdp_paths.size() < 1 || policy_paths.size() < 1)
  {
    ROS_ERROR("This requires a single (PO)MDP and a single Policy.");
    exit(EXIT_FAILURE);
  }
  else if(mdp_paths.size() > 1 || policy_paths.size() > 1)
  {
    ROS_ERROR("This node only supports a single (PO)MDP and Policy.");
    exit(EXIT_FAILURE);
  }

  //
  // Load complex StateSpace from param server, if path is available.
  //
  // Will use a SimpleStateSpace extracted from MDP file if none is found.
  //
  FeatureSpaceFactory fs_fact;
  shared_ptr<FeatureSpace> state_space;
  std::string state_space_path;
  bool hasAlternateStateSpace = false;
  if(nh.getParam("state_space_path", state_space_path))
  // Only does anything on success.
  {
    hasAlternateStateSpace = true;

    // Construct state space from given file.
    state_space = fs_fact.loadFromFile(state_space_path, "state_space");
    ROS_INFO_STREAM(*state_space);
  }

  //
  // Read POMDP files.
  //
  the_mdp = MDPFactory::loadFromFile(mdp_paths[0]);
  if(the_mdp == NULL)
  {
    ROS_ERROR("Unable to load (PO)MDP '%s'.", mdp_paths[0].c_str());
    exit(EXIT_FAILURE);
  }

  //
  // Replace (PO)MDP's StateSpace if a more complex one was loaded.
  //
  if(state_space && state_space->isInit())
  {
    if(the_mdp->setStateSpace(state_space))
    {
      ROS_INFO_STREAM("Replaced (PO)MDP \"" << the_mdp->getName() << "\"'s StateSpace with:");
      ROS_INFO_STREAM(*state_space); // Null check above
    }
    else
    {
      ROS_ERROR_STREAM("Unable to use " << *state_space);
      ROS_ERROR_STREAM("with MDP \"" << the_mdp->getName() << "\"");
      exit(EXIT_FAILURE);
    }
  }
  // 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;
}