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

I add the publisher and I'm doing the function

parent 408d6cfc
int main ()
{
}
......@@ -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 tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -93,7 +93,8 @@ 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} <msg_package_name>_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
......
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})
......@@ -26,6 +26,7 @@
#define _foot_alg_h_
#include <iri_foot/FootConfig.h>
#include <tf/transform_listener.h>
//include foot_alg main library
......@@ -126,7 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
double angle(tf::Vector p1, tf::Vector p2, tf::Vector p3, tf::Vector p4);
double angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4);
};
#endif
......@@ -26,6 +26,7 @@
#define _foot_alg_h_
#include <iri_foot/FootConfig.h>
#include <tf/transform_listener.h>
//include foot_alg main library
......@@ -126,7 +127,7 @@ class FootAlgorithm
*
*/
~FootAlgorithm(void);
double angle(tf::Vector );
double angle(tf::Vector p1, tf::Vector p2, tf::Vector p3, tf::Vector p4);
};
#endif
......@@ -31,6 +31,7 @@
// [publisher subscriber headers]
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
// [service client headers]
......@@ -54,6 +55,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 rightknee;
// [publisher attributes]
ros::Publisher myfeet_publisher_;
std_msgs::String myfeet_String_msg_;
// [subscriber attributes]
......
......@@ -44,7 +44,10 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{
private:
tf::TransformListener listener;
tf::StampedTransform transform1;
tf::StampedTransform transform2;
tf::StampedTransform transform3;
tf::StampedTransform transform4;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......
......@@ -25,14 +25,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
{
const double PI = 3.14159265359.
double numleft = (p1.getX() * p2.getX()) + (p1.getY() * p2.getY()) + (p1.getZ() * p2.getZ());
double numright = (p3.getX() * p4.getX()) + (p3.getY() * p4.getY()) + (p3.getZ() * p4.getZ());
double denleft = sqrt((p1.getX() * p1.getX()) + (p1.getY() * p1.getY()) + (p1.getZ() * p1.getZ())) * sqrt((p2.getX() * p2.getX()) + (p2.getY() * p2.getY()) + (p2.getZ() * p2.getZ()));
double denright = sqrt((p3.getX() * p3.getX()) + (p3.getY() * p3.getY()) + (p3.getZ() * p3.getZ())) * sqrt((p4.getX() * p4.getX()) + (p4.getY() * p4.getY()) + (p4.getZ() * p4.getZ()));
double angleft = acos (numleft/denleft);
double angright = acos (numright/denright);
double numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
double angleft = atan (numleft/denleft);
double angright = atan (numright/denright);
return angleft;
return angright;
}
......@@ -25,13 +25,13 @@ void FootAlgorithm::config_update(Config& config, uint32_t level)
double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
{
const double PI = 3.14159265359.
double numleft = (p1.getX() * p2.getX()) + (p1.getY() * p2.getY()) + (p1.getZ() * p2.getZ());
double numright = (p3.getX() * p4.getX()) + (p3.getY() * p4.getY()) + (p3.getZ() * p4.getZ());
double denleft = sqrt((p1.getX() * p1.getX()) + (p1.getY() * p1.getY()) + (p1.getZ() * p1.getZ())) * sqrt((p2.getX() * p2.getX()) + (p2.getY() * p2.getY()) + (p2.getZ() * p2.getZ()));
double denright = sqrt((p3.getX() * p3.getX()) + (p3.getY() * p3.getY()) + (p3.getZ() * p3.getZ())) * sqrt((p4.getX() * p4.getX()) + (p4.getY() * p4.getY()) + (p4.getZ() * p4.getZ()));
double angleft = acos (numleft/denleft);
double angright = acos (numright/denright);
double numleft = p2.getZ() - p1.getZ();
double numright = p4.getZ() - p3.getZ();
double denleft = p2.getY() - p1.getY();
double denright = p4.getY() - p3.getY();
double angleft = atan (numleft/denleft);
double angright = atan (numright/denright);
return angleft;
return angright;
}
#include "foot_alg_node.h"
using namespace std;
#include <string>
FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
......@@ -7,6 +9,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -26,13 +29,60 @@ FootAlgNode::~FootAlgNode(void)
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time::now();
ros::Duration five_seconds = ros::Duration(5.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");
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = trasnform2.getOrigin();
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = trasnform3.getOrigin();
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
}
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_);
}
/* [subscriber callbacks] */
......
#include "foot_alg_node.h"
using namespace std;
#include <string>
FootAlgNode::FootAlgNode(void) :
algorithm_base::IriBaseAlgorithm<FootAlgorithm>()
......@@ -7,6 +9,7 @@ FootAlgNode::FootAlgNode(void) :
//this->loop_rate_ = 2;//in [Hz]
// [init publishers]
this->myfeet_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("myfeet", 10);
// [init subscribers]
......@@ -26,13 +29,60 @@ FootAlgNode::~FootAlgNode(void)
void FootAlgNode::mainNodeThread(void)
{
string my_var = "";
try{
ros::Time now = ros::Time::now();
ros::Duration five_seconds = ros::Duration(5.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");
//left_foot
listener.waitForTransform(parent_cameraframe, child_leftfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftfoot, now, transform1);
leftfoot = transform1.getOrigin();
//left_knee
listener.waitForTransform(parent_cameraframe, child_leftknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_leftknee, now, transform2);
leftknee = trasnform2.getOrigin();
//right_foot
listener.waitForTransform(parent_cameraframe, child_rightfoot, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightfoot, now, transform3);
rightfoot = trasnform3.getOrigin();
//right_knee
listener.waitForTransform(parent_cameraframe, child_rightknee, now, five_seconds);
listener.lookupTransform(parent_cameraframe, child_rightknee, now, transform4);
rightknee = trasnform4.getOrigin();
}
// [fill msg structures]
// Initialize the topic message structure
this->myfeet_String_msg_.data = my_var;
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
// Uncomment the following line to publish the topic message
this->myfeet_publisher_.publish(this->myfeet_String_msg_);
}
/* [subscriber callbacks] */
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment