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> ...@@ -53,6 +53,10 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 leftfoot; tf::Vector3 leftfoot;
tf::Vector3 leftknee; tf::Vector3 leftknee;
tf::Vector3 rightknee; tf::Vector3 rightknee;
tf::Vector3 sumalfoot;
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
// [publisher attributes] // [publisher attributes]
ros::Publisher myfeet_publisher_; ros::Publisher myfeet_publisher_;
......
#include "foot_alg.h" #include "foot_alg.h"
#include <math.h> #include <math.h>
using namespace std;
FootAlgorithm::FootAlgorithm(void) FootAlgorithm::FootAlgorithm(void)
{ {
...@@ -25,10 +26,10 @@ void FootAlgorithm::config_update(Config& config, uint32_t level) ...@@ -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) 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 denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.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 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)); angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright)); angright = (180/M_PI) * (atan (numright/denright));
} }
......
#include "foot_alg.h" #include "foot_alg.h"
#include <math.h> #include <math.h>
using namespace std;
FootAlgorithm::FootAlgorithm(void) FootAlgorithm::FootAlgorithm(void)
{ {
...@@ -25,10 +26,10 @@ void FootAlgorithm::config_update(Config& config, uint32_t level) ...@@ -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) 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 denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.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 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)); angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright)); angright = (180/M_PI) * (atan (numright/denright));
} }
......
...@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void) ...@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void)
string child_rightfoot("/brix_2/user_1/right_foot"); string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee"); string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame"); string parent_cameraframe("/brix_2_camera_frame");
sumalfoot.setX(0); sumalfoot.setY(0); sumalfoot.setZ(0);
//left_foot sumarfoot.setX(0); sumarfoot.setY(0); sumarfoot.setZ(0);
sumalknee.setX(0); sumalknee.setY(0); sumalknee.setZ(0);
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds); sumarknee.setX(0); sumarknee.setY(0); sumarknee.setZ(0);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1); for(int i=0; i<30; ++i)
leftfoot = transform1.getOrigin(); {
//left_foot
//left_knee listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds); sumalfoot = sumalfoot + leftfoot;
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin(); //left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
//right_foot listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds); sumalknee = sumalknee + leftknee;
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin(); //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 leftfoot = sumalfoot/30;
leftknee = sumalknee/30;
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds); rightfoot = sumarfoot/30;
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4); rightknee = sumarknee/30;
rightknee = transform4.getOrigin();
//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. //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) ...@@ -71,47 +82,53 @@ void FootAlgNode::mainNodeThread(void)
double angleft = 0.0; double angleft = 0.0;
double angright = 0.0; double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright); 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"; {
} my_var = "Left foot and right foot extended";
}
else if (angleft >= 60.0)
{ else if (angleft >= 45.0)
my_var = "Left foot extended"; {
} my_var = "Left foot extended";
}
else if (angright >= 60.0)
{ else if (angright >= 45.0)
my_var = "Right foot extended"; {
} my_var = "Right foot extended";
}
else
{ else
my_var = "No foot extended"; {
} my_var = "No foot extended";
}
*/
} }
catch (tf::TransformException &ex) { //Apply the distance (20 cm) between the foot and the gripper (The safe distance).
ROS_ERROR("%s",ex.what()); //leftfoot = transform1.getOrigin() ;
ros::Duration(1.0).sleep(); //rightfoot = transform3.getOrigin();
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
// [fill msg structures] // [fill msg structures]
// Initialize the topic message structure // Initialize the topic message structure
this->myfeet_String_msg_.data = my_var; 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] // [publish messages]
// Uncomment the following line to publish the topic message // Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_); this->myfeet_publisher_.publish(this->myfeet_String_msg_);
} }
......
...@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void) ...@@ -40,30 +40,41 @@ void FootAlgNode::mainNodeThread(void)
string child_rightfoot("/brix_2/user_1/right_foot"); string child_rightfoot("/brix_2/user_1/right_foot");
string child_rightknee("/brix_2/user_1/right_knee"); string child_rightknee("/brix_2/user_1/right_knee");
string parent_cameraframe("/brix_2_camera_frame"); string parent_cameraframe("/brix_2_camera_frame");
sumalfoot = 0;
//left_foot sumarfoot = 0;
sumalknee = 0;
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds); sumarknee = 0;
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1); for(int i=0; i<30; ++i)
leftfoot = transform1.getOrigin(); {
//left_foot
//left_knee listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds); sumalfoot = sumalfoot + leftfoot;
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin(); //left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
//right_foot listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds); sumalknee = sumalknee + leftknee;
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin(); //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 leftfoot = sumalfoot/30;
leftknee = sumalknee/30;
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds); rightfoot = sumarfoot/30;
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4); rightknee = sumarknee/30;
rightknee = transform4.getOrigin();
//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. //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) ...@@ -71,47 +82,53 @@ void FootAlgNode::mainNodeThread(void)
double angleft = 0.0; double angleft = 0.0;
double angright = 0.0; double angright = 0.0;
alg_.angle(leftfoot, leftknee, rightfoot, rightknee, angleft, angright); 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"; {
} my_var = "Left foot and right foot extended";
}
else if (angleft >= 60.0)
{ else if (angleft >= 45.0)
my_var = "Left foot extended"; {
} my_var = "Left foot extended";
}
else if (angright >= 60.0)
{ else if (angright >= 45.0)
my_var = "Right foot extended"; {
} my_var = "Right foot extended";
}
else
{ else
my_var = "No foot extended"; {
} my_var = "No foot extended";
}
*/
} }
catch (tf::TransformException &ex) { //Apply the distance (20 cm) between the foot and the gripper (The safe distance).
ROS_ERROR("%s",ex.what()); //leftfoot = transform1.getOrigin() ;
ros::Duration(1.0).sleep(); //rightfoot = transform3.getOrigin();
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
// [fill msg structures] // [fill msg structures]
// Initialize the topic message structure // Initialize the topic message structure
this->myfeet_String_msg_.data = my_var; 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] // [publish messages]
// Uncomment the following line to publish the topic message // Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_); this->myfeet_publisher_.publish(this->myfeet_String_msg_);
} }
......
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