foot_extended_alg_node.cpp~ 4.2 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
#include "foot_extended_alg_node.h"
using namespace std;
#include <string>

FootExtendedAlgNode::FootExtendedAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<FootExtendedAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
  // [init publishers]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
11
  this->feetextended_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("feetextended", 10);
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

FootExtendedAlgNode::~FootExtendedAlgNode(void)
{
  // [free dynamic memory]
}

void FootExtendedAlgNode::mainNodeThread(void)
{
  string my_var = "";
  try{
    ros::Time zero = ros::Time(0);
    ros::Duration three_seconds = ros::Duration(3.0);
    sumalfoot.setX(0); sumalfoot.setY(0); sumalfoot.setZ(0);
    sumarfoot.setX(0); sumarfoot.setY(0); sumarfoot.setZ(0);
    sumalknee.setX(0); sumalknee.setY(0); sumalknee.setZ(0);
    sumarknee.setX(0); sumarknee.setY(0); sumarknee.setZ(0);
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
39
40
41
42
43
    string child_leftfoot("/brix_2/user_1/left_foot");
    string child_leftknee("/brix_2/user_1/left_knee");
    string child_rightfoot("/brix_2/user_1/right_foot");
    string child_rightknee("/brix_2/user_1/right_knee");
    string parent_cameraframe("/brix_2_camera_frame");
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
    
    for(int i=0; i<100; ++i)
    {
	//left_foot
	listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
	listener.lookupTransform(parent_cameraframe, child_leftfoot, zero, transform1);
	leftfoot = transform1.getOrigin();
  	sumalfoot = sumalfoot + leftfoot;

	//left_knee
	listener.waitForTransform(parent_cameraframe, child_leftknee, zero, three_seconds);
	listener.lookupTransform(parent_cameraframe, child_leftknee, zero, transform2);
	leftknee = transform2.getOrigin();
	sumalknee = sumalknee + leftknee;

	//right_foot
	listener.waitForTransform(parent_cameraframe, child_rightfoot, zero, three_seconds);
	listener.lookupTransform(parent_cameraframe, child_rightfoot, zero, transform3);
	rightfoot = transform3.getOrigin();
	sumarfoot = sumarfoot + rightfoot;

	//right_knee
	listener.waitForTransform(parent_cameraframe, child_rightknee, zero, three_seconds);
	listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
	rightknee = transform4.getOrigin();
	sumarknee = sumarknee + rightknee;
    }
	
    leftfoot = sumalfoot/100;
    leftknee = sumalknee/100; 
    rightfoot = sumarfoot/100;
    rightknee = sumarknee/100;

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
77
    //Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 35 degrees) the publisher says us that the foot is extended. 
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
78
79
80
81
82
83
84
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
	
    double angleft = 0.0;
    double angright = 0.0;
    alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
    //stringstream converter; converter << leftfoot(1); my_var = converter.str();
    //stringstream converter; converter << angleft; my_var = converter.str();   
    
    if ((angleft >= 35.0) and (angright >= 35.0))
    {
      my_var = "Left foot and right foot extended";	
    }

    else if (angleft >= 35.0)
    {
      my_var = "Left foot extended";
    }

    else if (angright >= 35.0)
    {
      my_var = "Right foot extended";
    }

    else
    {
      my_var = "No foot extended";
    }
}

  catch (tf::TransformException &ex) 
  {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }


  // [fill msg structures]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
114
115
116
  // Initialize the topic message structure
  this->feetextended_String_msg_.data = my_var;

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
117
118
119
120
121
122
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
123
124
125
126
  // Uncomment the following line to publish the topic message
  this->feetextended_publisher_.publish(this->feetextended_String_msg_);

}
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void FootExtendedAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  this->config_=config;
  this->alg_.unlock();
}

void FootExtendedAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<FootExtendedAlgNode>(argc, argv, "foot_extended_alg_node");
}