View difference between Paste ID: cdawwNsQ and ELHQr28n
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_ */