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

5 May 2017

parent 15385d9a
<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>
<?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>
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)
## 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})
// 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_extended_alg_h_
#define _foot_extended_alg_h_
#include <iri_foot_extended/FootExtendedConfig.h>
#include <tf/transform_listener.h>
//include foot_extended_alg main library
/**
* \brief IRI ROS Specific Driver Class
*
*
*/
class FootExtendedAlgorithm
{
protected:
/**
* \brief define config type
*
* Define a Config type with the FootExtendedConfig. 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 FootExtendedConfig. All driver implementations
* will then use the same variable type Config.
*/
typedef iri_foot_extended::FootExtendedConfig 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.
*/
FootExtendedAlgorithm(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_extended_alg interface methods to retrieve and set
// the driver parameters
/**
* \brief Destructor
*
* This destructor is called when the object is about to be destroyed.
*
*/
~FootExtendedAlgorithm(void);
void angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4, double& angleft, double& angright);
};
#endif
<launch>
<node pkg="iri_foot_extended" type ="iri_foot" name="iri_foot_extended"/>
</launch>
#include "foot_extended_alg.h"
FootExtendedAlgorithm::FootExtendedAlgorithm(void)
{
pthread_mutex_init(&this->access_,NULL);
}
FootExtendedAlgorithm::~FootExtendedAlgorithm(void)
{
pthread_mutex_destroy(&this->access_);
}
void FootExtendedAlgorithm::config_update(Config& config, uint32_t level)
{
this->lock();
// save the current configuration
this->config_=config;
this->unlock();
}
// FootExtendedAlgorithm 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));
}
# ********************************************************************
# The new CMakeLists.txt file starts here
# ********************************************************************
cmake_minimum_required(VERSION 2.8.3)
project(iri_wam_dmp_tracker)
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS actionlib geometry_msgs trajectory_msgs iri_base_driver iri_common_drivers_msgs iri_wam_common_msgs)
## 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/WamDmpTracker.cfg)
# ********************************************************************
# Add run time dependencies here
# ********************************************************************
catkin_package(
# INCLUDE_DIRS
# LIBRARIES
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS
actionlib geometry_msgs trajectory_msgs iri_base_driver iri_common_drivers_msgs iri_wam_common_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
DEPENDS iriutils
)
###########
## 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} <list of source files>)
add_executable(${PROJECT_NAME} src/wam_dmp_tracker_driver.cpp src/wam_dmp_tracker_driver_node.cpp)
# ********************************************************************
# Add the libraries
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
# ********************************************************************
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
\subsubsection usage Usage
\verbatim
<node name="WamDmpTrackerDriver" pkg="iri_wam_dmp_tracker" type="WamDmpTrackerDriver">
</node>
\endverbatim
\subsubsection parameters ROS parameters
Reads and maintains the following parameters on the ROS server
# Autogenerated param section. Do not hand edit.
param {
group.0 {
name=Dynamically Reconfigurable Parameters
desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters.
}
}
# End of autogenerated section. You may edit below.
// 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 _wam_dmp_tracker_driver_h_
#define _wam_dmp_tracker_driver_h_
#include <iri_base_driver/iri_base_driver.h>
#include <iri_wam_dmp_tracker/WamDmpTrackerConfig.h>
//include wam_dmp_tracker_driver main library
/**
* \brief IRI ROS Specific Driver Class
*
* This class inherits from the IRI Base class IriBaseDriver, which provides the
* guidelines to implement any specific driver. The IriBaseDriver class offers an
* easy framework to integrate functional drivers implemented in C++ with the
* ROS driver structure. ROS driver_base state transitions are already managed
* by IriBaseDriver.
*
* The WamDmpTrackerDriver class must implement all specific driver requirements to
* safetely open, close, run and stop the driver at any time. It also must
* guarantee an accessible interface for all driver's parameters.
*
* The WamDmpTrackerConfig.cfg needs to be filled up with those parameters suitable
* to be changed dynamically by the ROS dyanmic reconfigure application. The
* implementation of the CIriNode class will manage those parameters through
* methods like postNodeOpenHook() and reconfigureNodeHook().
*
*/
class WamDmpTrackerDriver : public iri_base_driver::IriBaseDriver
{
private:
// private attributes and methods
public:
/**
* \brief define config type
*
* Define a Config type with the WamDmpTrackerConfig. All driver implementations
* will then use the same variable type Config.
*/
typedef iri_wam_dmp_tracker::WamDmpTrackerConfig 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.
*/
WamDmpTrackerDriver(void);
/**
* \brief open driver
*
* In this function, the driver must be openned. Openning errors must be
* taken into account. This function is automatically called by
* IriBaseDriver::doOpen(), an state transition is performed if return value
* equals true.
*
* \return bool successful
*/
bool openDriver(void);
/**
* \brief close driver
*
* In this function, the driver must be closed. Variables related to the
* driver state must also be taken into account. This function is automatically
* called by IriBaseDriver::doClose(), an state transition is performed if
* return value equals true.
*
* \return bool successful
*/
bool closeDriver(void);
/**
* \brief start driver
*
* After this function, the driver and its thread will be started. The driver
* and related variables should be properly setup. This function is
* automatically called by IriBaseDriver::doStart(), an state transition is
* performed if return value equals true.
*
* \return bool successful
*/
bool startDriver(void);
/**
* \brief stop driver
*
* After this function, the driver's thread will stop its execution. The driver
* and related variables should be properly setup. This function is
* automatically called by IriBaseDriver::doStop(), an state transition is
* performed if return value equals true.
*
* \return bool successful
*/
bool stopDriver(void);
/**
* \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& new_cfg, uint32_t level=0);