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