View difference between Paste ID: PcuNaWYm and cPTiyQ1d
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
  };