Commit dc8c9520 authored by Víctor Silos Sánchez's avatar Víctor Silos Sánchez
Browse files

I have added the publisher and I'm doing the function

parent a3fda5ae
......@@ -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
......@@ -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
......@@ -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]
......
......@@ -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);
}
......@@ -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);
}
#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)
......
#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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment