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

Foot filter

parent c14f342c
......@@ -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_;
......
#include "foot_alg.h"
#include <math.h>
using namespace std;
FootAlgorithm::FootAlgorithm(void)
{
......@@ -28,7 +29,7 @@ void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Ve
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));
}
......
#include "foot_alg.h"
#include <math.h>
using namespace std;
FootAlgorithm::FootAlgorithm(void)
{
......@@ -27,8 +28,8 @@ void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Ve
{
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 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));
}
......
......@@ -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");
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);
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;
}
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,19 +82,20 @@ 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();
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
stringstream converter; converter << angleft; my_var = converter.str();
if (angleft >= 60.0 and angright >= 60.0)
/*if ((angleft >= 45.0) and (angright >= 45.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 60.0)
else if (angleft >= 45.0)
{
my_var = "Left foot extended";
}
else if (angright >= 60.0)
else if (angright >= 45.0)
{
my_var = "Right foot extended";
}
......@@ -92,10 +104,15 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
*/
}
catch (tf::TransformException &ex) {
//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();
}
......
......@@ -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");
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);
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;
}
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,19 +82,20 @@ 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();
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
stringstream converter; converter << angleft; my_var = converter.str();
if (angleft >= 60.0 and angright >= 60.0)
/*if ((angleft >= 45.0) and (angright >= 45.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 60.0)
else if (angleft >= 45.0)
{
my_var = "Left foot extended";
}
else if (angright >= 60.0)
else if (angright >= 45.0)
{
my_var = "Right foot extended";
}
......@@ -92,10 +104,15 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
*/
}
catch (tf::TransformException &ex) {
//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();
}
......
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