Commit 555707cc authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[tools] Add set_parameter and get_parameter to robot-utils

parent b3bc8bde
......@@ -93,8 +93,10 @@ struct SOT_CORE_EXPORT ForceUtil {
struct SOT_CORE_EXPORT FootUtil {
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream &os) const;
......@@ -212,8 +214,29 @@ public:
void display(std::ostream &os) const;
/**{ \name Handling general parameters */
/** \brief Set a parameter of type string.
If parameter_name already exists the value is overwritten.
If not it is inserted.
*/
void set_parameter(const std::string &parameter_name,
const std::string &parameter_value);
/** \brief Get a parameter of type string.
If parameter_name already exists the value is overwritten.
If not it is inserted.
@param parameter_name: Name of the parameter
Return false if the parameter is not found.
*/
const std::string &get_parameter(const std::string &parameter_name);
/** @} */
protected:
Logger logger_;
/** \brief Map of the parameters: map of strings. */
std::map<std::string, std::string> parameters_strings_;
}; // struct RobotUtil
/// Accessors - This should be changed to RobotUtilPtrShared
......
......@@ -360,10 +360,21 @@ void RobotUtil::display(std::ostream &os) const {
}
os << std::endl;
}
void RobotUtil::sendMsg(const std::string &msg, MsgType t,
const std::string &lineId) {
const std::string &lineId) {
logger_.sendMsg("[RobotUtil]" + msg, t, lineId);
}
void RobotUtil::set_parameter(const std::string &parameter_name,
const std::string &parameter_value) {
parameters_strings_[parameter_name] = parameter_value;
}
const std::string &RobotUtil::get_parameter(const std::string &parameter_name) {
return parameters_strings_[parameter_name];
}
bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
assert(q_sot.size() == 6);
assert(pos.size() == 3);
......@@ -442,6 +453,7 @@ bool isNameInRobotUtil(std::string &robotName) {
return true;
return false;
}
RobotUtilShrPtr createRobotUtil(std::string &robotName) {
std::map<std::string, RobotUtilShrPtr>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
......@@ -451,8 +463,6 @@ RobotUtilShrPtr createRobotUtil(std::string &robotName) {
it = sgl_map_name_to_robot_util.find(robotName);
return it->second;
}
std::cout << "Another robot is already in the map for " << robotName
<< std::endl;
return RefVoidRobotUtil();
}
} // namespace sot
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment