foot_alg_node.cpp 4.27 KB
Newer Older
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
1
#include "foot_alg_node.h"
2
//#include "foot_alg.cpp"
3
4
using namespace std;
#include <string>
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
5
6
7
8
9
10
11
12

FootAlgNode::FootAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
13
  this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void FootAlgNode::mainNodeThread(void)
{
33
34
35
36
  string my_var = "";
  try{
    ros::Time now = ros::Time(0);
    ros::Duration five_seconds = ros::Duration(3.0);
37
	
38
39
40
41
42
    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
43
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
    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);
    for(int i=0; i<30; ++i)
    {
	//left_foot
	listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
	leftfoot = transform1.getOrigin();
  	sumalfoot = sumalfoot + leftfoot;

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

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

	//right_knee
	listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
	listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
	rightknee = transform4.getOrigin();
	sumarknee = sumarknee + rightknee;
    }
72
	
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
73
74
75
76
77
    leftfoot = sumalfoot/30;
    leftknee = sumalknee/30; 
    rightfoot = sumarfoot/30;
    rightknee = sumarknee/30;
   
78
	
79
	
80
81
82
83
84
    //Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 30 degrees) the publisher says us that the foot is extended. 
	
    double angleft = 0.0;
    double angright = 0.0;
    alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
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
    //stringstream converter; converter << leftfoot(1); my_var = converter.str();
    stringstream converter; converter << angleft; my_var = converter.str();   
    
    /*if ((angleft >= 45.0) and (angright >= 45.0))
    {
      my_var = "Left foot and right foot extended";	
    }

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

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

    else
    {
      my_var = "No foot extended";
    }
    */
108
109
}

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
110
111
112
113
114
115
116
117
118
    //Apply the distance (20 cm) between the foot and the gripper (The safe distance).
    //leftfoot = transform1.getOrigin() ; 
    //rightfoot = transform3.getOrigin();

  catch (tf::TransformException &ex) 
  {
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
  }
119
	
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
120
121
122
  // [fill msg structures]
  // Initialize the topic message structure
  this->myfeet_String_msg_.data = my_var;
123

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
124
  
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
125
  // [fill srv structure and make request to the server]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
126
  
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
127
  // [fill action structure and make request to the action server]
Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
128

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
129
130
131
  // [publish messages]
  // Uncomment the following line to publish the topic message
  this->myfeet_publisher_.publish(this->myfeet_String_msg_);
132

Víctor Silos Sánchez's avatar
Víctor Silos Sánchez committed
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
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

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

void FootAlgNode::addNodeDiagnostics(void)
{
}

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