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 <geometry_msgs/PoseStamped.h> | |
15 | #include "cmd_vel_subscribers.h" | |
16 | ||
17 | ||
18 | class CmdVelMux | |
19 | { | |
20 | public: | |
21 | ||
22 | CmdVelMux(); | |
23 | ~CmdVelMux(); | |
24 | ||
25 | bool init(ros::NodeHandle& nh); | |
26 | geometry_msgs::PoseWithCovarianceStamped copyCurrent; | |
27 | geometry_msgs::PoseStamped copyGoal; | |
28 | ||
29 | private: | |
30 | CmdVelSubscribers cmd_vel_sub; /**< Pool of cmd_vel topics subscribers */ | |
31 | ||
32 | ros::Publisher mux_cmd_vel_pub; /**< Multiplexed command velocity topic */ | |
33 | ros::Publisher allowed_sub_pub; /**< Currently allowed cmd_vel subscriber */ | |
34 | ||
35 | ros::Subscriber current_position; /**< Added by Rupam on 26-04-2014*/ | |
36 | ros::Subscriber goal_position; /**< Added by Rupam on 28-04-2014*/ | |
37 | ||
38 | void timerCallback(const ros::TimerEvent& event, unsigned int idx); | |
39 | void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx); | |
40 | void currentPosCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); /**< Added by Rupam on 26-04-2014*/ | |
41 | void goalPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg); /**< Added by Rupam on 28-04-2014*/ | |
42 | ||
43 | ||
44 | ||
45 | // Functor assigned to each incoming velocity topic to bind it to cmd_vel callback | |
46 | class CmdVelFunctor | |
47 | { | |
48 | private: | |
49 | unsigned int idx; | |
50 | CmdVelMux* node; | |
51 | ||
52 | public: | |
53 | CmdVelFunctor(unsigned int idx, CmdVelMux* node) : | |
54 | idx(idx), node(node) { } | |
55 | ||
56 | void operator()(const geometry_msgs::Twist::ConstPtr& msg) | |
57 | { | |
58 | node->cmdVelCallback(msg, idx); | |
59 | } | |
60 | }; | |
61 | ||
62 | // Functor assigned to each velocity messages source to bind it to timer callback | |
63 | class TimerFunctor | |
64 | { | |
65 | private: | |
66 | unsigned int idx; | |
67 | CmdVelMux* node; | |
68 | ||
69 | public: | |
70 | TimerFunctor(unsigned int idx, CmdVelMux* node) : | |
71 | idx(idx), node(node) { } | |
72 | ||
73 | void operator()(const ros::TimerEvent& event) | |
74 | { | |
75 | node->timerCallback(event, idx); | |
76 | } | |
77 | }; | |
78 | }; | |
79 | ||
80 | #endif /* CMD_VEL_MUX_H_ */ |