Skip to content
Snippets Groups Projects
Commit 77dfa256 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'kinetic_migration'

parents e0630bd1 9c455d6b
No related branches found
No related tags found
No related merge requests found
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(iri_base_driver) project(iri_base_driver)
# set(CMAKE_BUILD_TYPE "DEBUG")
## Add support for C++11, supported in ROS Kinetic and newer
# add_definitions(-std=c++11)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED) find_package(catkin REQUIRED COMPONENTS
roscpp
dynamic_reconfigure
message_generation
)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(Boost REQUIRED COMPONENTS system thread)
## Uncomment this if the package has a setup.py. This macro ensures ## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed ## modules and global scripts declared therein get installed
...@@ -39,11 +49,10 @@ find_package(catkin REQUIRED) ...@@ -39,11 +49,10 @@ find_package(catkin REQUIRED)
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
# add_message_files( add_message_files(
# FILES FILES
# Message1.msg SensorLevels.msg
# Message2.msg )
# )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
# add_service_files( # add_service_files(
...@@ -60,10 +69,10 @@ find_package(catkin REQUIRED) ...@@ -60,10 +69,10 @@ find_package(catkin REQUIRED)
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
# generate_messages( generate_messages(
# DEPENDENCIES DEPENDENCIES
# std_msgs # Or other packages containing msgs std_msgs # Or other packages containing msgs
# ) )
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
...@@ -77,8 +86,8 @@ find_package(catkin REQUIRED) ...@@ -77,8 +86,8 @@ find_package(catkin REQUIRED)
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp driver_base dynamic_reconfigure CATKIN_DEPENDS roscpp dynamic_reconfigure message_runtime
# DEPENDS system_lib DEPENDS Boost
) )
########### ###########
...@@ -88,13 +97,21 @@ catkin_package( ...@@ -88,13 +97,21 @@ catkin_package(
## Specify additional locations of header files ## Specify additional locations of header files
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
# include_directories(include) # include_directories(include)
INCLUDE_DIRECTORIES(include) include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
## Declare a cpp library ## Declare a cpp library
# add_library(foo # add_library(foo
# src/${PROJECT_NAME}/foo.cpp # src/${PROJECT_NAME}/foo.cpp
# ) # )
add_library(${PROJECT_NAME} src/iri_base_driver.cpp) add_library(${PROJECT_NAME} src/iri_base_driver.cpp)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
## Declare a cpp executable ## Declare a cpp executable
# add_executable(foo_node src/foo_node.cpp) # add_executable(foo_node src/foo_node.cpp)
...@@ -102,6 +119,7 @@ add_library(${PROJECT_NAME} src/iri_base_driver.cpp) ...@@ -102,6 +119,7 @@ add_library(${PROJECT_NAME} src/iri_base_driver.cpp)
## Add cmake target dependencies of the executable/library ## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes ## as an example, message headers may need to be generated before nodes
# add_dependencies(foo_node foo_generate_messages_cpp) # add_dependencies(foo_node foo_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against ## Specify libraries to link a library or executable target against
# target_link_libraries(foo_node # target_link_libraries(foo_node
......
README.md 0 → 100644
# Description
This project is intended to be used as a base class for ROS nodes that have to communicate with external hardware. The next diagram shows the basic structure of the IRI ROS base driver node:
<img src="doc/images/iri_base_driver_diagram.png" alt="Basic structure of the IRI ROS base driver node" width="998" height="630">
This node was originally based on the [ROS driver_base node](http://wiki.ros.org/driver_base) which is now deprecated. Since then, it re-implements most of the driver_base features.
This node is structured into two parts, both implemented as C++ classes:
## Driver
This part is intended for the implementation of the driver itself. The use of an external library to implement the driver is strongly recommended to separate the driver functionality from the ROS middleware. However it is also possible to implement the driver here. The main features of this part are listed next, and a detailed description can be found below.
* State machine to control the open, close, start and stop action of the driver.
* Mutex
<details><summary>Detailed features description</summary>
<p>
### State machine
The next figure shows the possible states and transitions (actions) between them of the state machine implemented for the driver.
<img src="doc/images/state_machine.png" alt="State diagram of the state machine implemented in the driver part" width="635" height="455">
The driver tries to transition form the initial state (CLOSED) to the RUNNING state every second until it succeeds. To do so, each transition calls a function that must be implemented by the inherited class (openDriver(), closeDriver(), startDriver() and stopDriver()). These functions should interact with the actual hardware to perform the desired action. If the hardware does not support one or more of these actions, the corresponding functions should do nothing but return success (true).
Some of the parameters of the driver may not be changed on the RUNNING state. Therefore, each of the parameters defined in the dynamic reconfigure interface of the driver node (see below for more information) provides the driver state in which it should be modified. Whenever the value of one of these parameters is changed, the state machine automatically transitions to the desired state, and then returns to the previous one, once the parameter has been updated.
In addition to the driver transition action functions, a set of callback functions are also provided so that the driver node can perform some action (if necessary) after the driver has been opened (postOpenHook) or started (postStartHook), and before it is stopped (preStopHook) or closed (preCloseHook). These functions do nothing by default, but may be implemented by the inherited class to perform any required action.
### Mutex
The driver part provides a mutex to protect the access to critical information. This mutex can be used internally by the driver functions and also by the driver node through a set of public functions of the driver class (lock(), unlock() and try_enter()). Make sure the mutex is left unlocked after use to avoid blocking the normal operation of the node.
</p>
</details>
## Driver node
This part is intended for the implementation of the ROS interfaces (topics, services and actions) and other ROS tools. It has an object of the driver class to have access to have full access to the driver functionalities. The main features of this part are listed next, and a detailed description can be found below.
* Independent thread with configurable rate.
* Dynamic reconfigure server.
* Diagnostics publisher.
* Public and private ROS node handlers.
<details><summary>Detailed features description</summary>
<p>
### Threads
The driver node provides three separate threads:
* The spin thread is used to call the ROS spin() function. The single threaded version of this function is used, so that all callbacks are executed, sequentially, in the same thread.
* The user thread is used to provide the ability to execute code in parallel and avoid blocking the callback functions. This thread is handled internally when the class is created and destroyed and a single function is provided to the user (mainNodeThread) which is called at a desired rate. The thread rate can be handled with the *setRate()* and *getRate()* functions. The user thread function is only called when the driver is in the RUNNING state.
* The main thread is used to handle the state machine of the driver and publish the node diagnostics data.
### Dynamic reconfigure
A dynamic reconfigure server is initialized by default. It has an associated .cfg file located in the *cfg* folder where the user can define all the parameters that can be updated at run-time. Whenever a parameter value is changed (using the [rqt_reconfigure](http://wiki.ros.org/rqt_reconfigure)) application) two callback functions are called:
* reconfigure callback at the driver part (config_update).
* reconfigure callback at the driver node part (node_config_update)
Each callback receives the whole dynamic reconfigure data structure, but should only use its associated parameters.
All node parameters can be defined this way, but it is recommended to only use it for run-time dynamic parameters.
### Diagnostic publisher
A diagnostic publisher is also initialized by default. It provides the ability to collect status data from the driver and the driver node, and publishes it. The [rqt_runtime_monitor](http://wiki.ros.org/rqt_runtime_monitor) application can be used to monitor the diagnostics.
By default a single diagnostic is published with the current status of the driver (CLOSED, OPENED or RUNNING). Additional diagnostics can be added at star-up by the user with the addNodeDiagnostics() function which can be implemented in the inherited class. For further information on ROS diagnostics feature go [here](http://wiki.ros.org/diagnostics).
### ROS node Handlers
The driver node provides two separate ROS NodeHandlers:
* the public node handle is initialized by a node handle provided to the constructor of the driver node. If no node handle is defined, "/" will be used.
* the private node handle is initialized to "~" by default.
These node handlers can be used to get/set ROS parameters and to set-up ROS interfaces. Depending on the node handle used, the namespace of the corresponding parameter or interface will be different.
</p>
</details>
# ROS Interface
### Service servers
- *~/set_parameters* ([dynamic_reconfigure/Reconfigure.srv](http://docs.ros.org/melodic/api/dynamic_reconfigure/html/srv/Reconfigure.html)):
Service to dynamically change the parameters of the node. Only the parameters defined in the .cfg file can be modified this way.
### topic publishers
- */diagnostics* ([diagnostic_msgs/DiagnosticArray](http://docs.ros.org/api/diagnostic_msgs/html/msg/DiagnosticArray.html)):
Topic with all the diagnostics published by the node.
### Parameters
- **rate** *(double; Default: 10.0; Max: 1000.0; Min: 0.1)*:
The rate of execution of the user thread in Hz.
# Dependencies
This base node has no dependencies.
# How to use it
This ROS node is not designed to be used on its own, an inherited class is always required.
To simplify the process of inheriting from this class, there is an script (*create_driver_package*) which creates a ROS driver package using this node as a base class.
Also, there exist other scripts to easily add ROS interfaces (topic publishers or subscribers, service servers or clients and action servers or clients) and also TF listener or broadcaster.
Check the [IRI ROS scripts](https://gitlab.iri.upc.edu/labrobotica/ros/iri_core/iri_ros_scripts) page for further information on all these scripts.
## Disclaimer
Copyright (C) Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair or correction.
In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
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/>
doc/images/iri_base_driver_diagram.png

76.7 KiB

doc/images/state_machine.png

33.4 KiB

File added
File added
...@@ -2,11 +2,11 @@ ...@@ -2,11 +2,11 @@
// Author Sergi Hernandez & Joan Perez // Author Sergi Hernandez & Joan Perez
// All rights reserved. // All rights reserved.
// //
// This file is part of iri-ros-pkg // This file is part of iri-ros-pkg.
// iri-ros-pkg is free software: you can redistribute it and/or modify // 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 // 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 // the Free Software Foundation, either version 3 of the License, or
// at your option) any later version. // at your option any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
...@@ -20,12 +20,16 @@ ...@@ -20,12 +20,16 @@
#define _IRI_BASE_DRIVER_H #define _IRI_BASE_DRIVER_H
// ros base driver class // ros base driver class
#include "driver_base/driver.h"
#include "ros/ros.h" #include "ros/ros.h"
// boost libraries
#include <boost/function.hpp>
namespace iri_base_driver namespace iri_base_driver
{ {
typedef enum {CLOSED=0,OPENED=1,RUNNING=2} state_t;
/** /**
* \brief IRI ROS Base Driver Class * \brief IRI ROS Base Driver Class
* *
...@@ -34,41 +38,56 @@ namespace iri_base_driver ...@@ -34,41 +38,56 @@ namespace iri_base_driver
* framework to integrate functional drivers implemented in C++ with the ROS * framework to integrate functional drivers implemented in C++ with the ROS
* driver structure. A shared library is generated together with IriBaseNodeDriver * driver structure. A shared library is generated together with IriBaseNodeDriver
* class to force implementation of virtual methods to final user. Common use * class to force implementation of virtual methods to final user. Common use
* functions to open, close, start and stop a driver need to be filled up when * functions to open, close, start and stop a driver need to be filled up when
* instantiating this class. ROS provides state transition methods to change * instantiating this class. ROS provides state transition methods to change
* from one state to another. * from one state to another.
* *
* This class is used as an intermediate level between ROS basic classes and the * This class is used as an intermediate level between ROS basic classes and the
* user final driver implementation. Methods to manage drivers are implemented * user final driver implementation. Methods for managing drivers are implemented
* to provide common features to all driver-type objects. New methods are * to provide common features to all driver-type objects. New methods are
* provided to the user for the specific driver needs. * provided to the user for the specific driver needs.
* *
* Instances of both IriBaseDriver and IriBaseNodeDriver can be easly generated * Instances of both IriBaseDriver and IriBaseNodeDriver can be easly generated
* with the iri_ros_scripts package. Similarly, data can be sent and read * with the iri_ros_scripts package. Similarly, data can be sent and read
* through ROS topics by using those scripts. The scripts can be downloaded * through ROS topics by using those scripts. The scripts can be downloaded
* from the iri_stack SVN. * from the GitLab repository (See http://wiki.iri.upc.edu).
*/ */
class IriBaseDriver : public driver_base::Driver class IriBaseDriver
{ {
protected: private:
/** state_t current_state_;
* \brief driver unique identifier
*
* This value is used to identify each driver. Must be set before changing
* the driver_base::state=run. Use the abstract function setDriverId().
*/
std::string driver_id_;
/** /**
* \brief variable to handle the mutual exclusion mechanism * \brief Variable to handle the mutual exclusion mechanism
* *
* this variable is automatically initialized when an object of this class * This variable is automatically initialized when an object of this class
* or any inherited class is created and holds all the necessary information * or any inherited class is created and holds all the necessary information
* to handle any shared resource. * to handle any shared resource.
* *
*/ */
pthread_mutex_t access_; pthread_mutex_t access_;
pthread_mutex_t internal_access_;
boost::function< void() > postOpenHook;
boost::function< void() > preCloseHook;
boost::function< void() > postStartHook;
boost::function< void() > preStopHook;
/**
* \brief driver unique identifier
*
* This value is used to identify each driver. Must be set before changing
* the driver_base::state=run. Use the abstract function setDriverId().
*/
std::string driver_id_;
std::string last_message;
protected:
/** /**
* \brief Set Driver Id * \brief Set Driver Id
* *
...@@ -80,9 +99,6 @@ class IriBaseDriver : public driver_base::Driver ...@@ -80,9 +99,6 @@ class IriBaseDriver : public driver_base::Driver
*/ */
void setDriverId(const std::string & id); void setDriverId(const std::string & id);
// typedef boost::function< void() > hookFunction;
hookFunction preCloseHook;
public: public:
/** /**
* \brief Constructor * \brief Constructor
...@@ -115,42 +131,6 @@ class IriBaseDriver : public driver_base::Driver ...@@ -115,42 +131,6 @@ class IriBaseDriver : public driver_base::Driver
*/ */
bool try_enter(void); bool try_enter(void);
/**
* \brief Generic Open Driver
*
* Currently only changes the ROS driver state to open and calls the abstract
* function openDriver(). Common actions required for opening any kind of
* drivers should be added here.
*/
void doOpen(void);
/**
* \brief Generic Close Driver
*
* Currently only changes the ROS driver state to close and calls the abstract
* function closeDriver(). Common actions required for opening any kind of
* drivers should be added here.
*/
void doClose(void);
/**
* \brief Generic Start Driver
*
* Currently only changes the ROS driver state to start and calls the abstract
* function startDriver().Common actions required for opening any kind of
* drivers should be added here.
*/
void doStart(void);
/**
* \brief Generic Stop Driver
*
* Currently only changes the ROS driver state to stop and calls the abstract
* function stopDriver(). Common actions required for opening any kind of
* drivers should be added here.
*/
void doStop(void);
/** /**
* \brief Get Driver Id * \brief Get Driver Id
* *
...@@ -162,6 +142,22 @@ class IriBaseDriver : public driver_base::Driver ...@@ -162,6 +142,22 @@ class IriBaseDriver : public driver_base::Driver
*/ */
std::string getID(void); std::string getID(void);
bool goState(state_t target_state);
state_t getState(void);
std::string getStateName(void);
std::string getStatusMessage(void);
bool isRunning(void);
bool isOpened(void);
bool isClosed(void);
bool isStopped(void);
/** /**
* \brief User Open Driver * \brief User Open Driver
* *
...@@ -204,8 +200,13 @@ class IriBaseDriver : public driver_base::Driver ...@@ -204,8 +200,13 @@ class IriBaseDriver : public driver_base::Driver
*/ */
virtual bool stopDriver(void) = 0; virtual bool stopDriver(void) = 0;
void setPostOpenHook(boost::function< void() > function);
void setPreCloseHook(hookFunction f);
void setPreCloseHook(boost::function< void() > function);
void setPostStartHook(boost::function< void() > function);
void setPreStopHook(boost::function< void() > function);
/** /**
* \brief Destructor * \brief Destructor
......
This diff is collapsed.
byte RECONFIGURE_CLOSE = 3 # Parameters that need a sensor to be stopped completely when changed
byte RECONFIGURE_STOP = 1 # Parameters that need a sensor to stop streaming when changed
byte RECONFIGURE_RUNNING = 0 # Parameters that can be changed while a sensor is streaming
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package format="2">
<name>iri_base_driver</name> <name>iri_base_driver</name>
<version>1.0.0</version> <version>1.0.0</version>
<description>The iri_base_driver package</description> <description>The iri_base_driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer> <maintainer email="labrobotica@iri.upc.edu">labrobotica</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> <license>LGPL</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://wiki.ros.org/iri_base_driver</url> <url type="website">http://wiki.ros.org/iri_base_driver</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<author>Joan Perez</author> <author>Joan Perez</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>roscpp</build_depend>
<build_depend>driver_base</build_depend>
<build_depend>dynamic_reconfigure</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>roscpp</run_depend>
<run_depend>driver_base</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>dynamic_reconfigure</depend>
<depend>message_runtime</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package> </package>
...@@ -3,9 +3,13 @@ ...@@ -3,9 +3,13 @@
namespace iri_base_driver namespace iri_base_driver
{ {
IriBaseDriver::IriBaseDriver() : driver_id_("none") IriBaseDriver::IriBaseDriver()
{ {
this->current_state_=iri_base_driver::CLOSED;
this->driver_id_="none";
this->last_message="not operational yet";
pthread_mutex_init(&this->access_,NULL); pthread_mutex_init(&this->access_,NULL);
pthread_mutex_init(&this->internal_access_,NULL);
} }
void IriBaseDriver::lock(void) void IriBaseDriver::lock(void)
...@@ -31,40 +35,232 @@ void IriBaseDriver::setDriverId(const std::string & id) ...@@ -31,40 +35,232 @@ void IriBaseDriver::setDriverId(const std::string & id)
driver_id_ = id; driver_id_ = id;
} }
void IriBaseDriver::doOpen(void) std::string IriBaseDriver::getID(void)
{ {
if(openDriver()) this->state_ = OPENED; return this->driver_id_;
} }
void IriBaseDriver::doClose(void) bool IriBaseDriver::goState(state_t target_state)
{ {
this->preCloseHook(); std::string transition;
if(closeDriver()) this->state_ = CLOSED; bool error=false;
pthread_mutex_lock(&this->internal_access_);
try{
do{
switch(this->current_state_)
{
case iri_base_driver::CLOSED: if(target_state == iri_base_driver::OPENED || target_state == iri_base_driver::RUNNING)
{
transition="open";
ROS_DEBUG("Trying transition open from CLOSED to OPENED.");
if(this->openDriver())
{
this->current_state_=iri_base_driver::OPENED;
this->postOpenHook();
this->last_message="Transition open from CLOSED to OPENED succeeded.";
ROS_DEBUG("%s",this->last_message.c_str());
}
else
{
this->current_state_=iri_base_driver::CLOSED;
this->last_message="Transition open from CLOSED to OPENED failed.";
ROS_DEBUG("%s",this->last_message.c_str());
error=true;
}
}
else
{
this->current_state_=iri_base_driver::CLOSED;
this->last_message="No transition required";
ROS_DEBUG("%s",this->last_message.c_str());
}
break;
case iri_base_driver::OPENED: if(target_state == iri_base_driver::CLOSED)
{
transition="close";
ROS_DEBUG("Trying transition close from OPENED to CLOSED.");
this->preCloseHook();
if(this->closeDriver())
{
this->current_state_=iri_base_driver::CLOSED;
this->last_message="Transition close from OPENED to CLOSED succeeded.";
ROS_DEBUG("%s",this->last_message.c_str());
}
else
{
this->current_state_=iri_base_driver::OPENED;
this->last_message="Transition close from OPENED to CLOSED failed.";
ROS_DEBUG("%s",this->last_message.c_str());
error=true;
}
}
else if(target_state == iri_base_driver::RUNNING)
{
transition="start";
ROS_DEBUG("Trying transition start from OPENED to RUNNING.");
if(this->startDriver())
{
this->current_state_=iri_base_driver::RUNNING;
this->last_message="Transition start from OPENED to RUNNING succeeded.";
ROS_DEBUG("%s",this->last_message.c_str());
}
else
{
this->current_state_=iri_base_driver::OPENED;
this->last_message="Transition start from OPENED to RUNNING failed.";
ROS_DEBUG("%s",this->last_message.c_str());
error=true;
}
}
else
{
this->current_state_=iri_base_driver::OPENED;
this->last_message="No transition required";
ROS_DEBUG("%s",this->last_message.c_str());
}
break;
case RUNNING: if(target_state == iri_base_driver::OPENED || target_state == iri_base_driver::CLOSED)
{
transition="stop";
ROS_DEBUG("Trying transition stop from RUNNING to OPENED.");
if(this->stopDriver())
{
this->current_state_=iri_base_driver::OPENED;
this->last_message="Transition stop from RUNING to OPENED succeeded.";
ROS_DEBUG("%s",this->last_message.c_str());
}
else
{
this->current_state_=iri_base_driver::RUNNING;
this->last_message="Transition stop from RUNNING to OPENED failed.";
ROS_DEBUG("%s",this->last_message.c_str());
error=true;
}
}
else
{
this->current_state_=iri_base_driver::RUNNING;
this->last_message="No transition required";
ROS_DEBUG("%s",this->last_message.c_str());
}
break;
}
}while(!error && this->current_state_!=target_state);
}catch(...){
ROS_WARN("Caught exception in state transition %s\n",transition.c_str());
error=true;
}
pthread_mutex_unlock(&this->internal_access_);
return error;
} }
void IriBaseDriver::doStart(void) state_t IriBaseDriver::getState(void)
{ {
if(startDriver()) this->state_ = RUNNING; return this->current_state_;
} }
void IriBaseDriver::doStop(void) std::string IriBaseDriver::getStateName(void)
{ {
if(stopDriver()) this->state_ = OPENED; switch(this->current_state_)
{
case iri_base_driver::CLOSED: return std::string("closed");
case iri_base_driver::OPENED: return std::string("opened");
case iri_base_driver::RUNNING: return std::string("running");
default: return std::string("default value: not_possible");
}
} }
std::string IriBaseDriver::getID(void) std::string IriBaseDriver::getStatusMessage(void)
{
return this->last_message;
}
bool IriBaseDriver::isRunning(void)
{
pthread_mutex_lock(&this->internal_access_);
if(this->current_state_ == iri_base_driver::RUNNING)
{
pthread_mutex_unlock(&this->internal_access_);
return true;
}
else
{
pthread_mutex_unlock(&this->internal_access_);
return false;
}
}
bool IriBaseDriver::isOpened(void)
{
pthread_mutex_lock(&this->internal_access_);
if(this->current_state_ == iri_base_driver::OPENED)
{
pthread_mutex_unlock(&this->internal_access_);
return true;
}
else
{
pthread_mutex_unlock(&this->internal_access_);
return false;
}
}
bool IriBaseDriver::isClosed(void)
{
pthread_mutex_lock(&this->internal_access_);
if(this->current_state_ == iri_base_driver::CLOSED)
{
pthread_mutex_unlock(&this->internal_access_);
return true;
}
else
{
pthread_mutex_unlock(&this->internal_access_);
return false;
}
}
bool IriBaseDriver::isStopped(void)
{
pthread_mutex_lock(&this->internal_access_);
if(this->current_state_ == iri_base_driver::OPENED || this->current_state_ == iri_base_driver::CLOSED)
{
pthread_mutex_unlock(&this->internal_access_);
return true;
}
else
{
pthread_mutex_unlock(&this->internal_access_);
return false;
}
}
void IriBaseDriver::setPostOpenHook(boost::function< void() > function)
{
this->postOpenHook = function;
}
void IriBaseDriver::setPreCloseHook(boost::function< void() > function)
{
this->preCloseHook = function;
}
void IriBaseDriver::setPostStartHook(boost::function< void() > function)
{ {
return driver_id_; this->postStartHook = function;
} }
void IriBaseDriver::setPreCloseHook(hookFunction f) void IriBaseDriver::setPreStopHook(boost::function< void() > function)
{ {
preCloseHook = f; this->preStopHook = function;
} }
IriBaseDriver::~IriBaseDriver() IriBaseDriver::~IriBaseDriver()
{ {
pthread_mutex_destroy(&this->access_); pthread_mutex_destroy(&this->access_);
pthread_mutex_destroy(&this->internal_access_);
} }
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment