/* -----------------------------------------------------------------------------
* Constructor
*/
TrackerKalmanNode::TrackerKalmanNode()
{
defaultTtl = 5;
defaultTtlTime = 5000; // = 5s
// Window name (for visualization detections and predictions)
if(VISUAL_OUTPUT) {
winName = "Tracker (white = detections, red = predictions)";
}
rosInit(); // ROS-related initialization
}
RosSubscribe::RosSubscribe (const std::string& n)
: dynamicgraph::Entity(n),
nh_ (rosInit (true)),
bindedSignal_ ()
{
std::string docstring =
"\n"
" Add a signal reading data from a ROS topic\n"
"\n"
" Input:\n"
" - type: string among ['double', 'matrix', 'vector', 'vector3',\n"
" 'vector3Stamped', 'matrixHomo', 'matrixHomoStamped',\n"
" 'twist', 'twistStamped'],\n"
" - signal: the signal name in dynamic-graph,\n"
" - topic: the topic name in ROS.\n"
"\n";
addCommand ("add",
new command::rosSubscribe::Add
(*this, docstring));
docstring =
"\n"
" Remove a signal reading data from a ROS topic\n"
"\n"
" Input:\n"
" - name of the signal to remove (see method list for the list of signals).\n"
"\n";
addCommand ("rm",
new command::rosSubscribe::Rm
(*this, docstring));
docstring =
"\n"
" Remove all signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("clear",
new command::rosSubscribe::Clear
(*this, docstring));
docstring =
"\n"
" List signals reading data from a ROS topic\n"
"\n"
" No input:\n"
"\n";
addCommand ("list",
new command::rosSubscribe::List
(*this, docstring));
}
RosJointState::RosJointState (const std::string& n)
: Entity (n),
// do not use callbacks, so do not create a useless spinner
nh_ (rosInit (false)),
state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
publisher_ (nh_, "dynamic_graph/joint_states", 5),
jointState_ (),
trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
sotNOSIGNAL,
MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
lastPublicated_ ()
{
try {
lastPublicated_ = ros::Time::now ();
} catch (const std::exception& exc) {
throw std::runtime_error ("Failed to call ros::Time::now ():" +
std::string (exc.what ()));
}
signalRegistration (state_ << trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
// Fill header.
jointState_.header.seq = 0;
jointState_.header.stamp.sec = 0;
jointState_.header.stamp.nsec = 0;
jointState_.header.frame_id = "";
std::string docstring =
"\n"
" Retrieve joint names using robot model contained in a Dynamic entity\n"
"\n"
" Input:\n"
" - dynamic entity name (i.e. robot.dynamic.name)\n"
"\n";
addCommand ("retrieveJointNames",
new command::RetrieveJointNames (*this, docstring));
}
