diff --git a/joints_cart_client/CMakeLists.txt b/joints_cart_client/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..a0b4e096fb36b01ddff9c0170a56e68cc544658c --- /dev/null +++ b/joints_cart_client/CMakeLists.txt @@ -0,0 +1,103 @@ +cmake_minimum_required(VERSION 2.8.3) +project(joints_cart_client) + +## Find catkin macros and libraries +find_package(catkin REQUIRED) +# ******************************************************************** +# Add catkin additional components here +# ******************************************************************** +find_package(catkin REQUIRED COMPONENTS iri_base_algorithm humanoid_modules geometry_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) +find_package(iriutils 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/JointsCartClient.cfg) + +# ******************************************************************** +# Add run time dependencies here +# ******************************************************************** +catkin_package( +# INCLUDE_DIRS +# LIBRARIES +# ******************************************************************** +# Add ROS and IRI ROS run time dependencies +# ******************************************************************** + CATKIN_DEPENDS iri_base_algorithm humanoid_modules 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(${iriutils_INCLUDE_DIR}) +# 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/joints_cart_client_alg.cpp src/joints_cart_client_alg_node.cpp) + +# ******************************************************************** +# Add the libraries +# ******************************************************************** +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY}) +# target_link_libraries(${PROJECT_NAME} ${<dependency>_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}) diff --git a/joints_cart_client/cfg/JointsCartClient.cfg b/joints_cart_client/cfg/JointsCartClient.cfg new file mode 100755 index 0000000000000000000000000000000000000000..5f92586c3c793e9c8ef9fe8f8672676b764988ae --- /dev/null +++ b/joints_cart_client/cfg/JointsCartClient.cfg @@ -0,0 +1,57 @@ +#! /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='joints_cart_client' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +enum_arm = gen.enum([ +gen.const("Left", int_t, 0, "Left arm"), +gen.const("Right", int_t, 1, "Right arm"), +], "Possible arm identifiers.") + +# 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) +gen.add("arm_id", int_t, 0, "Arm identifier", 0, 0, 1, edit_method=enum_arm) +gen.add("frame_id", str_t, 0, "Reference frame used to define the arm position","") +gen.add("x_pos", double_t, 0, "Arm end effector x position in m",0, -0.5, 0.5) +gen.add("y_pos", double_t, 0, "Arm end effector y position in m",0, -0.5, 0.5) +gen.add("z_pos", double_t, 0, "Arm end effector z position in m",0, -0.5, 0.5) +gen.add("time", double_t, 0, "Time in seconds to execute the motion",1.0,-1.0, 10.0) +gen.add("default_speed", double_t, 0, "Default servo speed used when time is set to -1",0.1,0.1,3.14159) +gen.add("start", bool_t, 0, "Start servo motion", False) +gen.add("stop", bool_t, 0, "Stop servo motion", False) + +exit(gen.generate(PACKAGE, "JointsCartClientAlgorithm", "JointsCartClient")) diff --git a/joints_cart_client/include/joints_cart_client_alg.h b/joints_cart_client/include/joints_cart_client_alg.h new file mode 100644 index 0000000000000000000000000000000000000000..d0c984b9a3b5a957a6fb3471c8906268340486c6 --- /dev/null +++ b/joints_cart_client/include/joints_cart_client_alg.h @@ -0,0 +1,131 @@ +// 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 _joints_cart_client_alg_h_ +#define _joints_cart_client_alg_h_ + +#include <joints_cart_client/JointsCartClientConfig.h> + +//include joints_cart_client_alg main library + +/** + * \brief IRI ROS Specific Driver Class + * + * + */ +class JointsCartClientAlgorithm +{ + protected: + /** + * \brief define config type + * + * Define a Config type with the JointsCartClientConfig. 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 JointsCartClientConfig. All driver implementations + * will then use the same variable type Config. + */ + typedef joints_cart_client::JointsCartClientConfig 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. + */ + JointsCartClientAlgorithm(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 joints_cart_client_alg interface methods to retrieve and set + // the driver parameters + + /** + * \brief Destructor + * + * This destructor is called when the object is about to be destroyed. + * + */ + ~JointsCartClientAlgorithm(void); +}; + +#endif diff --git a/joints_cart_client/include/joints_cart_client_alg_node.h b/joints_cart_client/include/joints_cart_client_alg_node.h new file mode 100644 index 0000000000000000000000000000000000000000..efb08694b28f9e03e58a459fd5e2f39852ad15be --- /dev/null +++ b/joints_cart_client/include/joints_cart_client_alg_node.h @@ -0,0 +1,125 @@ +// 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 _joints_cart_client_alg_node_h_ +#define _joints_cart_client_alg_node_h_ + +#include <iri_base_algorithm/iri_base_algorithm.h> +#include "joints_cart_client_alg.h" + +// [publisher subscriber headers] + +// [service client headers] + +// [action server client headers] +#include <humanoid_modules/joints_cart_module.h> + +/** + * \brief IRI ROS Specific Algorithm Class + * + */ +class JointsCartClientAlgNode : public algorithm_base::IriBaseAlgorithm<JointsCartClientAlgorithm> +{ + private: + // [publisher attributes] + + // [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_; + + CJointsCartModule action; + public: + /** + * \brief Constructor + * + * This constructor initializes specific class attributes and all ROS + * communications variables to enable message exchange. + */ + JointsCartClientAlgNode(void); + + /** + * \brief Destructor + * + * This destructor frees all necessary dynamic memory allocated within this + * this class. + */ + ~JointsCartClientAlgNode(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/joints_cart_client/package.xml b/joints_cart_client/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..908ba0b249fbb3ea7b54b2360e5eff61d5300afc --- /dev/null +++ b/joints_cart_client/package.xml @@ -0,0 +1,55 @@ +<?xml version="1.0"?> +<package> + <name>joints_cart_client</name> + <version>0.0.0</version> + <description>The joints_cart_client package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="shernand@todo.todo">shernand</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/joints_cart_client</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>humanoid_modules</build_depend> + <build_depend>geometry_msgs</build_depend> + <run_depend>iri_base_algorithm</run_depend> + <run_depend>humanoid_modules</run_depend> + <run_depend>geometry_msgs</run_depend> + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/joints_cart_client/src/joints_cart_client_alg.cpp b/joints_cart_client/src/joints_cart_client_alg.cpp new file mode 100644 index 0000000000000000000000000000000000000000..04b212cc9056720e6bbd10d28fccca509530f8af --- /dev/null +++ b/joints_cart_client/src/joints_cart_client_alg.cpp @@ -0,0 +1,23 @@ +#include "joints_cart_client_alg.h" + +JointsCartClientAlgorithm::JointsCartClientAlgorithm(void) +{ + pthread_mutex_init(&this->access_,NULL); +} + +JointsCartClientAlgorithm::~JointsCartClientAlgorithm(void) +{ + pthread_mutex_destroy(&this->access_); +} + +void JointsCartClientAlgorithm::config_update(Config& config, uint32_t level) +{ + this->lock(); + + // save the current configuration + this->config_=config; + + this->unlock(); +} + +// JointsCartClientAlgorithm Public API diff --git a/joints_cart_client/src/joints_cart_client_alg_node.cpp b/joints_cart_client/src/joints_cart_client_alg_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..239961f8e613aa35b510164e66eb75019da08271 --- /dev/null +++ b/joints_cart_client/src/joints_cart_client_alg_node.cpp @@ -0,0 +1,84 @@ +#include "joints_cart_client_alg_node.h" + +JointsCartClientAlgNode::JointsCartClientAlgNode(void) : + algorithm_base::IriBaseAlgorithm<JointsCartClientAlgorithm>(), + action("joints_cart_client",LEFT_ARM) +{ + //init class attributes if necessary + //this->loop_rate_ = 2;//in [Hz] + + // [init publishers] + + // [init subscribers] + + // [init services] + + // [init clients] + + // [init action servers] + + // [init action clients] +} + +JointsCartClientAlgNode::~JointsCartClientAlgNode(void) +{ + // [free dynamic memory] +} + +void JointsCartClientAlgNode::mainNodeThread(void) +{ + // [fill msg structures] + + // [fill srv structure and make request to the server] + + // [fill action structure and make request to the action server] + + // [publish messages] +} + +/* [subscriber callbacks] */ + +/* [service callbacks] */ + +/* [action callbacks] */ + +/* [action requests] */ + +void JointsCartClientAlgNode::node_config_update(Config &config, uint32_t level) +{ + geometry_msgs::PointStamped point; + + this->alg_.lock(); + if(config.start) + { + if(!this->action.is_finished()) + ROS_WARN("Impossible to execute action because the previous action is not finished yet"); + else + { + point.header.stamp=ros::Time::now(); + point.header.frame_id=config.frame_id; + point.point.x=config.x_pos; + point.point.y=config.y_pos; + point.point.z=config.z_pos; + this->action.go_to_position(point,config.time); + } + config.start=false; + } + else if(config.stop) + { + if(!this->action.is_finished()) + this->action.stop(); + config.stop=false; + } + this->alg_.unlock(); +} + +void JointsCartClientAlgNode::addNodeDiagnostics(void) +{ +} + +/* main function */ +int main(int argc,char *argv[]) +{ + return algorithm_base::main<JointsCartClientAlgNode>(argc, argv, "joints_cart_client_alg_node"); +}