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

28 April

parent a047e1a9
File mode changed from 100755 to 100644
......@@ -127,8 +127,6 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
#endif
......@@ -28,7 +28,7 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
//#include "tf/message_filter.h"
// [publisher subscriber headers]
......@@ -48,39 +48,15 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
tf::TransformBroadcaster broadcaster;
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobleftdist;
tf::StampedTransform transformrobrightdist;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
tf::Vector3 rightknee;
tf::Vector3 sumalfoot;
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goalleft;
tf::Vector3 goalright;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
// [subscriber attributes]
// [service attributes]
......
......@@ -28,7 +28,7 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_alg.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
//#include "tf/message_filter.h"
// [publisher subscriber headers]
......@@ -48,39 +48,15 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
tf::TransformBroadcaster broadcaster;
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobleftdist;
tf::StampedTransform transformrobrightdist;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
tf::Vector3 rightknee;
tf::Vector3 sumalfoot;
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goalleft;
tf::Vector3 goalright;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
// [publisher attributes]
ros::Publisher pose_surface_publisher_;
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
// [subscriber attributes]
// [service attributes]
......
<launch>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.32 -0.22 1.36 0 3.14159265359 -1.57079632679 brix_2_camera_frame world 100" />
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.21 -0.22 1.36 0 3.14159265359 -1.57079632679 /brix_2_camera_frame world 100" />
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
<launch>
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.32 -0.22 1.36 0 3.14159265359 -1.57079632679 brix_2_camera_frame world 100" />
<node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.11 -0.22 1.36 0 3.14159265359 -1.57079632679 /brix_2_camera_frame world 100" />
<node pkg="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
#include "foot_alg.h"
#include <math.h>
using namespace std;
FootAlgorithm::FootAlgorithm(void)
{
......@@ -24,13 +22,4 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
// FootAlgorithm Public API
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()));
angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright));
}
......@@ -24,13 +24,4 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
// FootAlgorithm Public API
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())*/);
angleft = (180/M_PI) * (atan (numleft/denleft));
angright = (180/M_PI) * (atan (numright/denright));
}
#include "foot_alg_node.h"
//#include "foot_alg.cpp"
using namespace std;
#include <string>
......@@ -8,11 +7,10 @@ FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
//this->loop_rate_ = 50;//in [Hz]
// [init publishers]
this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -29,121 +27,24 @@ 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 = "";
string child_righthand("/brix_2/user_1/right_hand");
string parent_cameraframe("/brix_2_camera_frame");
string child_robot("iri_wam_link_base");
try{
ros::Time zero = ros::Time::now();
ros::Time zero = ros::Time(0);
ros::Time now = ros::Time::now();
ros::Duration three_seconds = ros::Duration(3.0);
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<10; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, zero, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, zero, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, zero, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
}
leftfoot = sumalfoot/10;
leftknee = sumalknee/10;
rightfoot = sumarfoot/10;
rightknee = sumarknee/10;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
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.6, 0.6, 0.6));
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, child_leftfoot, zero,transformrobleftdist);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
listener.waitForTransform(child_robot, child_righthand, zero, three_seconds);
listener.lookupTransform(child_robot, child_righthand, zero,transformrobleftdist);
goalleft = transformrobleftdist.getOrigin();
double xleft = goalleft.x() + 0.6;
double yleft = goalleft.y() + 0.6;
double zleft = goalleft.z() + 0.6;
goalleft.setValue(xleft,yleft,zleft);
//obtain the transformation between the robot and the right distance safe
listener.waitForTransform(child_robot, child_distancesaferight, zero, three_seconds);
listener.lookupTransform(child_robot, child_rightfoot, zero, transformrobrightdist);
goalright = transformrobrightdist.getOrigin();
double xright = goalright.x() + 0.6;
double yright = goalright.y() + 0.6;
double zright = goalright.z() + 0.6;
goalright.setValue(xright,yright,zright);
//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 << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
else
{
my_var = "No foot extended";
}
double xleft = goalleft.x() - 0.0;
double yleft = goalleft.y() - 0.0;
double zleft = goalleft.z() + 0.0;
goalleft.setValue(xleft,yleft,zleft);
}
catch (tf::TransformException &ex)
......@@ -157,18 +58,17 @@ void FootAlgNode::mainNodeThread(void)
this->pose_surface_PoseStamped_msg_.pose.position.x = goalleft.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalleft.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalleft.getZ();
this->pose_surface_PoseStamped_msg_.pose.position.x = goalright.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalright.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalright.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = child_leftfoot;
this->pose_surface_PoseStamped_msg_.pose.orientation.x = 0;
this->pose_surface_PoseStamped_msg_.pose.orientation.y = 1;
this->pose_surface_PoseStamped_msg_.pose.orientation.z = 0;
this->pose_surface_PoseStamped_msg_.pose.orientation.w = 0;
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time(0);
this->pose_surface_PoseStamped_msg_.header.frame_id = "iri_wam_link_base";
// Initialize the topic message structure
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
......@@ -180,7 +80,7 @@ void FootAlgNode::mainNodeThread(void)
// 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_);
}
......
#include "foot_alg_node.h"
//#include "foot_alg.cpp"
using namespace std;
#include <string>
......@@ -8,11 +7,10 @@ FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
//this->loop_rate_ = 50;//in [Hz]
// [init publishers]
this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -29,121 +27,26 @@ 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 = "";
string child_righthand("/brix_2/user_1/right_hand");
string parent_cameraframe("/brix_2_camera_frame");
string child_robot("iri_wam_link_base");
try{
ros::Time zero = ros::Time::now();
ros::Time zero = ros::Time(0);
ros::Time now = ros::Time::now();
ros::Duration three_seconds = ros::Duration(3.0);
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<10; ++i)
{
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, zero, transform1);
leftfoot = transform1.getOrigin();
sumalfoot = sumalfoot + leftfoot;
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, zero, transform2);
leftknee = transform2.getOrigin();
sumalknee = sumalknee + leftknee;
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, zero, transform3);
rightfoot = transform3.getOrigin();
sumarfoot = sumarfoot + rightfoot;
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, zero, three_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, zero, transform4);
rightknee = transform4.getOrigin();
sumarknee = sumarknee + rightknee;
}
leftfoot = sumalfoot/10;
leftknee = sumalknee/10;
rightfoot = sumarfoot/10;
rightknee = sumarknee/10;
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
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.6, 0.6, 0.6));
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, child_leftfoot, zero,transformrobleftdist);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
listener.waitForTransform(child_robot, child_righthand, zero, three_seconds);
listener.lookupTransform(child_robot, child_righthand, zero,transformrobleftdist);
goalleft = transformrobleftdist.getOrigin();
double xleft = goalleft.x() + 0.6;
double yleft = goalleft.y() + 0.6;
double zleft = goalleft.z() + 0.6;
goalleft.setValue(xleft,yleft,zleft);
//obtain the transformation between the robot and the right distance safe
listener.waitForTransform(child_robot, child_distancesaferight, zero, three_seconds);
listener.lookupTransform(child_robot, child_rightfoot, zero, transformrobrightdist);
goalright = transformrobrightdist.getOrigin();
double xright = goalright.x() + 0.6;
double yright = goalright.y() + 0.6;
double zright = goalright.z() + 0.6;
goalright.setValue(xright,yright,zright);
//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 << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
else
{
my_var = "No foot extended";
}
double xleft = goalleft.x() - 0.0;
double yleft = goalleft.y() - 0.0;
double zleft = goalleft.z() + 0.0;
goalleft.setValue(xleft,yleft,zleft);
}
catch (tf::TransformException &ex)
......@@ -157,18 +60,17 @@ void FootAlgNode::mainNodeThread(void)
this->pose_surface_PoseStamped_msg_.pose.position.x = goalleft.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalleft.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalleft.getZ();
this->pose_surface_PoseStamped_msg_.pose.position.x = goalright.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalright.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalright.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = child_leftfoot;
this->pose_surface_PoseStamped_msg_.pose.orientation.x = 0;
this->pose_surface_PoseStamped_msg_.pose.orientation.y = 1;
this->pose_surface_PoseStamped_msg_.pose.orientation.z = 0;
this->pose_surface_PoseStamped_msg_.pose.orientation.w = 0;
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time(0);
this->pose_surface_PoseStamped_msg_.header.frame_id = "iri_wam_link_base";
// Initialize the topic message structure
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
......@@ -180,7 +82,7 @@ void FootAlgNode::mainNodeThread(void)
// 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_);
}
......
cmake_minimum_required(VERSION 2.8.3)
project(iri_foot_extended)
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
# ********************************************************************
# Add system and labrobotica dependencies here
# ********************************************************************
# find_package(<dependency> REQUIRED)
# ********************************************************************
# Add topic, service and action definition here
# ********************************************************************
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
# ********************************************************************
# Add the dynamic reconfigure file
# ********************************************************************
generate_dynamic_reconfigure_options(cfg/FootExtended.cfg)
# ********************************************************************
# Add run time dependencies here
# ********************************************************************
catkin_package(
# INCLUDE_DIRS
# LIBRARIES
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
# DEPENDS
)
###########
## Build ##
###########
# ********************************************************************
# Add the include directories
# ********************************************************************
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
# include_directories(${<dependency>_INCLUDE_DIR})
## Declare a cpp library
# add_library(${PROJECT_NAME} <list of source files>)
## Declare a cpp executable
add_executable(${PROJECT_NAME} src/foot_extended_alg.cpp src/foot_extended_alg_node.cpp)
# ********************************************************************
# Add the libraries
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})