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

Until 28 April

parent 36bc2f0f
...@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm> ...@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{ {
private: private:
tf::TransformListener listener; tf::TransformListener listener;
tf::StampedTransform transformrobleftdist; tf::StampedTransform transformrobrightfoot;
tf::StampedTransform transformrobrightdist; tf::StampedTransform transformrobleftfoot;
tf::Vector3 goalleft; tf::Vector3 goalleft;
tf::Vector3 goalright; tf::Vector3 goalright;
...@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm> ...@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_; geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
// [subscriber attributes] // [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] // [service attributes]
......
...@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm> ...@@ -48,8 +48,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
{ {
private: private:
tf::TransformListener listener; tf::TransformListener listener;
tf::StampedTransform transformrobleftdist; tf::StampedTransform transformrobrightfoot;
tf::StampedTransform transformrobrightdist; tf::StampedTransform transformrobleftfoot;
tf::Vector3 goalleft; tf::Vector3 goalleft;
tf::Vector3 goalright; tf::Vector3 goalright;
...@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm> ...@@ -58,6 +58,12 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_; geometry_msgs::PoseStamped pose_surface_PoseStamped_msg_;
// [subscriber attributes] // [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] // [service attributes]
......
#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");
}
#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");
}
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})
...@@ -6,7 +6,7 @@ find_package(catkin REQUIRED) ...@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ******************************************************************** # ********************************************************************
# Add catkin additional components here # 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 ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
...@@ -60,7 +60,7 @@ catkin_package( ...@@ -60,7 +60,7 @@ catkin_package(
# ******************************************************************** # ********************************************************************
# Add ROS and IRI ROS run time dependencies # 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 # Add system and labrobotica run time dependencies here
# ******************************************************************** # ********************************************************************
...@@ -94,6 +94,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ...@@ -94,6 +94,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Add message headers dependencies # 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 # Add dynamic reconfigure dependencies
# ******************************************************************** # ********************************************************************
......
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