54 Controller::init(nh, manager);
57 std::string mimic, controlled;
58 nh.
param<std::string>(
"mimic_joint", mimic,
"torso_lift_joint");
59 nh.
param<std::string>(
"controlled_joint", controlled,
"bellows_joint");
62 nh.
param<
double>(
"mimic_scale", scale_, 1.0);
68 nh.
param(
"autostart", autostart,
false);
75 bool ScaledMimicController::start()
80 "Unable to start, not initialized.");
86 bool ScaledMimicController::stop(
bool force)
92 bool ScaledMimicController::reset()
103 joint_to_control_->setPosition(scale_*joint_to_mimic_->getPosition(), 0, 0);
106 std::vector<std::string> ScaledMimicController::getCommandedNames()
108 std::vector<std::string> names;
109 names.push_back(joint_to_control_->getName());
113 std::vector<std::string> ScaledMimicController::getClaimedNames()
116 return getCommandedNames();
Controller for a having one joint be a (scaled) mimic of another. Primary design use case is making a...
std::string getName(void *handle)
virtual int requestStart(const std::string &name)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
#define ROS_ERROR_NAMED(name,...)
JointHandlePtr getJointHandle(const std::string &name)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)