Skip to content
GitLab
Commit 017c9c7a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the first version of the documentation for all the ROS tools develeoped at IRI.

parent ec8b3cda
Hide whitespace changes
Inline Side-by-side
include/iri_ros_tools/module.h
View file @ 017c9c7a
......@@ -15,40 +15,241 @@
#define MODULE_DEFAULT_RATE 10
/**
* \brief Base class for all state machine modules
*
* This class may be used as a base class for state machine modules. It
* implements the following features:
* * an abstract function (state_machine()) that is called at a configurable
* rate to implement the actual state machine in inherited classes. This
* function must be implemented in the inherited classes. This function
* is called with the internal mutex locked.
* * A dynamic reconfigure server. The callback function associated to this
* server is abstract and must be implemnted in the inherited class. This
* class is a template on the dynamic reconfigure data structure.
* * An internal node handle is used to define the module namespace with the
* parameters provided at construction time (name and namespace).
*
* Any inherited class must call (preferably in the constructor) the start
* operation function in order to properly set up the internal operation of
* the class.
*/
template<class ModuleCfg>
class CModule
{
protected:
/* internal node handle */
/**
* \brief internal ROS node handle
*
* A ROS node handle that can be used by inherited classes to initialize
* ROS interfaces. This node handle is initialized at construction time
* as follows:
*
* /<node namespace>/<provided namespace>/<provided name>
*
*/
ros::NodeHandle module_nh;
private:
/**
* \brief name of the module
*
* Attribute with the name of the module, together with the full namespace.
* This name is used to report information, errors and warnings regarding
* the module.
*/
std::string name;
/**
* \brief rate in Hz of the internal thread
*
* This is the actual rate at which the state_machine() function will be
* called. Use the set_rate() and get_rate() function to modify and
* retrieve its value.
*/
ros::Rate module_rate;
/**
* \brief Internal mutex
*
* Mutex variables to ensure atomic access to internal attributes from the
* state machine and the ROS callback functions. It is internally handled
* and the lock() and unlock() functions must be used to access it.
*/
CMutex module_access;
/* reconfiguer attributes */
/**
* \brief Dynamic reconfigure server
*
* A dynamic reconfigure server which depends on the dynamic reconfigure
* data structure this class is template of. This server has the
* reconfigure_callback() callback function associated to it.
*/
dynamic_reconfigure::Server<ModuleCfg> dyn_reconf;
/* thread attributes */
/**
* \brief IRI thread server
*
* Pointer to the single instance of the thread server in the current
* process. It is used to handle the class thread.
*/
CThreadServer *thread_server;
/**
* \brief Name of the internal thread
*
* Name of the thread used to execute the state machine of the module.
* It is used to identify it inside the thread server.
*/
std::string module_thread_id;
/* event attributes */
/**
* \brief IRI event server
*
* Pointer to the single instance of the event server in the current
* process. It is used to handle the class events.
*/
CEventServer *event_server;
/**
* \brief Name of the event to terminate the internal thread
*
* Name of the event used to signal the termniation of the internal
* thread. It is used to identify it inside the event server. The
* activation of this event is monitored by the thread periodically.
*/
std::string finish_thread_event_id;
protected:
/**
* \brief Main thread function
*
* This is the main function of the internal thread. The thread itself is
* configured at contruction time, but it is not started until the
* start_operation() functions is called. This function monitors the
* internal event to know when it has to stop, and calls the state_machine()
* function at the desired rate.
*/
static void *module_thread(void *param);
/**
* \brief Function to implement the state machine
*
* This is an abstract function that must be implemented by any inherited class
* in order to be able to instantiate an object. This function must implement
* the desired state machine of the module without blocking at any time, to
* ensure a correct operation.
*
* This function is called with the internal mutex locked(), and it is called
* at the desired rate.
*
* \param void pointer pointing to the this pointer of the current object.
*
* \return this functions always return a NULL pointer.
*/
virtual void state_machine(void)=0;
/**
* \brief Dynamic reconfigure callback
*
* This abstarct function must be implemented by any inherited class in order
* to be able to instantiate an object. This function is called whenever a
* new dynamic reconfigure service call is received and must configure the
* module with the provided data.
*
* \param config reference to the dynamic reconfigure data structure with
* all the configuration data for the class.
* \param level integer representing the execution level at which the
* function has been called. In this case it has no meaning
* and can be ignored.
*/
virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
/**
* \brief function to start the operation of the class
*
* This function must be called in the inherited class constructor in order
* to start the internal thread and also start the dynamic reconfigure
* server.
*
* TODO: this function should disapear
*/
void start_operation(void);
/**
* \brief lock the mutex
*
* Function to lock the internal mutex of the class. Since the mutex is not
* public or protected, inherited classes must use this function to access
* it.
*/
void lock(void);
/**
* \brief unlock the mutex
*
* Function to unlock the internal mutex of the class. Since the mutex is not
* public or protected, inherited classes must use this function to access
* it.
*/
void unlock(void);
public:
/**
* \brief Constructor
*
* Constructor of the base class. This function initializes and configures all
* the internal attributes, but the object is not operating properly until the
* start_operation() funciton is called.
*
* \param name string with the desired name for the module.
*
* \param namespace string with the base namespace for the module.
*/
CModule(const std::string &name);
/**
* \brief Function to set the rate
*
* This function sets the desired rate of the internal thread which defines
* the frequency at which the state_machine() function is called.
*
* If the desired rate is not valid, a CModuleException is thrown.
*
* \param rate_hz the desired rate of the internal thread in Hertz. It must be
* any non-negative float value.
*/
void set_rate(double rate_hz);
/**
* \brief Function to get the rate
*
* This function returns the current value of the thread rate. By default this
* value is set to 10 Hz.
*
* \return the value of the current thread rate in hertz.
*/
double get_rate(void);
/**
* \brief Function to get the name of the module
*
* This function returns the full name of the module, with the whole namespace.
*
* \return the name of the module.
*/
std::string get_name(void);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,bool value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,int value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value);
/**
* \brief
*
*/
bool set_parameter(ros::ServiceClient &service,const std::string &name,double value);
/**
* \brief Destructor
*
* This function signals the internal thread to stop, waits until it finishes and
* the frees all the internal resources.
*/
virtual ~CModule();
};
......
include/iri_ros_tools/module_action.h
View file @ 017c9c7a
......@@ -17,32 +17,186 @@
#define DEFAULT_ACTION_TIMEOUT 10
#define DEFAULT_WATCHDOG_TIME 2
typedef enum {ACTION_IDLE,ACTION_RUNNING,ACTION_SUCCESS,ACTION_TIMEOUT,ACTION_FB_WATCHDOG,ACTION_ABORTED,ACTION_PREEMPTED} action_status;
/**
* \brief Possible action states returned by the get_state() function
*
* These are the possible states of the action associated to this class. Most
* of them represent actual states of the goal inside the action server, except
* for two:
* * ACTION_TIMEOUT: indicates the action did not finish in the alloed time
* * ACTION_WATCHDOG: indicate the action has not received feedback for a long
* time
*/
typedef enum {ACTION_IDLE,
ACTION_RUNNING,
ACTION_SUCCESS,
ACTION_TIMEOUT,
ACTION_FB_WATCHDOG,
ACTION_ABORTED,
ACTION_PREEMPTED} action_status;
/**
* \brief Action client wrapper
*
* This class wraps a simple ROS action client in order to simplify its use.
* This class is a template of any ROS action and provides the common features
* to start, cancel and monitor the current state of the goal. In addition, it
* provides the following features:
*
* * the maximum number of times the action can fail to start before reporting
* an error.
* * Timeout to control the maximum time the action can be active.
* * A watchdog to monitor the correct reception of the feedback of the action.
*
* All these new features can be configured using the class API.
*
* The action topics are created inside the namespace defined by the (optional)
* namespace and the actual name provided at contruction time as follows
*
* <main node namespace>/<class namespace>/<class name>
*/
template<class action_ros>
class CModuleAction
{
private:
/**
* \brief Internal Mutex
*
* This Mutex is used to ensure atomic access to the internal attributes
* of the class.
*/
CMutex action_access;
/**
* \brief name of the action object
*
* Attribute with the name of the action object, together with the full
* namespace. This name is used to report information, errors and
* warnings regarding the action object.
*/
std::string name;
/**
* \brief Status of the action
*
* This attribute holds the current action of the object. This can be any
* value of the action_status data type. This status is updated internally
* and the get_state() function should be used to access it.
*/
action_status status;
// number of retries attributes
/**
* \brief Number of tries to start the action
*
* This attribute holds the number of attempts to start the action. It is
* only used internally to handle possible start failures.
*/
unsigned int current_num_retries;
/**
* \brief maximum number of start attempts
*
* This attribute holds the maximum number of attempts to start the action.
* This value can be modified by the user with the set_max_num_retries()
* functions, and the actual value can be obtain with the get_max_num_retries()
* functions.
*/
unsigned int max_num_retries;
// ROS node handle
/**
* \brief internal ROS node handle
*
* A ROS node handle used to initialize the internal ROS action. This node
* handle is initialized at construction time as follows:
*
* /<node namespace>/<provided namespace>/<provided name>
*
*/
ros::NodeHandle nh;
// timeout attributes
/**
* \brief Use the timeout feature
*
* This attribute specifies whether the timeout feature is to be used to limit
* the amount of time the action is active or not. Its value is controlles by
* the enable_timeout() and disable_timeout() functions.
*
* To check whether the timeout is active or not, use the is_timeout_enabled()
* function.
*/
bool use_timeout;
/**
* \brief Timeout value in seconds
*
* This is the actual maximum time allowed to finish the action before reporting
* and error. This value can be set when starting a timeout with the start_timeout()
* function or when updating the value with the update_timeout() function.
*/
double timeout_value;
/**
* \brief Timeout object
*
* This is a timeout object actually used to handle the timeout feature of this
* class. See the documentation of this class for further details.
*/
CROSTimeout action_timeout;
// watchdog attributes
/**
* \brief Maximum time between feedback messages
*
* This attribute holds the maximum time in seconds allowed between two consecutive
* feedback messages. This value can be modified with the set_feedback_watchdog_time().
* To get the current value use the get_feedback_watchdog_time() function.
*/
double watchdog_time;
/**
* \brief Watchdog object
*
* This is the watchdog object actually used to handle the watchdog feature of this
* class. This feature is also anables. See the documentation of this class for
* further details.
*/
CROSWatchdog feedback_watchdog;
// action attributes
/**
* \brief ROS topic messages names
*
* This declaration defines all the ROS topis names of the provided action.
*/
ACTION_DEFINITION(action_ros);
/**
* \brief Action client object
*
* This points to the ROS action client object. See the documentation on this
* class for further details.
*/
actionlib::SimpleActionClient<action_ros> *action_client;
/**
* \brief Result message of the action
*
* This attribute holds the result message of the last executed action. Use
* the get_result() function to get it. This data structure is updated by
* the action_done() callback.
*/
Result action_result_msg;
/**
* \brief Feedback message of the action
*
* This attribute holds the last feedback message received of the current
* action. Use the get_feedback() function to get it. This data structure
* is updated by the action_feedback() callback.
*/
Feedback action_feedback_msg;
/**
* \brief Action done callback
*
* This callback is called whenever the current action finishes. It stores
* the user defined result data structure into the internal attribute and
* reports how the action ended, both using the rosout feature and the
* internal status attribute.
*
* \param state termination state of the current action. Defined by ROS.
*
* \param result action dependant result data structure received from the
* action server.
*/
void action_done(const actionlib::SimpleClientGoalState& state,const ResultConstPtr& result)
{
this->action_access.enter();
......@@ -58,13 +212,28 @@ class CModuleAction
}
else if(state==actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_DEBUG_STREAM("CModuleAction::action_done: goal on server " << this->name << " successfull");
ROS_INFO_STREAM("CModuleAction::action_done: goal on server " << this->name << " successfull");
this->status=ACTION_SUCCESS;
}
this->action_result_msg=*result;
this->action_access.exit();
}
/**
* \brief Action active callback
*
* This callback is called when the action becomes active, after it has been
* received by the server.
*/
void action_active(void);
/**
* \brief Action feedback callback
*
* This callback is called whenever new feedback data is received from the
* action server. This function updated the internal feedback data structure.
*
* \param feedback action dependant feedback data structure received from the
* action server.
*/
void action_feedback(const FeedbackConstPtr& feedback)
{
ROS_DEBUG_STREAM("CModuleAction::action_feedback: Got Feedback from server " << this->name << "!");
......@@ -74,20 +243,157 @@ class CModuleAction
this->action_access.exit();
}
public:
/**
* \brief Constructor
*
* Constructor of the action class. This function initializes and configures all
* the internal attributes, including the action client.
*
* \param name string with the desired name for the action.
*
* \param namespace string with the base namespace for the action.
*/
CModuleAction(const std::string &name,const std::string &name_space=std::string(""));
/**
* \brief Sets the maximum number of attempts to start the action
*
* This function sets the maximum number of attempts allowed to start the
* action before giving up. By default this value is set to 5.
*
* \param max_retries non-negative integer with the maximum number of attempts
* to start the action
*/
void set_max_num_retries(unsigned int max_retries);
/**
* \brief Gets the maximum number of attempts to start the action
*
* This function returns the maximum number of attempts allowed to start the
* action before giving up. By default this value is set to 5.
*
* \return non-negative integer with the maximum number of attempts to start
* the action
*/
unsigned int get_max_num_retries(void);
/**
* \brief Sets the feedback watchdog time
*
* This functions sets the maximum allowed time between two consecutive
* feedback messages before reporting an error. By default this value is
* set to 2.
*
* \param time_s the maximum allowed time in seconds between two consecutive
* feedback messages.
*/
void set_feedback_watchdog_time(double time_s);
/**
* \brief Gets the feedback watchdog time
*
* This functions returns the maximum allowed time between two consecutive
* feedback messages before reporting an error. By default this value is
* set to 2.
*
* \return the maximum allowed time in seconds between two consecutive
* feedback messages.
*/
double get_feedback_watchdog_time(void);
/**
* \brief Checks the state of the watchdog
*
* This function checks whether the internal watchdog has been activated
* because the feedback callback functions has not been called for longer
* than the allowed time.
*
* \return A boolean indicating whether the watchdog is active (true) or
* not (false)
*/
bool is_watchdog_active(void);
/**
* \brief Enables the action timeout
*
* This function enables the internal action timeout to limit the maximum
* time the action can be active without finishing properly. By default
* this feature is disabled.
*/
void enable_timeout(void);
/**
* \brief Disables the action timeout
*
* This function disables the internal action timeout to limit the maximum
* time the action can be active without finishing properly. By default
* this feature is disabled.
*/
void disable_timeout(void);
/**
* \brief
*
* TODO: do it automatically in the make_request function
*/
void start_timeout(double time_s);
/**
* \brief Updates the current timeout value
*
* This function updates the current timeout value, if it is enabled. The
* time already elapsed since the start of the action is not taken into
* account when updating the value, so the provided time will be the new
* timeout time.
*
* \param time_s the new timeout time in seconds to wait for the termination
* of the current action.
*/
void update_timeout(double time_s);
/**
* \brief Stops the timeout
*
* This functions stops the internal timeout, if it is enabled, so that no
* timeout error will be reported.
*/
void stop_timeout(void);
/**
* \brief Checks whether the timeout is enabled or not
*
* This functions checks whether the internal timeout value for the current
* action is enabled or not.
*
* \return A boolean indicating whether the timeout is enabled (true) or not
* (false)
*/
bool is_timeout_enabled(void);
/**
* \brief Checks whether the timeout is active or not
*
* This functions chekcs whether the maximum time allowed to complete the
*action has elapsed or not.
*
* \return A boolean indicating whether the timeout is active (true) or not
* (false)
*/
bool is_timeout_active(void);
/**
* \brief Function to get the name of the module
*
* This function returns the full name of the module, with the whole namespace.
*
* \return the name of the module.
*
*/
std::string get_name(void);
/**
* \brief start the action
*
* This function start the action with the provided goal information. This function
* first checks whether the action server is connected or not. In case it is not
* connected it will return reporting a pending condition util the maximum number of
* repetitions has been reached. In this case it will report an error.
*
* If the action server is connected, it sends all the goal information, resets the
* feeedback watchdog and enables the internal timeout in case it is enabled.
*
* \param msg action dependant goal data structure to be send to the action server.
*
* \return the status of the request. It can be any value of the act_srv_status
* data type.
*/
act_srv_status make_request(Goal &msg)
{
if(this->action_client->isServerConnected())
......@@ -120,17 +426,69 @@ class CModuleAction
}
}
/**
* \brief Cancels the current action
*
* This function cancels the current action. When this function is called,
* the action in the action server may still be active, so it is necessary
* to check the is_finished() function to actually know when the action
* has ended.
*/