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;
}
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;
}