466 lines
18 KiB
C++
466 lines
18 KiB
C++
/* 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
|