matlab2016B robotics toolbox

/* Copyright 2015 The MathWorks, Inc. */ #ifndef _SLROS_GENERIC_PARAM_H_ #define _SLROS_GENERIC_PARAM_H_ #include <iostream> #include <string> #include <ros/console.h> #include <ros/ros.h> extern ros::NodeHandle * SLROSNodePtr; ///< The global node handle that is used by all ROS entities in the model // Namespace for parameter parsing code namespace param_parser { /** * Parse a scalar parameter value. This function is templatized by the * expected data type of the ROS parameter. * This function is needed, since the standard ROS C++ parameter parsing * does not strictly enforce data type consistency. * * @param[in] xmlValue The value of the parameter as XML-RPC data structure * @param[out] paramValue The value of the parameter as scalar data type * @retval TRUE if parameter with given type was parsed successfully. The * value of the parameter will be returned in @c paramValue. * @retval FALSE if parameter type did not match content in XML-RPC data structure */ template <class T> bool getScalar(const XmlRpc::XmlRpcValue& xmlValue, T& paramValue) { if (xmlValue.getType() != getXmlRpcType(paramValue)) return false; // Setting the output parameter value via overloaded conversion operator. // Since the operator is defined as non-const, using const_cast here to // avoid compiler warnings. // Since the conversion operator does not modify the xmlValue object, // the const_cast is safe. paramValue = const_cast<XmlRpc::XmlRpcValue&>(xmlValue); return true; } /** * Generic template function for getting enumerated XML-RPC data type. * See the specialized templates for handling specific data types. * * @param[in] paramValue The parameter value. The contents of the variable do * not matter, but its data type is crucial for calling the correct template * specialization. * * @return XML-RPC data type enumeration corresponding to the input parameter type */ template <class T> XmlRpc::XmlRpcValue::Type getXmlRpcType(const T& paramValue) { return XmlRpc::XmlRpcValue::TypeInvalid; } /** * Specialized template function for handling integer parameters. * * @param[in] paramValue The parameter value. * @return integer XML-RPC data type enumeration */ template <> inline XmlRpc::XmlRpcValue::Type getXmlRpcType<int>(const int& paramValue) { return XmlRpc::XmlRpcValue::TypeInt; } /** * Specialized template function for handling double parameters. * * @param[in] paramValue The parameter value. * @return double XML-RPC data type enumeration */ template <> inline XmlRpc::XmlRpcValue::Type getXmlRpcType<double>(const double& paramValue) { return XmlRpc::XmlRpcValue::TypeDouble; } /** * Specialized template function for handling boolean parameters. * * @param[in] paramValue The parameter value. * @return boolean XML-RPC data type enumeration */ template <> inline XmlRpc::XmlRpcValue::Type getXmlRpcType<bool>(const bool& paramValue) { return XmlRpc::XmlRpcValue::TypeBoolean; } /** * Specialized template function for handling string parameters. * * @param[in] paramValue The parameter value. * @return string XML-RPC data type enumeration */ template <> inline XmlRpc::XmlRpcValue::Type getXmlRpcType<std::string>(const std::string& paramValue) { return XmlRpc::XmlRpcValue::TypeString; } } /** * Base class for getting ROS parameters in C++. * * This class is used by derived classes used for handling scalar and array * parameter values. */ class SimulinkParameterGetterBase { public: void initialize(const std::string& pName); void initialize_error_codes(uint8_t codeSuccess, uint8_t codeNoParam, uint8_t codeTypeMismatch, uint8_t codeArrayTruncate); protected: ros::NodeHandle* nodePtr; ///< Pointer to node handle (node will be used to connect to parameter server) std::string paramName; ///< The name of the parameter bool hasValidValue; ///< Indicates if a valid value has been received yet. If TRUE, this last value will be stored in lastValidValue. uint8_t errorCodeSuccess; ///< Returned if parameter was retrieved successfully. uint8_t errorCodeNoParam; ///< Returned if parameter does not exist on server. uint8_t errorCodeTypeMismatch; ///< Returned if parameter data type did not match. uint8_t errorCodeArrayTruncate; ///< Returned if received array was truncated. }; /** * Class for getting scalar ROS parameters in C++. * * This class is used by code generated from the Simulink ROS * parameter blocks and is templatized by the expected C++ data type * for the parameter value. */ template <class CppParamType, class ROSCppParamType> class SimulinkParameterGetter : public SimulinkParameterGetterBase { public: void set_initial_value(const CppParamType initValue); uint8_t get_parameter(CppParamType* dataPtr); private: CppParamType initialValue; ///< The default value that should be returned by get_parameter if one of the error conditions occurs CppParamType lastValidValue; ///< The last valid value that was received from the parameter server uint8_t process_received_data(CppParamType* dataPtr, bool paramRetrieved); }; /** * Set initial value for returned parameter value. * * This initial value will be returned if the parameter does not exist or does not have the correct data type when the node is started. * @param[in] initValue The initial value. */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterGetter<CppParamType,ROSCppParamType>::set_initial_value(const CppParamType initValue) { initialValue = initValue; lastValidValue = initValue; } /** * Get the value for a named parameter from the parameter server. * @param[out] dataPtr Pointer to initialized data variable. The retrieved parameter value will be written to this location * @return Error code (=0 if value was read successfully, >0 if an error occurred) */ template <class CppParamType, class ROSCppParamType> uint8_t SimulinkParameterGetter<CppParamType,ROSCppParamType>::get_parameter(CppParamType* dataPtr) { XmlRpc::XmlRpcValue xmlValue; ROSCppParamType paramValue; bool paramRetrieved = false; // Get parameter as XmlRpcValue and then parse it through our own function if (nodePtr->getParam(paramName, xmlValue)) { paramRetrieved = param_parser::getScalar(xmlValue, paramValue); } // Cast the returned value into the data type that Simulink is expecting *dataPtr = static_cast<CppParamType>(paramValue); const uint8_t errorCode = process_received_data(dataPtr, paramRetrieved); return errorCode; } /** * Determine value and error code for retrieved parameter * @param[in,out] dataPtr Pointer to initialized data variable. The retrieved parameter value will be written to this location * @param[in] paramRetrieved Return value from ROS function for getting a parameter value * @return Error code (=0 if value was read successfully, >0 if an error occurred) */ template <class CppParamType, class ROSCppParamType> uint8_t SimulinkParameterGetter<CppParamType,ROSCppParamType>::process_received_data(CppParamType* dataPtr, bool paramRetrieved) { // By default, assume that parameter can be retrieved successfully uint8_t errorCode = errorCodeSuccess; if (!paramRetrieved) { // An error code of "errorCodeNoParam" means that the parameter does not exist and // "errorCodeTypeMismatch" means that it exists, but the data types don