diff --git a/iri_foot/include/foot_alg_node.h b/iri_foot/include/foot_alg_node.h
index a7524c8f1f41eb1456231236443192bf0c641f37..fa6240a6c22ee94ab993e50ef24ea7b91ba6e793 100644
--- a/iri_foot/include/foot_alg_node.h
+++ b/iri_foot/include/foot_alg_node.h
@@ -53,6 +53,10 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
tf::Vector3 rightknee;
+ tf::Vector3 sumalfoot;
+ tf::Vector3 sumarfoot;
+ tf::Vector3 sumalknee;
+ tf::Vector3 sumarknee;
// [publisher attributes]
ros::Publisher myfeet_publisher_;
diff --git a/iri_foot/src/foot_alg.cpp b/iri_foot/src/foot_alg.cpp
index e456c0afa7b50362dd3743a2722dd99bbc9eadff..a447d665e89bc105eaacacae9175ef160f1d6aa1 100644
--- a/iri_foot/src/foot_alg.cpp
+++ b/iri_foot/src/foot_alg.cpp
@@ -1,5 +1,6 @@
#include "foot_alg.h"
#include <math.h>
+using namespace std;
FootAlgorithm::FootAlgorithm(void)
{
@@ -25,10 +26,10 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
{
- double denleft = p2.getY() - p1.getY();
- double denright = p4.getY() - p3.getY();
+ double denleft = p2.getY() - p1.getY();
+ double denright = p4.getY() - p3.getY();
double numleft = sqrt ((p2.getZ() - p1.getZ())*(p2.getZ() - p1.getZ()) + (p2.getX() - p1.getX())*(p2.getX() - p1.getX()));
- double numright = sqrt ((p4.getZ() - p3.getZ())*(p4.getZ() - p3.getZ()) + (p4.getX() - p3.getX())*(p4.getX() - p3.getX()));
+ double numright = sqrt ((p4.getZ() - p3.getZ())*(p4.getZ() - p3.getZ())+ (p4.getX() - p3.getX())*(p4.getX() - p3.getX()));
angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright));
}
diff --git a/iri_foot/src/foot_alg.cpp~ b/iri_foot/src/foot_alg.cpp~
index e456c0afa7b50362dd3743a2722dd99bbc9eadff..233f1975d25d9998472017bb70b6c37ffb0b9e24 100644
--- a/iri_foot/src/foot_alg.cpp~
+++ b/iri_foot/src/foot_alg.cpp~
@@ -1,5 +1,6 @@
#include "foot_alg.h"
#include <math.h>
+using namespace std;
FootAlgorithm::FootAlgorithm(void)
{
@@ -25,10 +26,10 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
{
- double denleft = p2.getY() - p1.getY();
- double denright = p4.getY() - p3.getY();
- double numleft = sqrt ((p2.getZ() - p1.getZ())*(p2.getZ() - p1.getZ()) + (p2.getX() - p1.getX())*(p2.getX() - p1.getX()));
- double numright = sqrt ((p4.getZ() - p3.getZ())*(p4.getZ() - p3.getZ()) + (p4.getX() - p3.getX())*(p4.getX() - p3.getX()));
+ double denleft = p2.getY() - p1.getY();
+ double denright = p4.getY() - p3.getY();
+ double numleft = sqrt ((p2.getZ() - p1.getZ())*(p2.getZ() - p1.getZ()) /*+ (p2.getX() - p1.getX())*(p2.getX() - p1.getX())*/);
+ double numright = sqrt ((p4.getZ() - p3.getZ())*(p4.getZ() - p3.getZ())/*+ (p4.getX() - p3.getX())*(p4.getX() - p3.getX())*/);
angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright));
}
diff --git a/iri_foot/src/foot_alg_node.cpp b/iri_foot/src/foot_alg_node.cpp
index 5d553554982bdb1952386e17179b81382662c22c..ef15402917152feac25695cc7c4ad12d85806a54 100644
--- a/iri_foot/src/foot_alg_node.cpp
+++ b/iri_foot/src/foot_alg_node.cpp
@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void)
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");
-
- //left_foot
-
- listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
- leftfoot = transform1.getOrigin();
-
- //left_knee
-
- listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
- leftknee = transform2.getOrigin();
-
- //right_foot
-
- listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
- rightfoot = transform3.getOrigin();
+ 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;
+ }
- //right_knee
-
- listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
- rightknee = transform4.getOrigin();
+ leftfoot = sumalfoot/30;
+ leftknee = sumalknee/30;
+ rightfoot = sumarfoot/30;
+ rightknee = sumarknee/30;
+
//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.
@@ -71,47 +82,53 @@ void FootAlgNode::mainNodeThread(void)
double angleft = 0.0;
double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
- //stringstream converter; converter << angleft; my_var = converter.str();
-
- if (angleft >= 60.0 and angright >= 60.0)
- {
- my_var = "Left foot and right foot extended";
- }
-
- else if (angleft >= 60.0)
- {
- my_var = "Left foot extended";
- }
-
- else if (angright >= 60.0)
- {
- my_var = "Right foot extended";
- }
-
- else
- {
- my_var = "No foot extended";
- }
-
+ //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";
+ }
+ */
}
- catch (tf::TransformException &ex) {
- ROS_ERROR("%s",ex.what());
- ros::Duration(1.0).sleep();
- }
+ //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();
+ }
- // [fill msg structures]
- // Initialize the topic message structure
- this->myfeet_String_msg_.data = my_var;
+ // [fill msg structures]
+ // Initialize the topic message structure
+ this->myfeet_String_msg_.data = my_var;
- // [fill srv structure and make request to the server]
+ // [fill srv structure and make request to the server]
- // [fill action structure and make request to the action server]
+ // [fill action structure and make request to the action server]
- // [publish messages]
- // Uncomment the following line to publish the topic message
- this->myfeet_publisher_.publish(this->myfeet_String_msg_);
+ // [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->myfeet_publisher_.publish(this->myfeet_String_msg_);
}
diff --git a/iri_foot/src/foot_alg_node.cpp~ b/iri_foot/src/foot_alg_node.cpp~
index 5d553554982bdb1952386e17179b81382662c22c..4a6e30b25369282f4f82f44b130d9fd47e34e5db 100644
--- a/iri_foot/src/foot_alg_node.cpp~
+++ b/iri_foot/src/foot_alg_node.cpp~
@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void)
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");
-
- //left_foot
-
- listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
- leftfoot = transform1.getOrigin();
-
- //left_knee
-
- listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
- leftknee = transform2.getOrigin();
-
- //right_foot
-
- listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
- rightfoot = transform3.getOrigin();
+ sumalfoot = 0;
+ sumarfoot = 0;
+ sumalknee = 0;
+ sumarknee = 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;
+ }
- //right_knee
-
- listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
- listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
- rightknee = transform4.getOrigin();
+ leftfoot = sumalfoot/30;
+ leftknee = sumalknee/30;
+ rightfoot = sumarfoot/30;
+ rightknee = sumarknee/30;
+
//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.
@@ -71,47 +82,53 @@ void FootAlgNode::mainNodeThread(void)
double angleft = 0.0;
double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
- //stringstream converter; converter << angleft; my_var = converter.str();
-
- if (angleft >= 60.0 and angright >= 60.0)
- {
- my_var = "Left foot and right foot extended";
- }
-
- else if (angleft >= 60.0)
- {
- my_var = "Left foot extended";
- }
-
- else if (angright >= 60.0)
- {
- my_var = "Right foot extended";
- }
-
- else
- {
- my_var = "No foot extended";
- }
-
+ //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";
+ }
+ */
}
- catch (tf::TransformException &ex) {
- ROS_ERROR("%s",ex.what());
- ros::Duration(1.0).sleep();
- }
+ //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();
+ }
- // [fill msg structures]
- // Initialize the topic message structure
- this->myfeet_String_msg_.data = my_var;
+ // [fill msg structures]
+ // Initialize the topic message structure
+ this->myfeet_String_msg_.data = my_var;
- // [fill srv structure and make request to the server]
+ // [fill srv structure and make request to the server]
- // [fill action structure and make request to the action server]
+ // [fill action structure and make request to the action server]
- // [publish messages]
- // Uncomment the following line to publish the topic message
- this->myfeet_publisher_.publish(this->myfeet_String_msg_);
+ // [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->myfeet_publisher_.publish(this->myfeet_String_msg_);
}