The TwistMux class implements a top-level twist multiplexer module that priorize different velocity command topic inputs according to locks. More...
#include <twist_mux.h>
Public Types | |
typedef std::list < LockTopicHandle > | lock_topic_container |
typedef std::list < VelocityTopicHandle > | velocity_topic_container |
Public Member Functions | |
bool | hasPriority (const VelocityTopicHandle &twist) |
void | publishTwist (const geometry_msgs::TwistConstPtr &msg) |
TwistMux (int window_size=10) | |
void | updateDiagnostics (const ros::TimerEvent &event) |
~TwistMux () | |
Protected Types | |
typedef TwistMuxDiagnostics | diagnostics_type |
typedef TwistMuxDiagnosticsStatus | status_type |
Protected Member Functions | |
int | getLockPriority () |
template<typename T > | |
void | getTopicHandles (ros::NodeHandle &nh, ros::NodeHandle &nh_priv, const std::string ¶m_name, std::list< T > &topic_hs) |
Protected Attributes | |
ros::Publisher | cmd_pub_ |
boost::shared_ptr < diagnostics_type > | diagnostics_ |
ros::Timer | diagnostics_timer_ |
geometry_msgs::Twist | last_cmd_ |
boost::shared_ptr < lock_topic_container > | lock_hs_ |
boost::shared_ptr< status_type > | status_ |
boost::shared_ptr < velocity_topic_container > | velocity_hs_ |
velocity_hs_ Velocity topics' handles. Note that if we use a vector, as a consequence of the re-allocation and the fact that we have a subscriber inside with a pointer to 'this', we must reserve the number of handles initially. | |
Static Protected Attributes | |
static constexpr double | DIAGNOSTICS_PERIOD = 1.0 |
The TwistMux class implements a top-level twist multiplexer module that priorize different velocity command topic inputs according to locks.
Definition at line 44 of file twist_mux.h.
typedef TwistMuxDiagnostics twist_mux::TwistMux::diagnostics_type [protected] |
Definition at line 66 of file twist_mux.h.
typedef std::list<LockTopicHandle> twist_mux::TwistMux::lock_topic_container |
Definition at line 53 of file twist_mux.h.
typedef TwistMuxDiagnosticsStatus twist_mux::TwistMux::status_type [protected] |
Definition at line 67 of file twist_mux.h.
typedef std::list<VelocityTopicHandle> twist_mux::TwistMux::velocity_topic_container |
Definition at line 52 of file twist_mux.h.
twist_mux::TwistMux::TwistMux | ( | int | window_size = 10 | ) |
Get topics and locks:
Publisher for output topic:
Diagnostics:
Definition at line 51 of file twist_mux.cpp.
Definition at line 74 of file twist_mux.cpp.
int twist_mux::TwistMux::getLockPriority | ( | ) | [protected] |
max_element on the priority of lock topic handles satisfying that is locked:
Definition at line 118 of file twist_mux.cpp.
void twist_mux::TwistMux::getTopicHandles | ( | ros::NodeHandle & | nh, |
ros::NodeHandle & | nh_priv, | ||
const std::string & | param_name, | ||
std::list< T > & | topic_hs | ||
) | [protected] |
Definition at line 89 of file twist_mux.cpp.
bool twist_mux::TwistMux::hasPriority | ( | const VelocityTopicHandle & | twist | ) |
max_element on the priority of velocity topic handles satisfying that is NOT masked by the lock priority:
Definition at line 141 of file twist_mux.cpp.
void twist_mux::TwistMux::publishTwist | ( | const geometry_msgs::TwistConstPtr & | msg | ) |
Definition at line 83 of file twist_mux.cpp.
void twist_mux::TwistMux::updateDiagnostics | ( | const ros::TimerEvent & | event | ) |
Definition at line 77 of file twist_mux.cpp.
ros::Publisher twist_mux::TwistMux::cmd_pub_ [protected] |
Definition at line 85 of file twist_mux.h.
boost::shared_ptr<diagnostics_type> twist_mux::TwistMux::diagnostics_ [protected] |
Definition at line 94 of file twist_mux.h.
constexpr double twist_mux::TwistMux::DIAGNOSTICS_PERIOD = 1.0 [static, protected] |
Definition at line 71 of file twist_mux.h.
ros::Timer twist_mux::TwistMux::diagnostics_timer_ [protected] |
Definition at line 69 of file twist_mux.h.
geometry_msgs::Twist twist_mux::TwistMux::last_cmd_ [protected] |
Definition at line 87 of file twist_mux.h.
boost::shared_ptr<lock_topic_container> twist_mux::TwistMux::lock_hs_ [protected] |
Definition at line 83 of file twist_mux.h.
boost::shared_ptr<status_type> twist_mux::TwistMux::status_ [protected] |
Definition at line 95 of file twist_mux.h.
boost::shared_ptr<velocity_topic_container> twist_mux::TwistMux::velocity_hs_ [protected] |
velocity_hs_ Velocity topics' handles. Note that if we use a vector, as a consequence of the re-allocation and the fact that we have a subscriber inside with a pointer to 'this', we must reserve the number of handles initially.
Definition at line 82 of file twist_mux.h.