diff --git a/iri_foot/CMakeLists.txt b/Foot_extended/iri_foot/CMakeLists.txt
similarity index 100%
rename from iri_foot/CMakeLists.txt
rename to Foot_extended/iri_foot/CMakeLists.txt
diff --git a/iri_foot/CMakeLists.txt~ b/Foot_extended/iri_foot/CMakeLists.txt~
similarity index 100%
rename from iri_foot/CMakeLists.txt~
rename to Foot_extended/iri_foot/CMakeLists.txt~
diff --git a/iri_foot/cfg/Foot.cfg b/Foot_extended/iri_foot/cfg/Foot.cfg
similarity index 100%
rename from iri_foot/cfg/Foot.cfg
rename to Foot_extended/iri_foot/cfg/Foot.cfg
diff --git a/iri_foot/include/foot_alg.h b/Foot_extended/iri_foot/include/foot_alg.h
similarity index 100%
rename from iri_foot/include/foot_alg.h
rename to Foot_extended/iri_foot/include/foot_alg.h
diff --git a/iri_foot/include/foot_alg.h~ b/Foot_extended/iri_foot/include/foot_alg.h~
similarity index 100%
rename from iri_foot/include/foot_alg.h~
rename to Foot_extended/iri_foot/include/foot_alg.h~
diff --git a/iri_foot/include/foot_alg_node.h b/Foot_extended/iri_foot/include/foot_alg_node.h
similarity index 92%
rename from iri_foot/include/foot_alg_node.h
rename to Foot_extended/iri_foot/include/foot_alg_node.h
index 799ac87eb7d4d1457e396223446fdc7973ee2ba4..c7fe249da58f819e9fd3e591a921ab3d8f21ab08 100644
--- a/iri_foot/include/foot_alg_node.h
+++ b/Foot_extended/iri_foot/include/foot_alg_node.h
@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
- tf::StampedTransform transformrobleftdist;
- tf::StampedTransform transformrobrightdist;
+ tf::StampedTransform transformrobrightfoot;
+ tf::StampedTransform transformrobleftfoot;
tf::Vector3 goalleft;
tf::Vector3 goalright;
@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
// [subscriber attributes]
+ ros::Subscriber feetextended_subscriber_;
+ void feetextended_callback(const std_msgs::String::ConstPtr& msg);
+ pthread_mutex_t feetextended_mutex_;
+ void feetextended_mutex_enter(void);
+ void feetextended_mutex_exit(void);
+
// [service attributes]
diff --git a/iri_foot/include/foot_alg_node.h~ b/Foot_extended/iri_foot/include/foot_alg_node.h~
similarity index 92%
rename from iri_foot/include/foot_alg_node.h~
rename to Foot_extended/iri_foot/include/foot_alg_node.h~
index 799ac87eb7d4d1457e396223446fdc7973ee2ba4..c7fe249da58f819e9fd3e591a921ab3d8f21ab08 100644
--- a/iri_foot/include/foot_alg_node.h~
+++ b/Foot_extended/iri_foot/include/foot_alg_node.h~
@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
- tf::StampedTransform transformrobleftdist;
- tf::StampedTransform transformrobrightdist;
+ tf::StampedTransform transformrobrightfoot;
+ tf::StampedTransform transformrobleftfoot;
tf::Vector3 goalleft;
tf::Vector3 goalright;
@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
// [subscriber attributes]
+ ros::Subscriber feetextended_subscriber_;
+ void feetextended_callback(const std_msgs::String::ConstPtr& msg);
+ pthread_mutex_t feetextended_mutex_;
+ void feetextended_mutex_enter(void);
+ void feetextended_mutex_exit(void);
+
// [service attributes]
diff --git a/iri_foot/launch/foot.launch b/Foot_extended/iri_foot/launch/foot.launch
similarity index 100%
rename from iri_foot/launch/foot.launch
rename to Foot_extended/iri_foot/launch/foot.launch
diff --git a/iri_foot/launch/foot.launch~ b/Foot_extended/iri_foot/launch/foot.launch~
similarity index 100%
rename from iri_foot/launch/foot.launch~
rename to Foot_extended/iri_foot/launch/foot.launch~
diff --git a/iri_foot/package.xml b/Foot_extended/iri_foot/package.xml
similarity index 100%
rename from iri_foot/package.xml
rename to Foot_extended/iri_foot/package.xml
diff --git a/iri_foot/package.xml~ b/Foot_extended/iri_foot/package.xml~
similarity index 100%
rename from iri_foot/package.xml~
rename to Foot_extended/iri_foot/package.xml~
diff --git a/iri_foot/src/foot_alg.cpp b/Foot_extended/iri_foot/src/foot_alg.cpp
similarity index 100%
rename from iri_foot/src/foot_alg.cpp
rename to Foot_extended/iri_foot/src/foot_alg.cpp
diff --git a/iri_foot/src/foot_alg.cpp~ b/Foot_extended/iri_foot/src/foot_alg.cpp~
similarity index 100%
rename from iri_foot/src/foot_alg.cpp~
rename to Foot_extended/iri_foot/src/foot_alg.cpp~
diff --git a/Foot_extended/iri_foot/src/foot_alg_node.cpp b/Foot_extended/iri_foot/src/foot_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3861b4e6cc268950ced91d851c9b94d7741bf15a
--- /dev/null
+++ b/Foot_extended/iri_foot/src/foot_alg_node.cpp
@@ -0,0 +1,170 @@
+#include "foot_alg_node.h"
+using namespace std;
+#include <string>
+
+
+FootAlgNode::FootAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 50;//in [Hz]
+
+ // [init publishers]
+ this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
+
+ // [init subscribers]
+ this->feetextended_subscriber_ = this->public_node_handle_.subscribe("feetextended", 10, &FootAlgNode::feetextended_callback, this);
+ pthread_mutex_init(&this->feetextended_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+}
+
+FootAlgNode::~FootAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::mainNodeThread(void)
+{
+ string child_rightfoot("/brix_2/user_1/right_foot");
+ string child_leftfoot("/brix_2/user_1/left_foot");
+ string parent_cameraframe("/brix_2_camera_frame");
+ string child_robot("iri_wam_link_base");
+ try{
+ ros::Time zero = ros::Time(0);
+ ros::Time now = ros::Time::now();
+ ros::Duration three_seconds = ros::Duration(3.0);
+ listener.waitForTransform(child_robot, child_rightfoot, zero, three_seconds);
+ listener.waitForTransform(child_robot, child_leftfoot, zero, three_seconds);
+ listener.lookupTransform(child_robot, child_rightfoot, zero,transformrobrightfoot);
+ listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftfoot);
+ /*if (msg->data == "Right foot extended")
+ {
+ //The robot has to go to the right foot
+ goalright = transformrobrightfoot.getOrigin();
+ }
+ else if (msg->data == "Left foot extended")
+ {
+ //The robot has to go to the left foot
+ goalleft = transformrobleftfoot.getOrigin();
+ }
+ else
+ {
+ //The robot has to turn back to the initial position
+
+
+ }
+ */
+
+}
+
+ catch (tf::TransformException &ex)
+ {
+ ROS_ERROR("%s",ex.what());
+ ros::Duration(1.0).sleep();
+ }
+
+ // [fill msg structures]
+ // Initialize the topic message structure
+ 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.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
+
+ // [fill srv structure and make request to the server]
+
+ // [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
+
+
+}
+
+/* [subscriber callbacks] */
+void FootAlgNode::feetextended_callback(const std_msgs::String::ConstPtr& msg)
+{
+ ROS_INFO("FootAlgNode::feetextended_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ this->alg_.lock();
+ this->feetextended_mutex_enter();
+
+ std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ this->alg_.unlock();
+ this->feetextended_mutex_exit();
+ if (msg->data == "Right foot extended")
+ {
+ //The robot has to go to the right foot
+ goalright = transformrobrightfoot.getOrigin();
+ }
+ else if (msg->data == "Left foot extended")
+ {
+ //The robot has to go to the left foot
+ goalleft = transformrobleftfoot.getOrigin();
+ }
+ /*else
+ {
+ //The robot has to turn back to the initial position
+
+
+ }
+ */
+
+}
+
+void FootAlgNode::feetextended_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::feetextended_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->feetextended_mutex_);
+}
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void FootAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void FootAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<FootAlgNode>(argc, argv, "foot_alg_node");
+}
diff --git a/Foot_extended/iri_foot/src/foot_alg_node.cpp~ b/Foot_extended/iri_foot/src/foot_alg_node.cpp~
new file mode 100644
index 0000000000000000000000000000000000000000..3861b4e6cc268950ced91d851c9b94d7741bf15a
--- /dev/null
+++ b/Foot_extended/iri_foot/src/foot_alg_node.cpp~
@@ -0,0 +1,170 @@
+#include "foot_alg_node.h"
+using namespace std;
+#include <string>
+
+
+FootAlgNode::FootAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 50;//in [Hz]
+
+ // [init publishers]
+ this->pose_surface_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose_surface", 10);
+
+ // [init subscribers]
+ this->feetextended_subscriber_ = this->public_node_handle_.subscribe("feetextended", 10, &FootAlgNode::feetextended_callback, this);
+ pthread_mutex_init(&this->feetextended_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+}
+
+FootAlgNode::~FootAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::mainNodeThread(void)
+{
+ string child_rightfoot("/brix_2/user_1/right_foot");
+ string child_leftfoot("/brix_2/user_1/left_foot");
+ string parent_cameraframe("/brix_2_camera_frame");
+ string child_robot("iri_wam_link_base");
+ try{
+ ros::Time zero = ros::Time(0);
+ ros::Time now = ros::Time::now();
+ ros::Duration three_seconds = ros::Duration(3.0);
+ listener.waitForTransform(child_robot, child_rightfoot, zero, three_seconds);
+ listener.waitForTransform(child_robot, child_leftfoot, zero, three_seconds);
+ listener.lookupTransform(child_robot, child_rightfoot, zero,transformrobrightfoot);
+ listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftfoot);
+ /*if (msg->data == "Right foot extended")
+ {
+ //The robot has to go to the right foot
+ goalright = transformrobrightfoot.getOrigin();
+ }
+ else if (msg->data == "Left foot extended")
+ {
+ //The robot has to go to the left foot
+ goalleft = transformrobleftfoot.getOrigin();
+ }
+ else
+ {
+ //The robot has to turn back to the initial position
+
+
+ }
+ */
+
+}
+
+ catch (tf::TransformException &ex)
+ {
+ ROS_ERROR("%s",ex.what());
+ ros::Duration(1.0).sleep();
+ }
+
+ // [fill msg structures]
+ // Initialize the topic message structure
+ 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.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
+
+ // [fill srv structure and make request to the server]
+
+ // [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
+
+
+}
+
+/* [subscriber callbacks] */
+void FootAlgNode::feetextended_callback(const std_msgs::String::ConstPtr& msg)
+{
+ ROS_INFO("FootAlgNode::feetextended_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ this->alg_.lock();
+ this->feetextended_mutex_enter();
+
+ std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ this->alg_.unlock();
+ this->feetextended_mutex_exit();
+ if (msg->data == "Right foot extended")
+ {
+ //The robot has to go to the right foot
+ goalright = transformrobrightfoot.getOrigin();
+ }
+ else if (msg->data == "Left foot extended")
+ {
+ //The robot has to go to the left foot
+ goalleft = transformrobleftfoot.getOrigin();
+ }
+ /*else
+ {
+ //The robot has to turn back to the initial position
+
+
+ }
+ */
+
+}
+
+void FootAlgNode::feetextended_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->feetextended_mutex_);
+}
+
+void FootAlgNode::feetextended_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->feetextended_mutex_);
+}
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void FootAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void FootAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<FootAlgNode>(argc, argv, "foot_alg_node");
+}
diff --git a/Foot_extended/iri_foot_extended/CMakeLists.txt b/Foot_extended/iri_foot_extended/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..99775c9c3f0746af55f90fb73674fec097daf437
--- /dev/null
+++ b/Foot_extended/iri_foot_extended/CMakeLists.txt
@@ -0,0 +1,101 @@
+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 std_msgs tf)
+
+## 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 std_msgs
+# ********************************************************************
+# 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})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/iri_foot_extended/CMakeLists.txt b/Foot_extended/iri_foot_extended/CMakeLists.txt~
similarity index 95%
rename from iri_foot_extended/CMakeLists.txt
rename to Foot_extended/iri_foot_extended/CMakeLists.txt~
index 6597219c6338baebb9a31552646edfdd985a8ad3..528d54ad7535d63acbfcb4f65430777d71591c2f 100644
--- a/iri_foot_extended/CMakeLists.txt
+++ b/Foot_extended/iri_foot_extended/CMakeLists.txt~
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs)
## 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 std_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} std_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
diff --git a/iri_foot_extended/cfg/FootExtended.cfg b/Foot_extended/iri_foot_extended/cfg/FootExtended.cfg
similarity index 100%
rename from iri_foot_extended/cfg/FootExtended.cfg
rename to Foot_extended/iri_foot_extended/cfg/FootExtended.cfg
diff --git a/iri_foot_extended/include/foot_extended_alg.h b/Foot_extended/iri_foot_extended/include/foot_extended_alg.h
similarity index 100%
rename from iri_foot_extended/include/foot_extended_alg.h
rename to Foot_extended/iri_foot_extended/include/foot_extended_alg.h
diff --git a/iri_foot_extended/include/foot_extended_alg.h~ b/Foot_extended/iri_foot_extended/include/foot_extended_alg.h~
similarity index 100%
rename from iri_foot_extended/include/foot_extended_alg.h~
rename to Foot_extended/iri_foot_extended/include/foot_extended_alg.h~
diff --git a/iri_foot_extended/include/foot_extended_alg_node.h b/Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h
similarity index 95%
rename from iri_foot_extended/include/foot_extended_alg_node.h
rename to Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h
index 250b78c121c3b1180b47b1fed9e33fd999b9a6f9..3e73cef9ec3eacf1a23eb884150e5a0fbaf4fcb8 100644
--- a/iri_foot_extended/include/foot_extended_alg_node.h
+++ b/Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h
@@ -28,7 +28,6 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "foot_extended_alg.h"
#include <tf/transform_listener.h>
-#include <tf/transform_broadcaster.h>
// [publisher subscriber headers]
#include <std_msgs/String.h>
@@ -45,13 +44,10 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
{
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::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
@@ -60,8 +56,11 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
- tf::Vector3 distancesafe;
+
// [publisher attributes]
+ ros::Publisher feetextended_publisher_;
+ std_msgs::String feetextended_String_msg_;
+
// [subscriber attributes]
diff --git a/iri_foot_extended/include/foot_extended_alg_node.h~ b/Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h~
similarity index 97%
rename from iri_foot_extended/include/foot_extended_alg_node.h~
rename to Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h~
index 250b78c121c3b1180b47b1fed9e33fd999b9a6f9..86a024239e018a56c8a8b801c54f07ad344dbd8c 100644
--- a/iri_foot_extended/include/foot_extended_alg_node.h~
+++ b/Foot_extended/iri_foot_extended/include/foot_extended_alg_node.h~
@@ -50,8 +50,6 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
- tf::StampedTransform transform5;
- tf::StampedTransform transform6;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
@@ -60,8 +58,11 @@ class FootExtendedAlgNode : public algorithm_base::IriBaseAlgorithm<FootExtended
tf::Vector3 sumarfoot;
tf::Vector3 sumalknee;
tf::Vector3 sumarknee;
- tf::Vector3 distancesafe;
+
// [publisher attributes]
+ ros::Publisher feetextended_publisher_;
+ std_msgs::String feetextended_String_msg_;
+
// [subscriber attributes]
diff --git a/iri_foot_extended/launch/foot.launch~ b/Foot_extended/iri_foot_extended/launch/foot.launch~
similarity index 100%
rename from iri_foot_extended/launch/foot.launch~
rename to Foot_extended/iri_foot_extended/launch/foot.launch~
diff --git a/Foot_extended/iri_foot_extended/launch/foot_extended.launch b/Foot_extended/iri_foot_extended/launch/foot_extended.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bc74684f0406518f585b1606709a6d7d0ad5b5ab
--- /dev/null
+++ b/Foot_extended/iri_foot_extended/launch/foot_extended.launch
@@ -0,0 +1,4 @@
+<launch>
+ <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_extended" type ="iri_foot_extended" name="iri_foot_extended"/>
+</launch>
diff --git a/Foot_extended/iri_foot_extended/launch/foot_extended.launch~ b/Foot_extended/iri_foot_extended/launch/foot_extended.launch~
new file mode 100644
index 0000000000000000000000000000000000000000..bc74684f0406518f585b1606709a6d7d0ad5b5ab
--- /dev/null
+++ b/Foot_extended/iri_foot_extended/launch/foot_extended.launch~
@@ -0,0 +1,4 @@
+<launch>
+ <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_extended" type ="iri_foot_extended" name="iri_foot_extended"/>
+</launch>
diff --git a/Foot_extended/iri_foot_extended/package.xml b/Foot_extended/iri_foot_extended/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e4fab076bea53d0ce7300dcc8dd60e844a898f87
--- /dev/null
+++ b/Foot_extended/iri_foot_extended/package.xml
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<package>
+ <name>iri_foot_extended</name>
+ <version>0.0.0</version>
+ <description>The iri_foot_extended package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="vsilos@todo.todo">vsilos</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/iri_foot_extended</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>tf</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/iri_foot_extended/package.xml b/Foot_extended/iri_foot_extended/package.xml~
similarity index 96%
rename from iri_foot_extended/package.xml
rename to Foot_extended/iri_foot_extended/package.xml~
index e8446db6a804fd84e1ff7d8338ba7e5d7eac1a55..b169fd8d50358eab704bc8e7dc4d50a2cfc3cf4b 100644
--- a/iri_foot_extended/package.xml
+++ b/Foot_extended/iri_foot_extended/package.xml~
@@ -41,7 +41,9 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
+ <build_depend>std_msgs</build_depend>
<run_depend>iri_base_algorithm</run_depend>
+ <run_depend>std_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/iri_foot_extended/src/foot_extended_alg.cpp b/Foot_extended/iri_foot_extended/src/foot_extended_alg.cpp
similarity index 86%
rename from iri_foot_extended/src/foot_extended_alg.cpp
rename to Foot_extended/iri_foot_extended/src/foot_extended_alg.cpp
index b640d9cd34238541ae3eed0191ea75587c56f2da..ab95e234c7871e80f58a43b83f5cdfe0af566643 100644
--- a/iri_foot_extended/src/foot_extended_alg.cpp
+++ b/Foot_extended/iri_foot_extended/src/foot_extended_alg.cpp
@@ -21,7 +21,7 @@ void FootExtendedAlgorithm::config_update(Config& config, uint32_t level)
}
// FootExtendedAlgorithm Public API
-void FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright)
+void FootExtendedAlgorithm::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();
diff --git a/iri_foot_extended/src/foot_extended_alg.cpp~ b/Foot_extended/iri_foot_extended/src/foot_extended_alg.cpp~
similarity index 100%
rename from iri_foot_extended/src/foot_extended_alg.cpp~
rename to Foot_extended/iri_foot_extended/src/foot_extended_alg.cpp~
diff --git a/iri_foot_extended/src/foot_extended_alg_node.cpp b/Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp
similarity index 77%
rename from iri_foot_extended/src/foot_extended_alg_node.cpp
rename to Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp
index 8a6b9c675d620d27ab0c05a599ce10b24083518f..597943794d9fd1f17c3c627ccaa28eae4f722a35 100644
--- a/iri_foot_extended/src/foot_extended_alg_node.cpp
+++ b/Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp
@@ -8,6 +8,7 @@ FootExtendedAlgNode::FootExtendedAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
+ this->feetextended_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("feetextended", 10);
// [init subscribers]
@@ -27,24 +28,19 @@ FootExtendedAlgNode::~FootExtendedAlgNode(void)
void FootExtendedAlgNode::mainNodeThread(void)
{
- 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");
string my_var = "";
try{
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);
+ 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");
for(int i=0; i<100; ++i)
{
@@ -77,19 +73,8 @@ void FootExtendedAlgNode::mainNodeThread(void)
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
-
- //safe distance leftfoot
- transform5.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
- transform5.setRotation(tf::Quaternion(1, 0, 0, 0));
- broadcaster.sendTransform(tf::StampedTransform(transform5, now, child_leftfoot, child_distancesafeleft));
-
- //safe distance rightfoot
- transform6.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
- transform6.setRotation(tf::Quaternion(1, 0, 0, 0));
- broadcaster.sendTransform(tf::StampedTransform(transform6, now, child_rightfoot, child_distancesaferight));
-
- //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.
+ //Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 35 degrees) the publisher says us that the foot is extended.
double angleft = 0.0;
double angright = 0.0;
@@ -126,12 +111,19 @@ void FootExtendedAlgNode::mainNodeThread(void)
// [fill msg structures]
+ // Initialize the topic message structure
+ this->feetextended_String_msg_.data = my_var;
+
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->feetextended_publisher_.publish(this->feetextended_String_msg_);
+
+}
/* [subscriber callbacks] */
/* [service callbacks] */
diff --git a/iri_foot_extended/src/foot_extended_alg_node.cpp~ b/Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp~
similarity index 77%
rename from iri_foot_extended/src/foot_extended_alg_node.cpp~
rename to Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp~
index 8a6b9c675d620d27ab0c05a599ce10b24083518f..597943794d9fd1f17c3c627ccaa28eae4f722a35 100644
--- a/iri_foot_extended/src/foot_extended_alg_node.cpp~
+++ b/Foot_extended/iri_foot_extended/src/foot_extended_alg_node.cpp~
@@ -8,6 +8,7 @@ FootExtendedAlgNode::FootExtendedAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
+ this->feetextended_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("feetextended", 10);
// [init subscribers]
@@ -27,24 +28,19 @@ FootExtendedAlgNode::~FootExtendedAlgNode(void)
void FootExtendedAlgNode::mainNodeThread(void)
{
- 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");
string my_var = "";
try{
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);
+ 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");
for(int i=0; i<100; ++i)
{
@@ -77,19 +73,8 @@ void FootExtendedAlgNode::mainNodeThread(void)
leftknee = sumalknee/100;
rightfoot = sumarfoot/100;
rightknee = sumarknee/100;
-
- //safe distance leftfoot
- transform5.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
- transform5.setRotation(tf::Quaternion(1, 0, 0, 0));
- broadcaster.sendTransform(tf::StampedTransform(transform5, now, child_leftfoot, child_distancesafeleft));
-
- //safe distance rightfoot
- transform6.setOrigin(tf::Vector3(0.1, 0.1, 0.1));
- transform6.setRotation(tf::Quaternion(1, 0, 0, 0));
- broadcaster.sendTransform(tf::StampedTransform(transform6, now, child_rightfoot, child_distancesaferight));
-
- //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.
+ //Apply the function (angle) that measures the angle between the knee and the foot. When pass the limit (angle equals 35 degrees) the publisher says us that the foot is extended.
double angleft = 0.0;
double angright = 0.0;
@@ -126,12 +111,19 @@ void FootExtendedAlgNode::mainNodeThread(void)
// [fill msg structures]
+ // Initialize the topic message structure
+ this->feetextended_String_msg_.data = my_var;
+
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
+ // Uncomment the following line to publish the topic message
+ this->feetextended_publisher_.publish(this->feetextended_String_msg_);
+
+}
/* [subscriber callbacks] */
/* [service callbacks] */
diff --git a/iri_wam_dmp_tracker/CMakeLists.txt b/Foot_extended/iri_wam_dmp_tracker/CMakeLists.txt
similarity index 100%
rename from iri_wam_dmp_tracker/CMakeLists.txt
rename to Foot_extended/iri_wam_dmp_tracker/CMakeLists.txt
diff --git a/iri_wam_dmp_tracker/Makefile b/Foot_extended/iri_wam_dmp_tracker/Makefile
similarity index 100%
rename from iri_wam_dmp_tracker/Makefile
rename to Foot_extended/iri_wam_dmp_tracker/Makefile
diff --git a/iri_wam_dmp_tracker/cfg/WamDmpTracker.cfg b/Foot_extended/iri_wam_dmp_tracker/cfg/WamDmpTracker.cfg
similarity index 100%
rename from iri_wam_dmp_tracker/cfg/WamDmpTracker.cfg
rename to Foot_extended/iri_wam_dmp_tracker/cfg/WamDmpTracker.cfg
diff --git a/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig-usage.dox b/Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig-usage.dox
similarity index 100%
rename from iri_wam_dmp_tracker/docs/WamDmpTrackerConfig-usage.dox
rename to Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig-usage.dox
diff --git a/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.dox b/Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.dox
similarity index 100%
rename from iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.dox
rename to Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.dox
diff --git a/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.wikidoc b/Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.wikidoc
similarity index 100%
rename from iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.wikidoc
rename to Foot_extended/iri_wam_dmp_tracker/docs/WamDmpTrackerConfig.wikidoc
diff --git a/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver.h b/Foot_extended/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver.h
similarity index 100%
rename from iri_wam_dmp_tracker/include/wam_dmp_tracker_driver.h
rename to Foot_extended/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver.h
diff --git a/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver_node.h b/Foot_extended/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver_node.h
similarity index 100%
rename from iri_wam_dmp_tracker/include/wam_dmp_tracker_driver_node.h
rename to Foot_extended/iri_wam_dmp_tracker/include/wam_dmp_tracker_driver_node.h
diff --git a/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch b/Foot_extended/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch
similarity index 100%
rename from iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch
rename to Foot_extended/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch
diff --git a/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch~ b/Foot_extended/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch~
similarity index 100%
rename from iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch~
rename to Foot_extended/iri_wam_dmp_tracker/launch/iri_wam_dmp_tracker.launch~
diff --git a/iri_wam_dmp_tracker/launch/test.launch b/Foot_extended/iri_wam_dmp_tracker/launch/test.launch
similarity index 100%
rename from iri_wam_dmp_tracker/launch/test.launch
rename to Foot_extended/iri_wam_dmp_tracker/launch/test.launch
diff --git a/iri_wam_dmp_tracker/mainpage.dox b/Foot_extended/iri_wam_dmp_tracker/mainpage.dox
similarity index 100%
rename from iri_wam_dmp_tracker/mainpage.dox
rename to Foot_extended/iri_wam_dmp_tracker/mainpage.dox
diff --git a/iri_wam_dmp_tracker/package.xml b/Foot_extended/iri_wam_dmp_tracker/package.xml
similarity index 100%
rename from iri_wam_dmp_tracker/package.xml
rename to Foot_extended/iri_wam_dmp_tracker/package.xml
diff --git a/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver.cpp b/Foot_extended/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver.cpp
similarity index 100%
rename from iri_wam_dmp_tracker/src/wam_dmp_tracker_driver.cpp
rename to Foot_extended/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver.cpp
diff --git a/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver_node.cpp b/Foot_extended/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver_node.cpp
similarity index 100%
rename from iri_wam_dmp_tracker/src/wam_dmp_tracker_driver_node.cpp
rename to Foot_extended/iri_wam_dmp_tracker/src/wam_dmp_tracker_driver_node.cpp
diff --git a/Hand_extended_good_tracking/iri_foot/CMakeLists.txt b/Hand_extended_good_tracking/iri_foot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f73ed60fbf694fa9f9aefa6a942bda8f6ee60ce2
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/CMakeLists.txt
@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_foot)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+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)
+
+# ********************************************************************
+# 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/Foot.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_algorithm geometry_msgs
+# ********************************************************************
+# 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_alg.cpp src/foot_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# 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
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/Hand_extended_good_tracking/iri_foot/CMakeLists.txt~ b/Hand_extended_good_tracking/iri_foot/CMakeLists.txt~
new file mode 100644
index 0000000000000000000000000000000000000000..94c17f7e3adf8339dc6fe4e1ac9c33d9176321d9
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/CMakeLists.txt~
@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_foot)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs tf)
+
+## 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/Foot.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_alg.cpp src/foot_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/Hand_extended_good_tracking/iri_foot/cfg/Foot.cfg b/Hand_extended_good_tracking/iri_foot/cfg/Foot.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..74773d54a2a7ef6c7618c3fce465457dcca43624
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/cfg/Foot.cfg
@@ -0,0 +1,43 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='iri_foot'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+
+exit(gen.generate(PACKAGE, "FootAlgorithm", "Foot"))
diff --git a/Hand_extended_good_tracking/iri_foot/include/foot_alg.h b/Hand_extended_good_tracking/iri_foot/include/foot_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..e40744981fed0def4b1fa4adaf66b3ffa77d24e7
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/include/foot_alg.h
@@ -0,0 +1,134 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _foot_alg_h_
+#define _foot_alg_h_
+
+#include <iri_foot/FootConfig.h>
+#include <tf/transform_listener.h>
+
+//include foot_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class FootAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the FootConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the FootConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef iri_foot::FootConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ FootAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all foot_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~FootAlgorithm(void);
+
+ void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
+};
+
+#endif
diff --git a/Hand_extended_good_tracking/iri_foot/include/foot_alg.h~ b/Hand_extended_good_tracking/iri_foot/include/foot_alg.h~
new file mode 100644
index 0000000000000000000000000000000000000000..e40744981fed0def4b1fa4adaf66b3ffa77d24e7
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/include/foot_alg.h~
@@ -0,0 +1,134 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _foot_alg_h_
+#define _foot_alg_h_
+
+#include <iri_foot/FootConfig.h>
+#include <tf/transform_listener.h>
+
+//include foot_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class FootAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the FootConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the FootConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef iri_foot::FootConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ FootAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all foot_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~FootAlgorithm(void);
+
+ void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
+};
+
+#endif
diff --git a/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h b/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..34e21b986adae3225682a8e868ce2966883041cd
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h
@@ -0,0 +1,161 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _foot_alg_node_h_
+#define _foot_alg_node_h_
+
+#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]
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float64.h>
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+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 righthand;
+ 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]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ FootAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~FootAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+};
+
+#endif
diff --git a/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h~ b/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h~
new file mode 100644
index 0000000000000000000000000000000000000000..34e21b986adae3225682a8e868ce2966883041cd
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h~
@@ -0,0 +1,161 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _foot_alg_node_h_
+#define _foot_alg_node_h_
+
+#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]
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float64.h>
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+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 righthand;
+ 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]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ FootAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~FootAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+};
+
+#endif
diff --git a/Hand_extended_good_tracking/iri_foot/launch/foot.launch b/Hand_extended_good_tracking/iri_foot/launch/foot.launch
new file mode 100644
index 0000000000000000000000000000000000000000..acceaed5669941de2b2a4cd30eece799a5327f6f
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/launch/foot.launch
@@ -0,0 +1,4 @@
+<launch>
+ <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>
diff --git a/Hand_extended_good_tracking/iri_foot/launch/foot.launch~ b/Hand_extended_good_tracking/iri_foot/launch/foot.launch~
new file mode 100644
index 0000000000000000000000000000000000000000..a90d2ff87729d12bff6cf99b9a91c4808acb32fb
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/launch/foot.launch~
@@ -0,0 +1,4 @@
+<launch>
+ <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>
diff --git a/Hand_extended_good_tracking/iri_foot/package.xml b/Hand_extended_good_tracking/iri_foot/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b5a0677bfc72eb98794ed8a5e3a15578eee8e8d5
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/package.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+ <name>iri_foot</name>
+ <version>0.0.0</version>
+ <description>The iri_foot package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="vsilos@todo.todo">vsilos</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/iri_foot</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <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 -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/Hand_extended_good_tracking/iri_foot/package.xml~ b/Hand_extended_good_tracking/iri_foot/package.xml~
new file mode 100644
index 0000000000000000000000000000000000000000..9f2dc3b5a3295b1a41adb6b5c498346d10f4b312
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/package.xml~
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<package>
+ <name>iri_foot</name>
+ <version>0.0.0</version>
+ <description>The iri_foot package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="vsilos@todo.todo">vsilos</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/iri_foot</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp b/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a447d665e89bc105eaacacae9175ef160f1d6aa1
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp
@@ -0,0 +1,36 @@
+#include "foot_alg.h"
+#include <math.h>
+using namespace std;
+
+FootAlgorithm::FootAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+FootAlgorithm::~FootAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void FootAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// 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));
+}
+
diff --git a/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp~ b/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp~
new file mode 100644
index 0000000000000000000000000000000000000000..233f1975d25d9998472017bb70b6c37ffb0b9e24
--- /dev/null
+++ b/Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp~
@@ -0,0 +1,36 @@
+#include "foot_alg.h"
+#include <math.h>
+using namespace std;
+
+FootAlgorithm::FootAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+FootAlgorithm::~FootAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void FootAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// 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));
+}
+
diff --git a/iri_foot/src/foot_alg_node.cpp~ b/Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp
similarity index 94%
rename from iri_foot/src/foot_alg_node.cpp~
rename to Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp
index d6344559b3e34cff77f5f04a399039e844e9b2ef..b26683ce28c6f38a1599bddf6c1d65a4fdc7fa74 100644
--- a/iri_foot/src/foot_alg_node.cpp~
+++ b/Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp
@@ -1,4 +1,5 @@
#include "foot_alg_node.h"
+//#include "foot_alg.cpp"
using namespace std;
#include <string>
@@ -11,6 +12,7 @@ FootAlgNode::FootAlgNode(void) :
// [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]
@@ -82,7 +84,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_);
}
diff --git a/iri_foot/src/foot_alg_node.cpp b/Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp~
similarity index 92%
rename from iri_foot/src/foot_alg_node.cpp
rename to Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp~
index 687174664466e44f66fa53bf5b04c30b6fbac5cb..fb62173e8ff1969644ae51c4bc70f518f26e69f0 100644
--- a/iri_foot/src/foot_alg_node.cpp
+++ b/Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp~
@@ -1,4 +1,5 @@
#include "foot_alg_node.h"
+//#include "foot_alg.cpp"
using namespace std;
#include <string>
@@ -11,6 +12,7 @@ FootAlgNode::FootAlgNode(void) :
// [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]
@@ -28,6 +30,8 @@ FootAlgNode::~FootAlgNode(void)
// [free dynamic memory]
}
+
+
void FootAlgNode::mainNodeThread(void)
{
string child_righthand("/brix_2/user_1/right_hand");
@@ -63,7 +67,7 @@ void FootAlgNode::mainNodeThread(void)
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";
+ this->pose_surface_PoseStamped_msg_.header.frame_id = child_robot;
// Initialize the topic message structure
@@ -80,7 +84,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_);
}
diff --git a/iri_foot_extended/launch/foot_extended.launch b/iri_foot_extended/launch/foot_extended.launch
deleted file mode 100644
index 784b05e3963fb5e409a8ac28cb6cf22218eef3ea..0000000000000000000000000000000000000000
--- a/iri_foot_extended/launch/foot_extended.launch
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
- <node pkg="iri_foot_extended" type ="iri_foot_extended" name="iri_foot_extended"/>
-</launch>