93 int mode = boost::lexical_cast<
int>(str_mode);
112 if(config.restore_defaults)
115 config.restore_defaults =
false;
132 int sign = boost::lexical_cast<
int>(str_mdir) ? -1 : 1;
138 int stall = boost::lexical_cast<
int>(str_stall);
144 unsigned int tmp = boost::lexical_cast<
unsigned int>(str_alim);
145 double alim = ((double) tmp) / 10.0;
152 int max_forward = boost::lexical_cast<
unsigned int>(str_max_fw);
159 int max_reverse = boost::lexical_cast<
unsigned int>(str_max_re);
166 unsigned int rpm_motor = boost::lexical_cast<
unsigned int>(str_rpm_motor);
168 double max_rpm = ((double) rpm_motor) / ratio;
176 unsigned int rpm_acceleration_motor = boost::lexical_cast<
unsigned int>(str_rpm_acceleration_motor);
178 double rpm_acceleration = ((double) rpm_acceleration_motor) / ratio;
185 unsigned int rpm_deceleration_motor = boost::lexical_cast<
unsigned int>(str_rpm_deceleration_motor);
187 double rpm_deceleration = ((double) rpm_deceleration_motor) / ratio;
191 }
catch (std::bad_cast& e)
193 ROS_WARN_STREAM(
"Failure parsing feedback data. Dropping message." << e.what());
209 if(config.restore_defaults)
212 config.restore_defaults =
false;
217 if(config.load_roboteq)
220 config.load_roboteq =
false;
234 unsigned int rpm_motor = boost::lexical_cast<
unsigned int>(str_rpm_motor);
236 config.max_speed = ((double) rpm_motor) / config.ratio;
241 unsigned int rpm_acceleration_motor = boost::lexical_cast<
unsigned int>(str_rpm_acceleration_motor);
243 config.max_acceleration = ((double) rpm_acceleration_motor) / config.ratio;
248 unsigned int rpm_deceleration_motor = boost::lexical_cast<
unsigned int>(str_rpm_deceleration_motor);
250 config.max_deceleration = ((double) rpm_deceleration_motor) / config.ratio;
256 int direction = (config.rotation == -1) ? 1 : 0;
269 int alim = config.amper_limit * 10;
289 long int max_speed_motor = config.ratio * config.max_speed;
297 long int max_acceleration_motor = config.ratio * config.max_acceleration;
304 long int max_deceleration_motor = config.ratio * config.max_deceleration;
bool setParam(string msg, string params="")
void reconfigureCBParam(roboteq_control::RoboteqParameterConfig &config, uint32_t level)
reconfigureCBParam when the dynamic reconfigurator change some values start this method ...
void setOperativeMode(int type)
setOperativeMode Reference in page 321 - MMOD
void reconfigureCBPIDtype(roboteq_control::RoboteqPIDtypeConfig &config, uint32_t level)
reconfigureCBEncoder when the dynamic reconfigurator change some values start this method ...
void initConfigurator(bool load_from_board)
initConfigurator Initialize all parameter and syncronize parameters between ros and roboteq board ...
roboteq_control::RoboteqParameterConfig default_param_config
unsigned int mNumber
Number motor.
MotorParamConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::string name, unsigned int number)
MotorParamConfigurator.
roboteq::serial_controller * mSerial
Serial port.
void getParamFromRoboteq()
getParamFromRoboteq Load parameters from Roboteq board
dynamic_reconfigure::Server< roboteq_control::RoboteqPIDtypeConfig > * ds_pid_type
Dynamic reconfigure PID.
const std::string & getNamespace() const
string mName
Associate name space.
#define ROS_WARN_STREAM(args)
dynamic_reconfigure::Server< roboteq_control::RoboteqParameterConfig > * ds_param
Dynamic reconfigure parameters.
roboteq_control::RoboteqPIDtypeConfig _last_pid_type_config
string getParam(string msg, string params="")
bool getParam(const std::string &key, std::string &s) const
roboteq_control::RoboteqParameterConfig _last_param_config
bool hasParam(const std::string &key) const
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
bool setup_param
Setup variable.
roboteq_control::RoboteqPIDtypeConfig default_pid_type_config
ros::NodeHandle nh_
Private namespace.