#include <MultiJointTrackerNode.h>
Public Member Functions | |
virtual void | measurementCallback (const boost::shared_ptr< ks_measurement_ros_t const > &poses_and_vels) |
Callback for the measurements for this RE level (Poses and vels of the tracker RB from the lower level, RBT) | |
MultiJointTrackerNode () | |
virtual bool | publishURDF (joint_tracker::publish_urdf::Request &request, joint_tracker::publish_urdf::Response &response) |
void | ReadBag () |
void | ReadParameters () |
virtual void | statePredictionCallback (const boost::shared_ptr< ks_state_ros_t const > &predicted_next_state) |
Callback for the predictions about the state of this RE level coming from the higher level of the hierarchy. | |
virtual | ~MultiJointTrackerNode () |
Protected Member Functions | |
virtual void | _generateURDF (std_msgs::String &urdf_string_msg, sensor_msgs::JointState &joint_state_msg) const |
virtual void | _PrintResults () const |
Print the results of the joint tracker on the terminal. | |
virtual void | _publishPredictedMeasurement () const |
Publish the prediction about the next measurement by this RE level. | |
virtual void | _publishState () const |
Publish the current state of this RE level. | |
Protected Attributes | |
double | _loop_period_ns |
int | _min_joint_age_for_ee |
int | _processing_factor |
double | _sensor_fps |
std::string | _sr_path |
ros::Publisher | _state_publisher_joint_states |
ros::Publisher | _state_publisher_rviz_markers |
ros::Publisher | _state_publisher_urdf |
ros::ServiceServer | _urdf_pub_service |
Definition at line 70 of file MultiJointTrackerNode.h.
Constructor
Definition at line 36 of file MultiJointTrackerNode.cpp.
MultiJointTrackerNode::~MultiJointTrackerNode | ( | ) | [virtual] |
Destructor
Definition at line 65 of file MultiJointTrackerNode.cpp.
void MultiJointTrackerNode::_generateURDF | ( | std_msgs::String & | urdf_string_msg, |
sensor_msgs::JointState & | joint_state_msg | ||
) | const [protected, virtual] |
Definition at line 312 of file MultiJointTrackerNode.cpp.
void MultiJointTrackerNode::_PrintResults | ( | ) | const [protected, virtual] |
Print the results of the joint tracker on the terminal.
Definition at line 664 of file MultiJointTrackerNode.cpp.
void MultiJointTrackerNode::_publishPredictedMeasurement | ( | ) | const [protected, virtual] |
Publish the prediction about the next measurement by this RE level.
Implements omip::RecursiveEstimatorNodeInterface< ks_measurement_ros_t, ks_state_ros_t, MultiJointTracker >.
Definition at line 656 of file MultiJointTrackerNode.cpp.
void MultiJointTrackerNode::_publishState | ( | ) | const [protected, virtual] |
Publish the current state of this RE level.
Implements omip::RecursiveEstimatorNodeInterface< ks_measurement_ros_t, ks_state_ros_t, MultiJointTracker >.
Definition at line 521 of file MultiJointTrackerNode.cpp.
void MultiJointTrackerNode::measurementCallback | ( | const boost::shared_ptr< ks_measurement_ros_t const > & | poses_and_vels | ) | [virtual] |
Callback for the measurements for this RE level (Poses and vels of the tracker RB from the lower level, RBT)
poses_and_vels | Input tracked RB poses and vels |
Definition at line 192 of file MultiJointTrackerNode.cpp.
bool MultiJointTrackerNode::publishURDF | ( | joint_tracker::publish_urdf::Request & | request, |
joint_tracker::publish_urdf::Response & | response | ||
) | [virtual] |
Definition at line 279 of file MultiJointTrackerNode.cpp.
void omip::MultiJointTrackerNode::ReadBag | ( | ) | [inline] |
Version to work offline. Not implemented
Definition at line 107 of file MultiJointTrackerNode.h.
Read parameters to create the JointTracker
Definition at line 69 of file MultiJointTrackerNode.cpp.
virtual void omip::MultiJointTrackerNode::statePredictionCallback | ( | const boost::shared_ptr< ks_state_ros_t const > & | predicted_next_state | ) | [inline, virtual] |
Callback for the predictions about the state of this RE level coming from the higher level of the hierarchy.
predicted_next_state | Predictions about the next state. Right now there is no higher lever (unused) |
Definition at line 102 of file MultiJointTrackerNode.h.
double omip::MultiJointTrackerNode::_loop_period_ns [protected] |
Reimplemented from omip::RecursiveEstimatorNodeInterface< ks_measurement_ros_t, ks_state_ros_t, MultiJointTracker >.
Definition at line 137 of file MultiJointTrackerNode.h.
int omip::MultiJointTrackerNode::_min_joint_age_for_ee [protected] |
Definition at line 146 of file MultiJointTrackerNode.h.
int omip::MultiJointTrackerNode::_processing_factor [protected] |
Definition at line 136 of file MultiJointTrackerNode.h.
double omip::MultiJointTrackerNode::_sensor_fps [protected] |
Definition at line 135 of file MultiJointTrackerNode.h.
std::string omip::MultiJointTrackerNode::_sr_path [protected] |
Definition at line 148 of file MultiJointTrackerNode.h.
Definition at line 142 of file MultiJointTrackerNode.h.
Definition at line 144 of file MultiJointTrackerNode.h.
Definition at line 140 of file MultiJointTrackerNode.h.
Definition at line 133 of file MultiJointTrackerNode.h.