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

The angle is calculated

parent dc8c9520
......@@ -127,7 +127,8 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
void 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::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
#endif
<launch>
<arg name="tracker_name" default="brix_2" />
<node pkg="kinect2_tracker" type="kinect2_tracker_node" name="kinect2_tracker_node2" output="screen">
<param name="tf_prefix" value="$(arg tracker_name)" />
<param name="relative_frame" value="/$(arg tracker_name)_camera_frame" />
</node>
<!-- TF Static Transforms to World -->
<node pkg="tf" type="static_transform_publisher" name="brix_to_global2"
args="0.295729 -0.463885 -0.0325348 0 0 0 global_space /$(arg tracker_name)_camera_frame 100"/>
<!--tx: 0.295729 ty: -0.463885 tz: -0.0325348 rw: 1 rx: 0 ry: 0 rz: 0-->
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
<launch>
<arg name="tracker_name" default="brix_2" />
<node pkg="kinect2_tracker" type="kinect2_tracker_node" name="kinect2_tracker_node2" output="screen">
<param name="tf_prefix" value="$(arg tracker_name)" />
<param name="relative_frame" value="/$(arg tracker_name)_camera_frame" />
</node>
<!-- TF Static Transforms to World -->
<node pkg="tf" type="static_transform_publisher" name="brix_to_global2"
args="0.295729 -0.463885 -0.0325348 0 0 0 global_space /$(arg tracker_name)_camera_frame 100"/>
<!--tx: 0.295729 ty: -0.463885 tz: -0.0325348 rw: 1 rx: 0 ry: 0 rz: 0-->
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
......@@ -25,11 +25,11 @@ 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 numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
angleft = atan (numleft/denleft);
angright = atan (numright/denright);
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));
}
......@@ -25,11 +25,11 @@ 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 numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
angleft = atan (numleft/denleft);
angright = atan (numright/denright);
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));
}
#include "foot_alg_node.h"
#include "foot_alg.cpp"
//#include "foot_alg.cpp"
using namespace std;
#include <string>
......@@ -30,64 +30,88 @@ FootAlgNode::~FootAlgNode(void)
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time::now();
ros::Duration five_seconds = ros::Duration(5.0);
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Duration five_seconds = ros::Duration(3.0);
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
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");
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
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
//left_foot
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_leftfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
//left_knee
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = trasnform2.getOrigin();
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
//right_foot
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = trasnform3.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin();
//right_knee
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = transform4.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)
//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.
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";
}
}
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
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 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_);
}
......
#include "foot_alg_node.h"
#include "foot_alg.cpp"
//#include "foot_alg.cpp"
using namespace std;
#include <string>
......@@ -30,64 +30,88 @@ FootAlgNode::~FootAlgNode(void)
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time::now();
ros::Duration five_seconds = ros::Duration(5.0);
string my_var = "";
try{
ros::Time now = ros::Time(0);
ros::Duration five_seconds = ros::Duration(3.0);
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
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");
string child_leftfoot("/brix_2/user_1/left_foot");
string child_leftknee("/brix_2/user_1/left_knee");
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
//left_foot
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_leftfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
//left_knee
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = trasnform2.getOrigin();
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = transform2.getOrigin();
//right_foot
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = trasnform3.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = transform3.getOrigin();
//right_knee
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = transform4.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);
//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.
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";
}
}
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
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 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