From 23b817b9ce043689a82e594dfadcc3ac2112aeef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Mon, 28 Aug 2017 23:12:49 +0200 Subject: [PATCH] Initial implementation of the ROS wrapper for the IR feet sensors. --- ir_feet_sensor/CMakeLists.txt | 113 +++++++++++ ir_feet_sensor/cfg/IrFeetSensor.cfg | 44 +++++ .../include/.ir_feet_sensor_driver.h.swp | Bin 0 -> 16384 bytes .../include/ir_feet_sensor_driver.h | 159 +++++++++++++++ .../include/ir_feet_sensor_driver_node.h | 181 ++++++++++++++++++ ir_feet_sensor/package.xml | 52 +++++ ir_feet_sensor/src/ir_feet_sensor_driver.cpp | 56 ++++++ .../src/ir_feet_sensor_driver_node.cpp | 82 ++++++++ 8 files changed, 687 insertions(+) create mode 100644 ir_feet_sensor/CMakeLists.txt create mode 100755 ir_feet_sensor/cfg/IrFeetSensor.cfg create mode 100644 ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp create mode 100644 ir_feet_sensor/include/ir_feet_sensor_driver.h create mode 100644 ir_feet_sensor/include/ir_feet_sensor_driver_node.h create mode 100644 ir_feet_sensor/package.xml create mode 100644 ir_feet_sensor/src/ir_feet_sensor_driver.cpp create mode 100644 ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp diff --git a/ir_feet_sensor/CMakeLists.txt b/ir_feet_sensor/CMakeLists.txt new file mode 100644 index 0000000..19c6fd1 --- /dev/null +++ b/ir_feet_sensor/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ir_feet_sensor) + +## Find catkin macros and libraries +find_package(catkin REQUIRED) +# ******************************************************************** +# Add catkin additional components here +# ******************************************************************** +find_package(catkin REQUIRED COMPONENTS iri_base_driver) + +## 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) +find_package(comm REQUIRED) +find_package(iriutils REQUIRED) +find_package(ir_feet 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, services and actions 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/IrFeetSensor.cfg) + +# ******************************************************************** +# Add run time dependencies here +# ******************************************************************** +catkin_package( +# INCLUDE_DIRS +# LIBRARIES +# ******************************************************************** +# Add ROS and IRI ROS run time dependencies +# ******************************************************************** + CATKIN_DEPENDS iri_base_driver +# ******************************************************************** +# 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(${comm_INCLUDE_DIR}) +include_directories(${dynamxiel_INCLUDE_DIR}) +include_directories(${ir_feet_INCLUDE_DIR}) +# include_directories(${<dependency>_INCLUDE_DIR}) + +## Declare a cpp library +# add_library(${PROJECT_NAME} <list of source files>) +# add_library(${PROJECT_NAME} <list of source files>) + +## Declare a cpp executable +add_executable(${PROJECT_NAME} src/ir_feet_sensor_driver.cpp src/ir_feet_sensor_driver_node.cpp) + +# ******************************************************************** +# Add the libraries +# ******************************************************************** +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY}) +target_link_libraries(${PROJECT_NAME} ${comm_LIBRARY}) +target_link_libraries(${PROJECT_NAME} ${dynamixel_LIBRARY}) +target_link_libraries(${PROJECT_NAME} ${ir_feet_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/ir_feet_sensor/cfg/IrFeetSensor.cfg b/ir_feet_sensor/cfg/IrFeetSensor.cfg new file mode 100755 index 0000000..eaa62d9 --- /dev/null +++ b/ir_feet_sensor/cfg/IrFeetSensor.cfg @@ -0,0 +1,44 @@ +#! /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='ir_feet_sensor' + +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, "IrFeetSensorDriver", "IrFeetSensor")) diff --git a/ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp b/ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp new file mode 100644 index 0000000000000000000000000000000000000000..4237eb4bf94311dfa474fc7b8fb78bebb48adf62 GIT binary patch literal 16384 zcmeHNON<;x8E#@A5HKJ|98e-u*n-%b*_pK+2^c$;H?wv;!rooY>?SO+JgMofnJ%`c zt5e;*<4Hs!B_aqE4hRVagai?g6Wl;TAUH&c#PfiN5Jm_%AP^66;h`WIh3~Jbo|o5N z5Eo1INZ<C(boF0<egC7X{_5`11I-KUz14d>eBS1HfBoo}x4yOMc|W|}^HQPvl78Rg z6XpXRD;cDFw0qnLtVt~wBi^NJXPDMf?biwk9bXLwD#oo&Pl&9Oia1reCSyO!12Ok= zC(v?7=xVq}x8?1Ufs%nd>n*--|K{;yN0*3<LkHi*Za;uPFFz#%B?Bb`B?Bb`B?Bb` zB?Bb`B?JFA3}neI-V+$@EpEgQyX$M_uKV2g)9%XW?z^w$rDULFpk$zApk$zApk$zA zpk$zApk$zApk$zA;3i~%2cGv@==DX~@Z<ddVW;C)-|BhK0UrmR2D-o<z>nVIc^?Bf za0$2rxU%2#o&`8?2{-_J|IMEF67VQ+7jWfGp7#}C2grdoa1>Yq_5m;7=6OE_o(G-; z20#Raz-i!K;LmULyw3t30It5l^L`Kf7Wfo!2G|F9z}If|ybf>xc=`37_Y>g9z%#%n zfG2=0U=cV3+zDKLo#*`n_!5u+5;z7N1r~t)z^%YHK-LbRfQNt&0d1fT+yndpg_381 zv%tH7cLE;pDhe#G0KW#V0G|Y!z(2wJ?||O`F96>Lj4hlU7(QL4BBl}%ub3Ae4)5Sy ziz6pIy0uWV7j{vLEZ4DZo94bR(zKUH_O5Fq9?5x>vP|cq>V9f!c^^-O6<n_eY7N~S zK2l*kW@*MVfz~`uWhPb3WXckudrA*Pz+{iPE_Zm83nwq*+X4;aOfl~JDvz@&+X(Ss zFOU5~2+uP$;F<J!6pa}Dg}m;F6*|Q%n|`E%uY%c^J=h>mGu9P`j#va$MheF=?lYma z(rMAa%5oAfcC{2e^Nf>2H);zDo@vn3XYsO0+q`RX1BwTHL_LE-#S~sVi4+f5DD;@j z<t)>(n`Z*U)oNhW9&j16SOwz&_z_Q2Hp4rEy+X!Q7Aci<G+3`V#bqor9?3^(pc|n| z1xvW*1Ca@g)Q61<eNvHub<^XNqZ}F2KDM{x)>m-~Kkzf9r$uTJq4$8ne8hR02w(Q3 zUyyHmw7NMB9ko;P=*F1ZhKhT#&u|-;&YaISQ<f)z=}>^rQ6>jsta0&-ZE^Srd7pKa zVl2k5cDR<j8wnPIKX}r#uS{_jh@OPI6dd`zLMPUfQDMMqhD53t9n1_X8zllF+3b5t z_4>3YxuM(3+{lO0Y8tZ~HZ@eks^h*G$!GS-Vzet}1dO|%ZIhwQLi6~w9aq^h`fyPr z2T3FbB1S|bkW)L*+2LA-9P37UF=ED&yc<csZXRefTNEuP8!Mxj59DPLwS|WD@K^Ha z6br<T^o1R7t;Q0hEJ`DM5*s`Y7+g41cwr2(ZG*b^m{)|xd^EDU(*S(SP8a#gz4}Fj zPvtMqq{9VV3i&LO+lU#JW~&H6N)_jn+J-Zc4q8~Z%(bAq_73P?V6${F)iWG7&>1yk zR!?I?y!giZWWl-E%tEd5en`R)ONdfG<Z&Mk7#P-U=0nOO)3?^z_?gE81lp8Nj3)_L zHx{Gl7s!tb=DLfq$)RH*0%Uj79lV;7YMwCXB6IqsIT8!P1|msnKSx@LGXXorl(;c@ zND3+>awL52BP@|9D7>sg-G!N8oQMo%DeoaGA)QnDsemGMSYbMcI8dEtDlubq{>U@@ zjL{?DtE`DHlv7P=-))RGCp+3rtFeKMPV;F>W1^GV_&f38#t61&yv3+(xHq#ZbfkO; zXQ*0>m|u;zT{9CR(`cFqwBW&rSsw^UbHURQazA2usPs13f*9;mN*^aSUXYkh{LE&i z#@%-p<|4zuDl!le@|Lmc)R;lt+?cI{%#6?RrFuP@EMQklEu~R21<4q5m&~T-s%ERn zXf@y(%t))SVJI<6C0gx3D`*rJ&f}&4dYb5L7n6oFTZ4M@%#3izy~xQ9y2M!x4|<ay zZEPT;;5|`f6ukXODTUh9idR7AGOo)5FaajNNOm>XBdqC)ZcfhT=h`lT6P}uBK9gR| z8>&<3ri#YiA(tpSv$CBgov;IWy_hD4>$kvoSeRu=y;d8_ZK-87PyDJ#G0H$(t|nnp zYpJfn3MuWJ=Ur;91<XheMHz#rrUwy(NIn#r0|keaF|Ex^!l=_TE^np2mPwXcw?-gB zYO!{JzanILV3uEbO3Nsl%Er&EM`$BR8r1A)m3HQCu|ToawGHI`2noQ1HNC`(xyE?y zB(sp>zS78W89W*uQDe;u>uarz<<$+6aHeioiaw=Sn$5ccOUb^71%(gRBsQgD2DBPX z)>tHpz$1kLxP%8khp!^^235wIJ*>g93QLjUP8Q4$ht+<ZSC#J9kY};_g8@!eNzj(c zSsJQ5!upQy2+GO=uNnvF+gAy4oShKvU)^NqF%%f)exqi@*3C@G&RcnqMdJ2sH!)!# zGA^UkXmR08t8otGEZ^5W-`v<@F!R~w#_E}No1I;2v1PWt+}db1HqS4&*!pH`eXV^4 zLv3S-6r1Q5OHuY<bK~6F<_23{-C_?ew_1?5b;59@a2LxiyJ)23A~C=r*EA8(5J`er zVmTUV0TxJPNwdTqFQq~xd~}G6E;Go*6oJ_es~T)RDsKF^H&VIA)Ih5bntVr$)|r|i z%2WJghD-i&Rzv%?>SfRa->`qc@(>LwOkkxmd4L@+Y~Yx#fE`j8I^Y?K6i7o4gS2S4 zw-MlSW}%aYAfh`m;_#|Ee10+6?$hvlq+aUOtJ9!NAZan-g*3p7S2Hmg$es~ncR*NZ znk^L987YN$Dr7sJXu+msKSbncVRB$#m|4ydl-js>1MX*<u)~cb?AX%LrA7QX%9>ct z%M2Dp?6}mynZ(a7BlsNu2iQANMy;?$yV+RWTyIo8+W$X?ee!o`?~MKbX}9lu6?^?x zfL{SW2c7}m5Bw4P{T~1?0?z|q0G<M#1RevZF7Oa=8}I_^0FMAGz$t(M?*RUVdcalS z@4zpCp8-z+CxAbp7Vur*o51IQPXm{MA&>x@z#2d`gRi3|@Oj`fz$M^=z@5O`fnT6T za0Pf8kU#+Vz(rsMI0gI-WBv<3b%gH$F99zCh6~3-c_|qv87LVj87LVj8Mw(9I3!{$ zu{`f1w%qji^#0E7K6k5?ySkTIlwl`ZltF1@j0y)Wf<w0|R^1O{w?d0H-lgTTt*w}6 z)Ce_ME6K@o+u)V4MEZSP2sMqcK&5r5D}qcaD8)@GCSwIhrAkxS*ma9JRg`g6G|~&T zc+|MDS25)lk=e>NofOO!Qf*c7J*XB9JE$5}SdJ3K@nfBgMR+{Acj=mw1*50E>;7W1 zZL0w^8e5<xl5FumiBK;xBSL8jp9OZ&N^m_I3h;kHLs5h?3w>K^nR(hOJSiYL){9DI zQJ*yx5WATn;k!s3?@myS!jc_48daQ<O;;HoA5`pWsp5EHT(}-rU}c=-*^J_o(m2(T zElt#5>=Wi(he~7D_tpRCT<5xnGHjO<pX(jI%v5{mB&ZF=Zg3KWauPlZ#@~u#gTfYR zv3tf=5W7d{hbjY(DDs4?L>sB8(;z2jxknqqZ2Io>!4E2qC;*_|X02!nid4K3mxZ6x z7SUF~Y-m}LPOvkBd17*V-U!%~-K184W;c%+jJb*TX=A|3I4jgB((G*n+WcP63Z`iM zKU!h>Oma8(pPV*c<M0itS>J%_nJ{zCWryMl#HKWDO4(rKzds3eBIgs)bQFx!kD&^5 YKj4nPsA3S=GMOn6?iK;&nR)X64XkgvdjJ3c literal 0 HcmV?d00001 diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver.h b/ir_feet_sensor/include/ir_feet_sensor_driver.h new file mode 100644 index 0000000..5510353 --- /dev/null +++ b/ir_feet_sensor/include/ir_feet_sensor_driver.h @@ -0,0 +1,159 @@ +// 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 _ir_feet_sensor_driver_h_ +#define _ir_feet_sensor_driver_h_ + +#include <iri_base_driver/iri_base_driver.h> +#include <ir_feet_sensor/IrFeetSensorConfig.h> + +//include ir_feet_sensor_driver main library +#include "dynamixelserver_serial.h" +#include "ir_feet.h" + +/** + * \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 IrFeetSensorDriver 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 IrFeetSensorConfig.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 IrFeetSensorDriver : public iri_base_driver::IriBaseDriver +{ + private: + // private attributes and methods + CIRFeet<CDynamixelServerSerial> device; + public: + /** + * \brief define config type + * + * Define a Config type with the IrFeetSensorConfig. All driver implementations + * will then use the same variable type Config. + */ + typedef ir_feet_sensor::IrFeetSensorConfig 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. + */ + IrFeetSensorDriver(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 ir_feet_sensor_driver interface methods to retrieve and set + // the driver parameters + + /** + * \brief Destructor + * + * This destructor is called when the object is about to be destroyed. + * + */ + ~IrFeetSensorDriver(void); +}; + +#endif diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver_node.h b/ir_feet_sensor/include/ir_feet_sensor_driver_node.h new file mode 100644 index 0000000..882899d --- /dev/null +++ b/ir_feet_sensor/include/ir_feet_sensor_driver_node.h @@ -0,0 +1,181 @@ +// 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 _ir_feet_sensor_driver_node_h_ +#define _ir_feet_sensor_driver_node_h_ + +#include <iri_base_driver/iri_base_driver_node.h> +#include "ir_feet_sensor_driver.h" + +// [publisher subscriber headers] + +// [service client headers] + +// [action server client headers] + +/** + * \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 IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver> +{ + private: + // [publisher attributes] + + // [subscriber attributes] + + // [service attributes] + + // [client attributes] + + // [action server attributes] + + // [action client attributes] + + /** + * \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 + * + * This constructor mainly creates and initializes the IrFeetSensorDriverNode topics + * through the given public_node_handle object. IriBaseNodeDriver attributes + * may be also modified to suit node specifications. + * + * All kind of ROS topics (publishers, subscribers, servers or clients) can + * be easyly generated with the scripts in the iri_ros_scripts package. Refer + * to ROS and IRI Wiki pages for more details: + * + * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++) + * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++) + * http://wikiri.upc.es/index.php/Robotics_Lab + * + * \param nh a reference to the node handle object to manage all ROS topics. + */ + IrFeetSensorDriverNode(ros::NodeHandle& nh); + + /** + * \brief Destructor + * + * This destructor is called when the object is about to be destroyed. + * + */ + ~IrFeetSensorDriverNode(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 driver 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); + + // [diagnostic functions] + + /** + * \brief node add diagnostics + * + * In this function ROS diagnostics applied to this specific node may be + * added. Common use diagnostics for all nodes are already called from + * IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information + * of how ROS diagnostics work can be readen here: + * http://www.ros.org/wiki/diagnostics/ + * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html + */ + void addNodeDiagnostics(void); + + // [driver test functions] + + /** + * \brief open status driver tests + * + * In this function tests checking driver's functionallity when driver_base + * status=open can be added. Common use tests for all nodes are already called + * from IriBaseNodeDriver tests methods. For more details on how ROS tests work, + * please refer to the Self Test example in: + * http://www.ros.org/wiki/self_test/ + */ + void addNodeOpenedTests(void); + + /** + * \brief stop status driver tests + * + * In this function tests checking driver's functionallity when driver_base + * status=stop can be added. Common use tests for all nodes are already called + * from IriBaseNodeDriver tests methods. For more details on how ROS tests work, + * please refer to the Self Test example in: + * http://www.ros.org/wiki/self_test/ + */ + void addNodeStoppedTests(void); + + /** + * \brief run status driver tests + * + * In this function tests checking driver's functionallity when driver_base + * status=run can be added. Common use tests for all nodes are already called + * from IriBaseNodeDriver tests methods. For more details on how ROS tests work, + * please refer to the Self Test example in: + * http://www.ros.org/wiki/self_test/ + */ + void addNodeRunningTests(void); + + /** + * \brief specific node dynamic reconfigure + * + * This function is called reconfigureHook() + * + * \param level integer + */ + void reconfigureNodeHook(int level); + +}; + +#endif diff --git a/ir_feet_sensor/package.xml b/ir_feet_sensor/package.xml new file mode 100644 index 0000000..da2d891 --- /dev/null +++ b/ir_feet_sensor/package.xml @@ -0,0 +1,52 @@ +<?xml version="1.0"?> +<package> + <name>ir_feet_sensor</name> + <version>0.0.0</version> + <description>The ir_feet_sensor package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="sergi@todo.todo">sergi</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/ir_feet_sensor</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_driver</build_depend> + <run_depend>iri_base_driver</run_depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver.cpp new file mode 100644 index 0000000..6eb4685 --- /dev/null +++ b/ir_feet_sensor/src/ir_feet_sensor_driver.cpp @@ -0,0 +1,56 @@ +#include "ir_feet_sensor_driver.h" + +IrFeetSensorDriver::IrFeetSensorDriver(void) +{ + //setDriverId(driver string id); +} + +bool IrFeetSensorDriver::openDriver(void) +{ + //setDriverId(driver string id); + + return true; +} + +bool IrFeetSensorDriver::closeDriver(void) +{ + return true; +} + +bool IrFeetSensorDriver::startDriver(void) +{ + return true; +} + +bool IrFeetSensorDriver::stopDriver(void) +{ + return true; +} + +void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level) +{ + this->lock(); + + // depending on current state + // update driver with new_cfg data + switch(this->getState()) + { + case IrFeetSensorDriver::CLOSED: + break; + + case IrFeetSensorDriver::OPENED: + break; + + case IrFeetSensorDriver::RUNNING: + break; + } + + // save the current configuration + this->config_=new_cfg; + + this->unlock(); +} + +IrFeetSensorDriver::~IrFeetSensorDriver(void) +{ +} diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp new file mode 100644 index 0000000..0faf9d0 --- /dev/null +++ b/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp @@ -0,0 +1,82 @@ +#include "ir_feet_sensor_driver_node.h" + +IrFeetSensorDriverNode::IrFeetSensorDriverNode(ros::NodeHandle &nh) : + iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>(nh) +{ + //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] +} + +void IrFeetSensorDriverNode::mainNodeThread(void) +{ + //lock access to driver if necessary + this->driver_.lock(); + + // [fill msg Header if necessary] + + // [fill msg structures] + + // [fill srv structure and make request to the server] + + // [fill action structure and make request to the action server] + + // [publish messages] + + //unlock access to driver if previously blocked + this->driver_.unlock(); +} + +/* [subscriber callbacks] */ + +/* [service callbacks] */ + +/* [action callbacks] */ + +/* [action requests] */ + +void IrFeetSensorDriverNode::postNodeOpenHook(void) +{ +} + +void IrFeetSensorDriverNode::addNodeDiagnostics(void) +{ +} + +void IrFeetSensorDriverNode::addNodeOpenedTests(void) +{ +} + +void IrFeetSensorDriverNode::addNodeStoppedTests(void) +{ +} + +void IrFeetSensorDriverNode::addNodeRunningTests(void) +{ +} + +void IrFeetSensorDriverNode::reconfigureNodeHook(int level) +{ +} + +IrFeetSensorDriverNode::~IrFeetSensorDriverNode(void) +{ + // [free dynamic memory] +} + +/* main function */ +int main(int argc,char *argv[]) +{ + return driver_base::main<IrFeetSensorDriverNode>(argc, argv, "ir_feet_sensor_driver_node"); +} -- GitLab