foot_alg_node.h 4.91 KB
Newer Older
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _foot_alg_node_h_
#define _foot_alg_node_h_

#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_alg.h"
#include <tf/transform_listener.h>
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
31
#include <tf/transform_broadcaster.h>
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
32
//#include "tf/message_filter.h"
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
33
34

// [publisher subscriber headers]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
35
#include <geometry_msgs/PoseStamped.h>
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
36
#include <std_msgs/String.h>
37
#include <std_msgs/Float64.h>
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
38
39
40
41
42
43
44
45
46
47
48
49
50

// [service client headers]

// [action server client headers]

/**
 * \brief IRI ROS Specific Algorithm Class
 *
 */
class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
  private:
    tf::TransformListener listener;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
51
52
53
54
55
56
57
    tf::TransformBroadcaster broadcaster;
    tf::StampedTransform transform1;
    tf::StampedTransform transform2;
    tf::StampedTransform transform3;
    tf::StampedTransform transform4;
    tf::StampedTransform transform5;
    tf::StampedTransform transform6;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
58
59
    tf::StampedTransform transformrobleftdist;
    tf::StampedTransform transformrobrightdist;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
60
61
62
63
64
65
66
67
68
69
70
    tf::Vector3 rightfoot;
    tf::Vector3 leftfoot;
    tf::Vector3 leftknee;
    tf::Vector3 rightknee;
    tf::Vector3 righthand;
    tf::Vector3 sumalfoot;
    tf::Vector3 sumarfoot;
    tf::Vector3 sumalknee;
    tf::Vector3 sumarknee;
    tf::Vector3 distancesafe;
    tf::Vector3 robot;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
71
72
    tf::Vector3 goalleft;
    tf::Vector3 goalright;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
73
74
75

    //message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
    //tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
76
77
		
    // [publisher attributes]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
78
79
80
    ros::Publisher pose_surface_publisher_;
    geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
81
82
83
84
    ros::Publisher myfeet_publisher_;
    std_msgs::String myfeet_String_msg_;


Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
    // [subscriber attributes]

    // [service attributes]

    // [client attributes]

    // [action server attributes]

    // [action client attributes]

   /**
    * \brief config variable
    *
    * This variable has all the driver parameters defined in the cfg config file.
    * Is updated everytime function config_update() is called.
    */
    Config config_;
  public:
   /**
    * \brief Constructor
    * 
    * This constructor initializes specific class attributes and all ROS
    * communications variables to enable message exchange.
    */
    FootAlgNode(void);

   /**
    * \brief Destructor
    * 
    * This destructor frees all necessary dynamic memory allocated within this
    * this class.
    */
    ~FootAlgNode(void);

  protected:
   /**
    * \brief main node thread
    *
    * This is the main thread node function. Code written here will be executed
    * in every node loop while the algorithm is on running state. Loop frequency 
    * can be tuned by modifying loop_rate attribute.
    *
    * Here data related to the process loop or to ROS topics (mainly data structs
    * related to the MSG and SRV files) must be updated. ROS publisher objects 
    * must publish their data in this process. ROS client servers may also
    * request data to the corresponding server topics.
    */
    void mainNodeThread(void);

   /**
    * \brief dynamic reconfigure server callback
    * 
    * This method is called whenever a new configuration is received through
    * the dynamic reconfigure. The derivated generic algorithm class must 
    * implement it.
    *
    * \param config an object with new configuration from all algorithm 
    *               parameters defined in the config file.
    * \param level  integer referring the level in which the configuration
    *               has been changed.
    */
    void node_config_update(Config &config, uint32_t level);

   /**
    * \brief node add diagnostics
    *
    * In this abstract function additional ROS diagnostics applied to the 
    * specific algorithms may be added.
    */
    void addNodeDiagnostics(void);

    // [diagnostic functions]
    
    // [test functions]
};

#endif