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

25 April

parent 1c0b333e
......@@ -55,7 +55,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::StampedTransform transformrobleftdist;
tf::StampedTransform transformrobrightdist;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -66,7 +67,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goal;
tf::Vector3 goalleft;
tf::Vector3 goalright;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
......
......@@ -55,7 +55,8 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::StampedTransform transform4;
tf::StampedTransform transform5;
tf::StampedTransform transform6;
tf::StampedTransform transformrobdistsafe;
tf::StampedTransform transformrobleftdist;
tf::StampedTransform transformrobrightdist;
tf::Vector3 rightfoot;
tf::Vector3 leftfoot;
tf::Vector3 leftknee;
......@@ -66,7 +67,9 @@ class FootAlgNode : public algorithm_base::IriBaseAlgorithm<FootAlgorithm>
tf::Vector3 sumarknee;
tf::Vector3 distancesafe;
tf::Vector3 robot;
tf::Vector3 goal;
tf::Vector3 goalleft;
tf::Vector3 goalright;
//message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
//tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
......
......@@ -87,28 +87,36 @@ void FootAlgNode::mainNodeThread(void)
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, zero, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, zero, child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the left distance safe
listener.waitForTransform(child_robot, child_distancesafeleft, zero, three_seconds);
//listener.lookupTransform(child_robot, child_distancesafeleft, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, leftfoot, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftdist);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
goal = transformrobdistsafe.getOrigin();
double x = goal.x() + 0.2;
double y = goal.y() + 0.2;
double z = goal.z() + 0.2;
goal.setValue(x,y,z);
goalleft = transformrobleftdist.getOrigin();
double xleft = goalleft.x() + 0.6;
double yleft = goalleft.y() + 0.6;
double zleft = goalleft.z() + 0.6;
goalleft.setValue(xleft,yleft,zleft);
//obtain the transformation between the robot and the right distance safe
listener.waitForTransform(child_robot, child_distancesaferight, zero, three_seconds);
listener.lookupTransform(child_robot, child_rightfoot, zero, transformrobrightdist);
goalright = transformrobrightdist.getOrigin();
double xright = goalright.x() + 0.6;
double yright = goalright.y() + 0.6;
double zright = goalright.z() + 0.6;
goalright.setValue(xright,yright,zright);
//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.
double angleft = 0.0;
......@@ -146,11 +154,14 @@ void FootAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg_.pose.position.x = goal.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goal.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goal.getZ();
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.position.x = goalright.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalright.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalright.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = leftfoot;
this->pose_surface_PoseStamped_msg_.header.frame_id = child_leftfoot;
// Initialize the topic message structure
......
......@@ -87,28 +87,36 @@ void FootAlgNode::mainNodeThread(void)
//safe distance leftfoot
transform5.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform5.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
transform5.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform5, zero, child_leftfoot, child_distancesafeleft));
//safe distance rightfoot
transform6.setOrigin(tf::Vector3(0.2, 0.2, 0.2));
transform6.setOrigin(tf::Vector3(0.6, 0.6, 0.6));
transform6.setRotation(tf::Quaternion(0, 0, 0, 1));
broadcaster.sendTransform(tf::StampedTransform(transform6, zero, child_rightfoot, child_distancesaferight));
//obtain the transformation between the robot and the left distance safe
listener.waitForTransform(child_robot, child_distancesafeleft, zero, three_seconds);
//listener.lookupTransform(child_robot, child_distancesafeleft, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, leftfoot, zero,transformrobdistsafe);
listener.lookupTransform(child_robot, child_leftfoot, zero,transformrobleftdist);
//tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
goal = transformrobdistsafe.getOrigin();
double x = goal.x() + 0.2;
double y = goal.y() + 0.2;
double z = goal.z() + 0.2;
goal.setValue(x,y,z);
goalleft = transformrobleftdist.getOrigin();
double xleft = goalleft.x() + 0.6;
double yleft = goalleft.y() + 0.6;
double zleft = goalleft.z() + 0.6;
goalleft.setValue(xleft,yleft,zleft);
//obtain the transformation between the robot and the right distance safe
listener.waitForTransform(child_robot, child_distancesaferight, zero, three_seconds);
listener.lookupTransform(child_robot, child_rightfoot, zero, transformrobrightdist);
goalright = transformrobrightdist.getOrigin();
double xright = goalright.x() + 0.6;
double yright = goalright.y() + 0.6;
double zright = goalright.z() + 0.6;
goalright.setValue(xright,yright,zright);
//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.
double angleft = 0.0;
......@@ -146,11 +154,14 @@ void FootAlgNode::mainNodeThread(void)
// [fill msg structures]
// Initialize the topic message structure
this->pose_surface_PoseStamped_msg_.pose.position.x = goal.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goal.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goal.getZ();
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.position.x = goalright.getX();
this->pose_surface_PoseStamped_msg_.pose.position.y = goalright.getY();
this->pose_surface_PoseStamped_msg_.pose.position.z = goalright.getZ();
this->pose_surface_PoseStamped_msg_.header.stamp = ros::Time::now();
this->pose_surface_PoseStamped_msg_.header.frame_id = leftfoot;
this->pose_surface_PoseStamped_msg_.header.frame_id = child_leftfoot;
// Initialize the topic message structure
......
# ********************************************************************
# 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
#! /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_wam_dmp_tracker'
from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
exit(gen.generate(PACKAGE, "WamDmpTrackerDriver", "WamDmpTracker"))
\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);
// here define all wam_dmp_tracker_driver interface methods to retrieve and set
// the driver parameters
/**
* \brief Destructor
*
* This destructor is called when the object is about to be destroyed.
*
*/
~WamDmpTrackerDriver(void);
};
#endif
// 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_node_h_
#define _wam_dmp_tracker_driver_node_h_
#include <iri_base_driver/iri_base_driver_node.h>
#include "wam_dmp_tracker_driver.h"
// [publisher subscriber headers]
#include <geometry_msgs/PoseStamped.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
// [service client headers]
#include <iri_common_drivers_msgs/QueryInverseKinematics.h>
// [action server client headers]
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <iri_wam_common_msgs/DMPTrackerAction.h>
/**
* \brief IRI ROS Specific Driver Class
*
* This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>,
* to provide an execution thread to the driver object. A complete framework
* with utilites to test the node functionallity or to add diagnostics to
* specific situations is also given. The inherit template design form allows
* complete access to any IriBaseDriver object implementation.
*
* As mentioned, tests in the different driver states can be performed through
* class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
* common to all nodes may be also executed in the pattern class IriBaseNodeDriver.
* Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
* more details:
* http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
* http://www.ros.org/wiki/self_test/ (Example: Self Test)
*/
class WamDmpTrackerDriverNode : public iri_base_driver::IriBaseNodeDriver<WamDmpTrackerDriver>
{
private:
// [publisher attributes]
ros::Publisher DMPTrackerNewGoal_publisher_;
trajectory_msgs::JointTrajectoryPoint JointTrajectoryPoint_msg_;
// [subscriber attributes]
ros::Subscriber pose_surface_subscriber_;
void pose_surface_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//CMutex pose_surface_mutex_;
// [service attributes]
// [client attributes]
ros::ServiceClient wamik_client_;
iri_common_drivers_msgs::QueryInverseKinematics wamik_srv_;
// [action server attributes]
// [action client attributes]
actionlib::SimpleActionClient<iri_wam_common_msgs::DMPTrackerAction> DMPTracker_client_;
iri_wam_common_msgs::DMPTrackerGoal DMPTracker_goal_;
void DMPTrackerMakeActionRequest();
void DMPTrackerDone(const actionlib::SimpleClientGoalState& state, const iri_wam_common_msgs::DMPTrackerResultConstPtr& result);
void DMPTrackerActive();
void DMPTrackerFeedback(const iri_wam_common_msgs::DMPTrackerFeedbackConstPtr& feedback);
/**
* \brief post open hook
*
* This function is called by IriBaseNodeDriver::postOpenHook(). In this function
* specific parameters from the driver must be added so the ROS dynamic
* reconfigure application can update them.
*/
void postNodeOpenHook(void);
public:
/**
* \brief constructor
*