SHOW:
|
|
- or go back to the newest paste.
| 1 | /* | |
| 2 | * cmd_vel_mux.h | |
| 3 | * | |
| 4 | * Created on: Nov 30, 2012 | |
| 5 | * Author: jorge | |
| 6 | */ | |
| 7 | ||
| 8 | #ifndef CMD_VEL_MUX_H_ | |
| 9 | #define CMD_VEL_MUX_H_ | |
| 10 | ||
| 11 | ||
| 12 | #include <ros/ros.h> | |
| 13 | #include <geometry_msgs/PoseWithCovarianceStamped.h> | |
| 14 | #include "cmd_vel_subscribers.h" | |
| 15 | ||
| 16 | ||
| 17 | class CmdVelMux | |
| 18 | {
| |
| 19 | public: | |
| 20 | ||
| 21 | CmdVelMux(); | |
| 22 | ~CmdVelMux(); | |
| 23 | ||
| 24 | bool init(ros::NodeHandle& nh); | |
| 25 | void currentPosCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); /**< Added by Rupam on 26-04-2014*/ | |
| 26 | ||
| 27 | private: | |
| 28 | CmdVelSubscribers cmd_vel_sub; /**< Pool of cmd_vel topics subscribers */ | |
| 29 | ||
| 30 | ros::Publisher mux_cmd_vel_pub; /**< Multiplexed command velocity topic */ | |
| 31 | ros::Publisher allowed_sub_pub; /**< Currently allowed cmd_vel subscriber */ | |
| 32 | ||
| 33 | ros::Subscriber current_position; /**< Added by Rupam on 26-04-2014*/ | |
| 34 | ||
| 35 | void timerCallback(const ros::TimerEvent& event, unsigned int idx); | |
| 36 | void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx); | |
| 37 | ||
| 38 | ||
| 39 | ||
| 40 | // Functor assigned to each incoming velocity topic to bind it to cmd_vel callback | |
| 41 | class CmdVelFunctor | |
| 42 | {
| |
| 43 | private: | |
| 44 | unsigned int idx; | |
| 45 | CmdVelMux* node; | |
| 46 | ||
| 47 | public: | |
| 48 | CmdVelFunctor(unsigned int idx, CmdVelMux* node) : | |
| 49 | idx(idx), node(node) { }
| |
| 50 | ||
| 51 | void operator()(const geometry_msgs::Twist::ConstPtr& msg) | |
| 52 | {
| |
| 53 | node->cmdVelCallback(msg, idx); | |
| 54 | } | |
| 55 | }; | |
| 56 | ||
| 57 | // Functor assigned to each velocity messages source to bind it to timer callback | |
| 58 | class TimerFunctor | |
| 59 | {
| |
| 60 | private: | |
| 61 | unsigned int idx; | |
| 62 | CmdVelMux* node; | |
| 63 | ||
| 64 | public: | |
| 65 | TimerFunctor(unsigned int idx, CmdVelMux* node) : | |
| 66 | idx(idx), node(node) { }
| |
| 67 | ||
| 68 | void operator()(const ros::TimerEvent& event) | |
| 69 | {
| |
| 70 | node->timerCallback(event, idx); | |
| 71 | } | |
| 72 | }; |