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

24 April

parent 8c3466ac
Hide whitespace changes
Inline Side-by-side
iri_foot/include/foot_alg_node.h
View file @ 1c0b333e
......@@ -29,6 +29,7 @@
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
//#include "tf/message_filter.h"
// [publisher subscriber headers]
#include <geometry_msgs/PoseStamped.h>
......@@ -55,7 +56,6 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -67,6 +67,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goal;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
......
iri_foot/include/foot_alg_node.h~
View file @ 1c0b333e
......@@ -29,6 +29,7 @@
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
//#include "tf/message_filter.h"
// [publisher subscriber headers]
#include <geometry_msgs/PoseStamped.h>
......@@ -55,7 +56,6 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -66,7 +66,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 transformation;
tf::Vector3 goal;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
......
iri_foot/src/foot_alg_node.cpp
View file @ 1c0b333e
......@@ -3,6 +3,7 @@
using namespace std;
#include <string>
FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
{
......@@ -28,27 +29,31 @@ FootAlgNode::~FootAlgNode(void)
{
// [free dynamic memory]
}
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_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time zero = ros::Time(0);
ros::Time zero = ros::Time::now();
ros::Duration three_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_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
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<100; ++i)
for(int i=0; i<10; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
......@@ -73,28 +78,35 @@ void FootAlgNode::mainNodeThread(void)
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, ros::Time::now(), child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, ros::Time::now(), child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the distance safe
transformer1.lookupTransform(child_distancesafeleft, child_robot, ros::Time::now(),transformrobdistsafe);
goal = transformrobdistsafe.getOrigin();
}
leftfoot = sumalfoot/100;
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
leftfoot = sumalfoot/10;
leftknee = sumalknee/10;
rightfoot = sumarfoot/10;
rightknee = sumarknee/10;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, zero, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, zero, child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the left distance safe
listener.waitForTransform(child_robot, child_distancesafeleft, zero, three_seconds);
//listener.lookupTransform(child_robot, child_distancesafeleft, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, leftfoot, zero,transformrobdistsafe);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
goal = transformrobdistsafe.getOrigin();
double x = goal.x() + 0.2;
double y = goal.y() + 0.2;
double z = goal.z() + 0.2;
goal.setValue(x,y,z);
//obtain the transformation between the robot and the right distance safe
//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.
......@@ -134,7 +146,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg_.data = goal;
this->pose_surface_PoseStamped_msg_.pose.position.x = goal.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goal.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goal.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = leftfoot;
// Initialize the topic message structure
......
iri_foot/src/foot_alg_node.cpp~
View file @ 1c0b333e
......@@ -3,6 +3,7 @@
using namespace std;
#include <string>
FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
{
......@@ -28,27 +29,31 @@ FootAlgNode::~FootAlgNode(void)
{
// [free dynamic memory]
}
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_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time zero = ros::Time(0);
ros::Time zero = ros::Time::now();
ros::Duration three_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_distancesafeleft("/brix_2/user_1/distancesafeleft");
string child_distancesaferight("/brix_2/user_1/distancesaferight");
string child_robot("iri_wam_link_base");
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<100; ++i)
for(int i=0; i<10; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
......@@ -73,28 +78,35 @@ void FootAlgNode::mainNodeThread(void)
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, ros::Time::now(), child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, ros::Time::now(), child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the distance safe
transformer1.lookupTransform(child_distancesafeleft, child_robot, ros::Time::now(),transformrobdistsafe);
goal = transformrobdistsafe.getOrigin();
}
leftfoot = sumalfoot/100;
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
leftfoot = sumalfoot/10;
leftknee = sumalknee/10;
rightfoot = sumarfoot/10;
rightknee = sumarknee/10;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, zero, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, zero, child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the left distance safe
listener.waitForTransform(child_robot, child_distancesafeleft, zero, three_seconds);
//listener.lookupTransform(child_robot, child_distancesafeleft, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, leftfoot, zero,transformrobdistsafe);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
goal = transformrobdistsafe.getOrigin();
double x = goal.x() + 0.2;
double y = goal.y() + 0.2;
double z = goal.z() + 0.2;
goal.setValue(x,y,z);
//obtain the transformation between the robot and the right distance safe
//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.
......@@ -134,7 +146,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg_.data = goal;
this->pose_surface_PoseStamped_msg_.pose.position.x = goal.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goal.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goal.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = leftfoot;
// Initialize the topic message structure
......
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