diff --git a/iri_foot/CMakeLists.txt b/iri_foot/CMakeLists.txt
index adb0c516946f0fe5f55917a811ce42a05c1eda09..f73ed60fbf694fa9f9aefa6a942bda8f6ee60ce2 100644
--- a/iri_foot/CMakeLists.txt
+++ b/iri_foot/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs tf)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm geometry_msgs std_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm
+ CATKIN_DEPENDS iri_base_algorithm geometry_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -94,6 +94,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Add message headers dependencies
# ********************************************************************
#add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
diff --git a/iri_foot/include/foot_alg.h~ b/iri_foot/include/foot_alg.h~
index 9a54ad9db2e4be0785cae7a1888577154e234eb7..e40744981fed0def4b1fa4adaf66b3ffa77d24e7 100644
--- a/iri_foot/include/foot_alg.h~
+++ b/iri_foot/include/foot_alg.h~
@@ -127,6 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
+
void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
diff --git a/iri_foot/include/foot_alg_node.h b/iri_foot/include/foot_alg_node.h
index 1eb5d68ceefe390edb22ac3007d26b9e4f313228..3d59dccbd4f84e537eaf516f2f6966b7f4931bbc 100644
--- a/iri_foot/include/foot_alg_node.h
+++ b/iri_foot/include/foot_alg_node.h
@@ -31,6 +31,7 @@
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
+#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
@@ -53,6 +54,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
+ tf::StampedTransform transformrobdistsafe;
+ tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
@@ -62,8 +65,13 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
+ tf::Vector3 robot;
+ tf::Vector3 goal;
// [publisher attributes]
+ ros::Publisher pose_surface_publisher_;
+ geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
+
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
diff --git a/iri_foot/include/foot_alg_node.h~ b/iri_foot/include/foot_alg_node.h~
index 126df2142808a2e1494c709f187133ceec5334b5..4423be6825c78747ab311df0e6ae8d55b2da5aa6 100644
--- a/iri_foot/include/foot_alg_node.h~
+++ b/iri_foot/include/foot_alg_node.h~
@@ -31,6 +31,7 @@
#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
+#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
@@ -52,6 +53,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::StampedTransform transform5;
+ tf::StampedTransform transform6;
+ tf::StampedTransform transformrobdistsafe;
+ tf::Transformer transformer1;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
@@ -61,8 +65,13 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
+ tf::Vector3 robot;
+ tf::Vector3 transformation;
// [publisher attributes]
+ ros::Publisher pose_surface_publisher_;
+ geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
+
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
diff --git a/iri_foot/launch/foot.launch b/iri_foot/launch/foot.launch
index 2faa6c2be195f6b89d29e8d7a7ca17c43ce1cbd4..6a3cea3f54d88ddee80b44cba3517c3fb7e7badd 100644
--- a/iri_foot/launch/foot.launch
+++ b/iri_foot/launch/foot.launch
@@ -1,4 +1,4 @@
<launch>
- <node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="0.3 0.29 0.3 0 0 -1.57079632679 brix_2_camera_frame world 100" />
+ <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="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
diff --git a/iri_foot/launch/foot.launch~ b/iri_foot/launch/foot.launch~
index 56001b9c77c536d8c8d62bed7086c0876609396a..6a3cea3f54d88ddee80b44cba3517c3fb7e7badd 100644
--- a/iri_foot/launch/foot.launch~
+++ b/iri_foot/launch/foot.launch~
@@ -1,4 +1,4 @@
<launch>
- <node pkg="tf" type="static_transform_publisher" name="tfcameratorobot" args="1.05 0.29 1.65 0 0 -1.57079632679 brix_2_camera_frame world 100" />
+ <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="iri_foot" type ="iri_foot" name="iri_foot"/>
</launch>
diff --git a/iri_foot/package.xml b/iri_foot/package.xml
index d79bc40c6ae04dd0e8f4aca04fc9dca04918dcf9..b5a0677bfc72eb98794ed8a5e3a15578eee8e8d5 100644
--- a/iri_foot/package.xml
+++ b/iri_foot/package.xml
@@ -41,9 +41,11 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
+ <build_depend>geometry_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
+ <run_depend>geometry_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/iri_foot/src/foot_alg_node.cpp b/iri_foot/src/foot_alg_node.cpp
index 87d0c2b44e30b3c929ca50e373ec8b68eb28e294..0c1a27b29cf2cac733c02402283a174320619bdd 100644
--- a/iri_foot/src/foot_alg_node.cpp
+++ b/iri_foot/src/foot_alg_node.cpp
@@ -10,6 +10,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//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]
@@ -41,6 +42,7 @@ void FootAlgNode::mainNodeThread(void)
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);
@@ -81,6 +83,11 @@ void FootAlgNode::mainNodeThread(void)
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;
@@ -98,17 +105,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
- if ((angleft >= 25.0) and (angright >= 25.0))
+ if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
- else if (angleft >= 25.0)
+ else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
- else if (angright >= 25.0)
+ else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
@@ -117,15 +124,6 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
- //Apply the distance (20 cm) between the foot and the gripper (The safe distance).
-
- /*leftfoot.setX(leftfoot.getX() + 0.2);
- leftfoot.setY(leftfoot.getY() + 0.2);
- leftfoot.setZ(leftfoot.getZ() + 0.2);
- rightfoot.setX(rightfoot.getX() + 0.2);
- rightfoot.setY(rightfoot.getY() + 0.2);
- rightfoot.setZ(rightfoot.getZ() + 0.2);*/
-
}
catch (tf::TransformException &ex)
@@ -135,6 +133,11 @@ void FootAlgNode::mainNodeThread(void)
}
// [fill msg structures]
+ // Initialize the topic message structure
+ this->pose_surface_PoseStamped_msg_ = goal;
+
+ // Initialize the topic message structure
+
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
@@ -144,6 +147,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
+
+ // 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_);
diff --git a/iri_foot/src/foot_alg_node.cpp~ b/iri_foot/src/foot_alg_node.cpp~
index 87d0c2b44e30b3c929ca50e373ec8b68eb28e294..0f2951ac240916ce911229bcf613cf2417134d75 100644
--- a/iri_foot/src/foot_alg_node.cpp~
+++ b/iri_foot/src/foot_alg_node.cpp~
@@ -10,6 +10,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//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]
@@ -41,6 +42,7 @@ void FootAlgNode::mainNodeThread(void)
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);
@@ -81,6 +83,11 @@ void FootAlgNode::mainNodeThread(void)
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;
@@ -98,17 +105,17 @@ void FootAlgNode::mainNodeThread(void)
//stringstream converter; converter << leftfoot(1); my_var = converter.str();
//stringstream converter; converter << angleft; my_var = converter.str();
- if ((angleft >= 25.0) and (angright >= 25.0))
+ if ((angleft >= 35.0) and (angright >= 35.0))
{
my_var = "Left foot and right foot extended";
}
- else if (angleft >= 25.0)
+ else if (angleft >= 35.0)
{
my_var = "Left foot extended";
}
- else if (angright >= 25.0)
+ else if (angright >= 35.0)
{
my_var = "Right foot extended";
}
@@ -117,15 +124,6 @@ void FootAlgNode::mainNodeThread(void)
{
my_var = "No foot extended";
}
- //Apply the distance (20 cm) between the foot and the gripper (The safe distance).
-
- /*leftfoot.setX(leftfoot.getX() + 0.2);
- leftfoot.setY(leftfoot.getY() + 0.2);
- leftfoot.setZ(leftfoot.getZ() + 0.2);
- rightfoot.setX(rightfoot.getX() + 0.2);
- rightfoot.setY(rightfoot.getY() + 0.2);
- rightfoot.setZ(rightfoot.getZ() + 0.2);*/
-
}
catch (tf::TransformException &ex)
@@ -135,6 +133,11 @@ void FootAlgNode::mainNodeThread(void)
}
// [fill msg structures]
+ // Initialize the topic message structure
+ this->pose_surface_PoseStamped_msg = goal;
+
+ // Initialize the topic message structure
+
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
@@ -144,6 +147,11 @@ void FootAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->pose_surface_publisher_.publish(this->pose_surface_PoseStamped_msg_);
+
+ // 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_);