From dc8c952059f0f352680f623cf29e20baf7332389 Mon Sep 17 00:00:00 2001
From: vsilos <vsilos@iri.upc.edu>
Date: Tue, 21 Mar 2017 13:47:38 +0100
Subject: [PATCH] I have added the publisher and I'm doing the function
---
iri_foot/include/foot_alg.h | 2 +-
iri_foot/include/foot_alg.h~ | 2 +-
iri_foot/include/foot_alg_node.h~ | 4 ++++
iri_foot/src/foot_alg.cpp | 8 +++-----
iri_foot/src/foot_alg.cpp~ | 8 +++-----
iri_foot/src/foot_alg_node.cpp | 8 +++++++-
iri_foot/src/foot_alg_node.cpp~ | 8 +++++++-
7 files changed, 26 insertions(+), 14 deletions(-)
diff --git a/iri_foot/include/foot_alg.h b/iri_foot/include/foot_alg.h
index 3b40850..7f89c38 100644
--- a/iri_foot/include/foot_alg.h
+++ b/iri_foot/include/foot_alg.h
@@ -127,7 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
- double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
+ double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
#endif
diff --git a/iri_foot/include/foot_alg.h~ b/iri_foot/include/foot_alg.h~
index bc34b98..3b40850 100644
--- a/iri_foot/include/foot_alg.h~
+++ b/iri_foot/include/foot_alg.h~
@@ -127,7 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
- double angle(tf::Vector p1, tf::Vector p2, tf::Vector p3, tf::Vector p4);
+ double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
};
#endif
diff --git a/iri_foot/include/foot_alg_node.h~ b/iri_foot/include/foot_alg_node.h~
index 6584c21..a7524c8 100644
--- a/iri_foot/include/foot_alg_node.h~
+++ b/iri_foot/include/foot_alg_node.h~
@@ -31,6 +31,7 @@
// [publisher subscriber headers]
#include <std_msgs/String.h>
+#include <std_msgs/Float64.h>
// [service client headers]
@@ -54,6 +55,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 rightknee;
// [publisher attributes]
+ ros::Publisher myfeet_publisher_;
+ std_msgs::String myfeet_String_msg_;
+
// [subscriber attributes]
diff --git a/iri_foot/src/foot_alg.cpp b/iri_foot/src/foot_alg.cpp
index a2a7bba..789e83b 100644
--- a/iri_foot/src/foot_alg.cpp
+++ b/iri_foot/src/foot_alg.cpp
@@ -23,15 +23,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
// FootAlgorithm Public API
-double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
+void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
{
double numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
- double angleft = atan (numleft/denleft);
- double angright = atan (numright/denright);
- return angleft;
- return angright;
+ angleft = atan (numleft/denleft);
+ angright = atan (numright/denright);
}
diff --git a/iri_foot/src/foot_alg.cpp~ b/iri_foot/src/foot_alg.cpp~
index a2a7bba..789e83b 100644
--- a/iri_foot/src/foot_alg.cpp~
+++ b/iri_foot/src/foot_alg.cpp~
@@ -23,15 +23,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
// FootAlgorithm Public API
-double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
+void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
{
double numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
- double angleft = atan (numleft/denleft);
- double angright = atan (numright/denright);
- return angleft;
- return angright;
+ angleft = atan (numleft/denleft);
+ angright = atan (numright/denright);
}
diff --git a/iri_foot/src/foot_alg_node.cpp b/iri_foot/src/foot_alg_node.cpp
index 6f916cd..6dd4312 100644
--- a/iri_foot/src/foot_alg_node.cpp
+++ b/iri_foot/src/foot_alg_node.cpp
@@ -1,4 +1,5 @@
#include "foot_alg_node.h"
+#include "foot_alg.cpp"
using namespace std;
#include <string>
@@ -64,7 +65,12 @@ try{
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
-
+
+ //Apply the function 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;
+ angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
+ if (angleft>30.0)
diff --git a/iri_foot/src/foot_alg_node.cpp~ b/iri_foot/src/foot_alg_node.cpp~
index 6f916cd..9ffde8d 100644
--- a/iri_foot/src/foot_alg_node.cpp~
+++ b/iri_foot/src/foot_alg_node.cpp~
@@ -1,4 +1,5 @@
#include "foot_alg_node.h"
+#include "foot_alg.cpp"
using namespace std;
#include <string>
@@ -64,7 +65,12 @@ try{
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
-
+
+ //Apply the function 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;
+ angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright);
+
--
GitLab