/* 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't match errorCode = nodePtr->hasParam(paramName) ? errorCodeTypeMismatch : errorCodeNoParam; } if (errorCode == errorCodeSuccess) { // Remember last valid value lastValidValue = *dataPtr; hasValidValue = true; } else { // An error occurred. If a valid value was previously // received, return it. Otherwise, return the // initial value. if (hasValidValue) *dataPtr = lastValidValue; else *dataPtr = initialValue; } return errorCode; } /** * Class for getting array ROS parameters in C++. * * This class is used by code generated from the Simulink ROS * parameter blocks. * Note that the ROSCppParamType template parameter needs to refer to a container * type that supports the following operations: * - resize * - std::copy * std::string (used for string parameters) and std::vector (used for numeric arrays) * fall into this category. */ template <class CppParamType, class ROSCppParamType> class SimulinkParameterArrayGetter : public SimulinkParameterGetterBase { public: void set_initial_value(const CppParamType* initValue, const uint32_t lengthToWrite); uint8_t get_parameter(const uint32_t maxLength, CppParamType* dataPtr, uint32_t* receivedLength); private: ROSCppParamType initialValue; ///< The default value that should be returned by get_parameter if one of the error conditions occurs ROSCppParamType lastValidValue; ///< The last valid value that was received from the parameter server uint8_t process_received_data(const ROSCppParamType& retrievedValue, const uint32_t maxLength, bool paramRetrieved, CppParamType* dataPtr, uint32_t* receivedLength); }; /** * 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. * @param[in] lengthToWrite The number of elements in the @c initValue array. Since the array is passed as a pointer, the @c lengthToWrite argument is required to indicate how many elements the array has. */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterArrayGetter<CppParamType,ROSCppParamType>::set_initial_value(const CppParamType* desiredInitialValue, const uint32_t lengthToWrite) { initialValue.resize(lengthToWrite); // Store the initial value in a member variable. Note that std::copy will work on any // iterable array, e.g. std::string or std::vector std::copy(desiredInitialValue, desiredInitialValue + lengthToWrite, initialValue.begin()); } /** * Get the value for a named parameter from the parameter server. * @param[in] maxLength The maximum length of the returned array (in elements). The array in @c dataPtr will have this many elements. * @param[out] dataPtr Pointer to initialized data array. The retrieved parameter value will be written to this location * @param[out] receivedLength The actual number of array elements that was received. This value will be <= than @c maxLength. * @return Error code (=0 if value was read successfully, >0 if an error occurred) */ template <class CppParamType, class ROSCppParamType> uint8_t SimulinkParameterArrayGetter<CppParamType,ROSCppParamType>::get_parameter( const uint32_t maxLength, CppParamType* dataPtr, uint32_t* receivedLength) { uint8_t errorCode = errorCodeSuccess; // Ensure that getParam is called with correct data type signature // This works for strings without any custom data type checking, but probably does not scale // to numeric arrays. See the data type checking code for scalars in SimulinkParameterGetter::get_parameter // as an example. ROSCppParamType paramValue; bool paramRetrieved = nodePtr->getParam(paramName, paramValue); errorCode = process_received_data(paramValue, maxLength, paramRetrieved, dataPtr, receivedLength); return errorCode; } /** * Determine value and error code for retrieved parameter * @param[in] retrievedValue Retrieved parameter value as ROS C++ data type * @param[in] maxLength The maximum length of the returned array (in elements). The array in @c dataPtr will have this many elements. * @param[in] paramRetrieved Return value from ROS function for getting a parameter value * @param[out] dataPtr Pointer to Simulink data array. The retrieved parameter value will be written to this location * @param[out] receivedLength The actual number of array elements that was received. This value will be <= than @c maxLength. * @return Error code (=0 if value was read successfully, >0 if an error occurred) */ template <class CppParamType, class ROSCppParamType> uint8_t SimulinkParameterArrayGetter<CppParamType,ROSCppParamType>::process_received_data( const ROSCppParamType& retrievedValue, const uint32_t maxLength, bool paramRetrieved, CppParamType* dataPtr, uint32_t* receivedLength) { // 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't match errorCode = nodePtr->hasParam(paramName) ? errorCodeTypeMismatch : errorCodeNoParam; } if (errorCode == errorCodeSuccess) { // Indicate truncation if received array has more elements than maxLength if (retrievedValue.size() > maxLength) errorCode = errorCodeArrayTruncate; // Copy the received data into the Simulink variable location // We don't need to do zero-padding here, because GetParameterArrayState::codegenStepImpl // initializes dataPtr to all zeros. uint32_t copyLength = std::min(maxLength, static_cast<uint32_t>(retrievedValue.size())); ROS_ASSERT(copyLength <= maxLength); std::copy(retrievedValue.begin(), retrievedValue.begin() + copyLength, dataPtr); *receivedLength = copyLength; // Remember last valid value lastValidValue.resize(copyLength); std::copy(retrievedValue.begin(), retrievedValue.begin() + copyLength, lastValidValue.begin()); hasValidValue = true; } else { // An error occurred. If a valid value was previously // received, return it. Otherwise, return the // initial value. if (hasValidValue) { // lastValidValue is truncated when it is saved, so there is no possibility // of a buffer overrun in dataPtr here ROS_ASSERT(lastValidValue.size() <= maxLength); std::copy(lastValidValue.begin(), lastValidValue.begin() + lastValidValue.size(), dataPtr); *receivedLength = uint32_t(lastValidValue.size()); } else { // initialValue is truncated in GetParameterArrayState::setupSetInitialValue, // so there is no possibility of a buffer overrun in dataPtr here ROS_ASSERT(initialValue.size() <= maxLength); std::copy(initialValue.begin(), initialValue.begin() + initialValue.size(), dataPtr); *receivedLength = uint32_t(initialValue.size()); } } return errorCode; } /** * Class for setting 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 SimulinkParameterSetter { public: void initialize(const std::string& pName); void set_parameter(const CppParamType& value); void set_parameter_array(const CppParamType* value, const uint32_t maxLength, const uint32_t lengthToWrite); void length_error(const std::string& modelName, const uint32_t lengthToWrite, const uint32_t arrayLength); private: ros::NodeHandle* nodePtr; ///< Pointer to node handle (node will be used to connect to parameter server) std::string paramName; ///< The name of the parameter }; /** * Initialize the class. * @param[in] pName The name of the ROS parameter */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterSetter<CppParamType,ROSCppParamType>::initialize(const std::string& pName) { nodePtr = SLROSNodePtr; paramName = pName; } /** * Set the value of a named scalar parameter. * @param[in] value The value that should be set. */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterSetter<CppParamType,ROSCppParamType>::set_parameter(const CppParamType& value) { // Cast from Simulink data type to data type that ROS expects ROSCppParamType paramValue = static_cast<ROSCppParamType>(value); nodePtr->setParam(paramName, paramValue); } /** * Set the value of a named array parameter. * @param[in] value The array that should be set. * @param[in] maxLength The maximum length that can be written. * @param[in] lengthToWrite The number of elements in the @c value array that * should be written. The value of @c lengthToWrite needs to be less than or * equal to the value of @c maxLength. */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterSetter<CppParamType,ROSCppParamType>::set_parameter_array(const CppParamType* value, const uint32_t maxLength, const uint32_t lengthToWrite) { // ROSCppParamType is either a std::vector or a std::string // The constructors of std::vector and std::string allow us to give a pointer // and the number of elements, so make use of that. // The validation that lengthToWrite <= than the length of the array should // occur in the calling code. ROS_ASSERT(lengthToWrite <= maxLength); ROSCppParamType paramValue(value, lengthToWrite); nodePtr->setParam(paramName, paramValue); } /** * Log a length error via rosout. * * This error occurs when the user-specified length to write is bigger than the number of elements in the user-provided array. * @param[in] modelName Name of the Simulink model in which the error occurred * @param[in] lengthToWrite The number of elements that should be written. * @param[in] arrayLength The number of elements that the array actually contains. */ template <class CppParamType, class ROSCppParamType> void SimulinkParameterSetter<CppParamType,ROSCppParamType>::length_error(const std::string& modelName, const uint32_t lengthToWrite, const uint32_t arrayLength) { ROS_ERROR_NAMED(modelName, "Error setting parameter '%s'. The number of array elements to write, %d, is larger than the length of the input array, %d.", paramName.c_str(), lengthToWrite, arrayLength); } #endif