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
......@@ -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();
};
......
......@@ -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.
*/