From 15385d9ae99ff9a1ef268762e4de04d22446b724 Mon Sep 17 00:00:00 2001
From: vsilos <vsilos@iri.upc.edu>
Date: Fri, 28 Apr 2017 13:26:19 +0200
Subject: [PATCH] Until 28 April
---
.../iri_foot}/CMakeLists.txt | 0
.../iri_foot}/CMakeLists.txt~ | 0
.../iri_foot}/cfg/Foot.cfg | 0
.../iri_foot}/include/foot_alg.h | 0
.../iri_foot}/include/foot_alg.h~ | 0
.../iri_foot}/include/foot_alg_node.h | 10 +-
.../iri_foot}/include/foot_alg_node.h~ | 10 +-
.../iri_foot}/launch/foot.launch | 0
.../iri_foot}/launch/foot.launch~ | 0
.../iri_foot}/package.xml | 0
.../iri_foot}/package.xml~ | 0
.../iri_foot}/src/foot_alg.cpp | 0
.../iri_foot}/src/foot_alg.cpp~ | 0
Foot_extended/iri_foot/src/foot_alg_node.cpp | 170 ++++++++++++++++++
Foot_extended/iri_foot/src/foot_alg_node.cpp~ | 170 ++++++++++++++++++
.../iri_foot_extended/CMakeLists.txt | 101 +++++++++++
.../iri_foot_extended/CMakeLists.txt~ | 5 +-
.../iri_foot_extended}/cfg/FootExtended.cfg | 0
.../include/foot_extended_alg.h | 0
.../include/foot_extended_alg.h~ | 0
.../include/foot_extended_alg_node.h | 9 +-
.../include/foot_extended_alg_node.h~ | 7 +-
.../iri_foot_extended}/launch/foot.launch~ | 0
.../launch/foot_extended.launch | 4 +
.../launch/foot_extended.launch~ | 4 +
Foot_extended/iri_foot_extended/package.xml | 55 ++++++
.../iri_foot_extended/package.xml~ | 2 +
.../src/foot_extended_alg.cpp | 2 +-
.../src/foot_extended_alg.cpp~ | 0
.../src/foot_extended_alg_node.cpp | 36 ++--
.../src/foot_extended_alg_node.cpp~ | 36 ++--
.../iri_wam_dmp_tracker}/CMakeLists.txt | 0
.../iri_wam_dmp_tracker}/Makefile | 0
.../cfg/WamDmpTracker.cfg | 0
.../docs/WamDmpTrackerConfig-usage.dox | 0
.../docs/WamDmpTrackerConfig.dox | 0
.../docs/WamDmpTrackerConfig.wikidoc | 0
.../include/wam_dmp_tracker_driver.h | 0
.../include/wam_dmp_tracker_driver_node.h | 0
.../launch/iri_wam_dmp_tracker.launch | 0
.../launch/iri_wam_dmp_tracker.launch~ | 0
.../iri_wam_dmp_tracker}/launch/test.launch | 0
.../iri_wam_dmp_tracker}/mainpage.dox | 0
.../iri_wam_dmp_tracker}/package.xml | 0
.../src/wam_dmp_tracker_driver.cpp | 0
.../src/wam_dmp_tracker_driver_node.cpp | 0
.../iri_foot/CMakeLists.txt | 102 +++++++++++
.../iri_foot/CMakeLists.txt~ | 100 +++++++++++
.../iri_foot/cfg/Foot.cfg | 43 +++++
.../iri_foot/include/foot_alg.h | 134 ++++++++++++++
.../iri_foot/include/foot_alg.h~ | 134 ++++++++++++++
.../iri_foot/include/foot_alg_node.h | 161 +++++++++++++++++
.../iri_foot/include/foot_alg_node.h~ | 161 +++++++++++++++++
.../iri_foot/launch/foot.launch | 4 +
.../iri_foot/launch/foot.launch~ | 4 +
.../iri_foot/package.xml | 56 ++++++
.../iri_foot/package.xml~ | 54 ++++++
.../iri_foot/src/foot_alg.cpp | 36 ++++
.../iri_foot/src/foot_alg.cpp~ | 36 ++++
.../iri_foot/src/foot_alg_node.cpp | 4 +-
.../iri_foot/src/foot_alg_node.cpp~ | 8 +-
iri_foot_extended/launch/foot_extended.launch | 3 -
62 files changed, 1596 insertions(+), 65 deletions(-)
rename {iri_foot => Foot_extended/iri_foot}/CMakeLists.txt (100%)
rename {iri_foot => Foot_extended/iri_foot}/CMakeLists.txt~ (100%)
rename {iri_foot => Foot_extended/iri_foot}/cfg/Foot.cfg (100%)
rename {iri_foot => Foot_extended/iri_foot}/include/foot_alg.h (100%)
rename {iri_foot => Foot_extended/iri_foot}/include/foot_alg.h~ (100%)
rename {iri_foot => Foot_extended/iri_foot}/include/foot_alg_node.h (92%)
rename {iri_foot => Foot_extended/iri_foot}/include/foot_alg_node.h~ (92%)
rename {iri_foot => Foot_extended/iri_foot}/launch/foot.launch (100%)
rename {iri_foot => Foot_extended/iri_foot}/launch/foot.launch~ (100%)
rename {iri_foot => Foot_extended/iri_foot}/package.xml (100%)
rename {iri_foot => Foot_extended/iri_foot}/package.xml~ (100%)
rename {iri_foot => Foot_extended/iri_foot}/src/foot_alg.cpp (100%)
rename {iri_foot => Foot_extended/iri_foot}/src/foot_alg.cpp~ (100%)
create mode 100644 Foot_extended/iri_foot/src/foot_alg_node.cpp
create mode 100644 Foot_extended/iri_foot/src/foot_alg_node.cpp~
create mode 100644 Foot_extended/iri_foot_extended/CMakeLists.txt
rename iri_foot_extended/CMakeLists.txt => Foot_extended/iri_foot_extended/CMakeLists.txt~ (95%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/cfg/FootExtended.cfg (100%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/include/foot_extended_alg.h (100%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/include/foot_extended_alg.h~ (100%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/include/foot_extended_alg_node.h (95%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/include/foot_extended_alg_node.h~ (97%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/launch/foot.launch~ (100%)
create mode 100644 Foot_extended/iri_foot_extended/launch/foot_extended.launch
create mode 100644 Foot_extended/iri_foot_extended/launch/foot_extended.launch~
create mode 100644 Foot_extended/iri_foot_extended/package.xml
rename iri_foot_extended/package.xml => Foot_extended/iri_foot_extended/package.xml~ (96%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/src/foot_extended_alg.cpp (86%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/src/foot_extended_alg.cpp~ (100%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/src/foot_extended_alg_node.cpp (77%)
rename {iri_foot_extended => Foot_extended/iri_foot_extended}/src/foot_extended_alg_node.cpp~ (77%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/CMakeLists.txt (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/Makefile (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/cfg/WamDmpTracker.cfg (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/docs/WamDmpTrackerConfig-usage.dox (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/docs/WamDmpTrackerConfig.dox (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/docs/WamDmpTrackerConfig.wikidoc (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/include/wam_dmp_tracker_driver.h (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/include/wam_dmp_tracker_driver_node.h (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/launch/iri_wam_dmp_tracker.launch (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/launch/iri_wam_dmp_tracker.launch~ (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/launch/test.launch (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/mainpage.dox (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/package.xml (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/src/wam_dmp_tracker_driver.cpp (100%)
rename {iri_wam_dmp_tracker => Foot_extended/iri_wam_dmp_tracker}/src/wam_dmp_tracker_driver_node.cpp (100%)
create mode 100644 Hand_extended_good_tracking/iri_foot/CMakeLists.txt
create mode 100644 Hand_extended_good_tracking/iri_foot/CMakeLists.txt~
create mode 100644 Hand_extended_good_tracking/iri_foot/cfg/Foot.cfg
create mode 100644 Hand_extended_good_tracking/iri_foot/include/foot_alg.h
create mode 100644 Hand_extended_good_tracking/iri_foot/include/foot_alg.h~
create mode 100644 Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h
create mode 100644 Hand_extended_good_tracking/iri_foot/include/foot_alg_node.h~
create mode 100644 Hand_extended_good_tracking/iri_foot/launch/foot.launch
create mode 100644 Hand_extended_good_tracking/iri_foot/launch/foot.launch~
create mode 100644 Hand_extended_good_tracking/iri_foot/package.xml
create mode 100644 Hand_extended_good_tracking/iri_foot/package.xml~
create mode 100644 Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp
create mode 100644 Hand_extended_good_tracking/iri_foot/src/foot_alg.cpp~
rename iri_foot/src/foot_alg_node.cpp~ => Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp (94%)
rename iri_foot/src/foot_alg_node.cpp => Hand_extended_good_tracking/iri_foot/src/foot_alg_node.cpp~ (92%)
delete mode 100644 iri_foot_extended/launch/foot_extended.launch
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 799ac87..c7fe249 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 799ac87..c7fe249 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 0000000..3861b4e
--- /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 0000000..3861b4e
--- /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 0000000..99775c9
--- /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 6597219..528d54a 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 250b78c..3e73cef 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 250b78c..86a0242 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 0000000..bc74684
--- /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 0000000..bc74684
--- /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 0000000..e4fab07
--- /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 e8446db..b169fd8 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 b640d9c..ab95e23 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 8a6b9c6..5979437 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 8a6b9c6..5979437 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 0000000..f73ed60
--- /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 0000000..94c17f7
--- /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 0000000..74773d5
--- /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 0000000..e407449
--- /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 0000000..e407449
--- /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 0000000..34e21b9
--- /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 0000000..34e21b9
--- /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 0000000..acceaed
--- /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 0000000..a90d2ff
--- /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 0000000..b5a0677
--- /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 0000000..9f2dc3b
--- /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 0000000..a447d66
--- /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 0000000..233f197
--- /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 d634455..b26683c 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 6871746..fb62173 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 784b05e..0000000
--- 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>
--
GitLab