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

Foot filter

parent c14f342c
Hide whitespace changes
Inline Side-by-side
iri_foot/include/foot_alg_node.h
View file @ da6e4822
......@@ -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_;
......
iri_foot/src/foot_alg.cpp
View file @ da6e4822
#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));
}
......
iri_foot/src/foot_alg.cpp~
View file @ da6e4822
#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));
}
......
iri_foot/src/foot_alg_node.cpp
View file @ da6e4822
......@@ -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_);
}
......
iri_foot/src/foot_alg_node.cpp~
View file @ da6e4822
......@@ -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_);
}
......
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