#include <cartesian_twist.h>
Public Member Functions | |
CartesianTwistController () | |
void | command (const geometry_msgs::TwistStamped::ConstPtr &goal) |
Controller command. | |
virtual std::vector< std::string > | getClaimedNames () |
Get the names of joints/controllers which this controller exclusively claims. | |
virtual std::vector< std::string > | getCommandedNames () |
Get the names of joints/controllers which this controller commands. | |
virtual std::string | getType () |
Get the type of this controller. | |
virtual int | init (ros::NodeHandle &nh, ControllerManager *manager) |
Initialize the controller and any required data structures. | |
virtual bool | reset () |
Cleanly reset the controller to it's initial state. Some controllers may choose to stop themselves. This is mainly used in the case of the the robot exiting some fault condition. | |
virtual bool | start () |
Attempt to start the controller. This should be called only by the ControllerManager instance. | |
virtual bool | stop (bool force) |
Attempt to stop the controller. This should be called only by the ControllerManager instance. | |
virtual void | update (const ros::Time &now, const ros::Duration &dt) |
This is the update loop for the controller. | |
virtual | ~CartesianTwistController () |
Private Member Functions | |
KDL::Frame | getPose () |
Private Attributes | |
ros::Subscriber | command_sub_ |
bool | enabled_ |
ros::Publisher | feedback_pub_ |
boost::shared_ptr < KDL::ChainFkSolverPos_recursive > | fksolver_ |
bool | initialized_ |
bool | is_active_ |
std::vector< JointHandlePtr > | joints_ |
KDL::Chain | kdl_chain_ |
ros::Time | last_command_time_ |
KDL::JntArray | last_tgt_jnt_vel_ |
ControllerManager * | manager_ |
boost::mutex | mutex_ |
boost::shared_ptr < KDL::ChainIkSolverVel_wdls > | solver_ |
KDL::JntArray | tgt_jnt_pos_ |
KDL::JntArray | tgt_jnt_vel_ |
KDL::Twist | twist_command_ |
std::string | twist_command_frame_ |
Definition at line 71 of file cartesian_twist.h.
Definition at line 54 of file cartesian_twist.cpp.
virtual robot_controllers::CartesianTwistController::~CartesianTwistController | ( | ) | [inline, virtual] |
Definition at line 75 of file cartesian_twist.h.
void robot_controllers::CartesianTwistController::command | ( | const geometry_msgs::TwistStamped::ConstPtr & | goal | ) |
Controller command.
Definition at line 300 of file cartesian_twist.cpp.
std::vector< std::string > robot_controllers::CartesianTwistController::getClaimedNames | ( | ) | [virtual] |
Get the names of joints/controllers which this controller exclusively claims.
Implements robot_controllers::Controller.
Definition at line 360 of file cartesian_twist.cpp.
std::vector< std::string > robot_controllers::CartesianTwistController::getCommandedNames | ( | ) | [virtual] |
Get the names of joints/controllers which this controller commands.
Implements robot_controllers::Controller.
Definition at line 348 of file cartesian_twist.cpp.
virtual std::string robot_controllers::CartesianTwistController::getType | ( | ) | [inline, virtual] |
Get the type of this controller.
Reimplemented from robot_controllers::Controller.
Definition at line 118 of file cartesian_twist.h.
int robot_controllers::CartesianTwistController::init | ( | ros::NodeHandle & | nh, |
ControllerManager * | manager | ||
) | [virtual] |
Initialize the controller and any required data structures.
nh | Node handle for this controller. |
manager | The controller manager instance, this is needed for the controller to get information about joints, etc. |
Reimplemented from robot_controllers::Controller.
Definition at line 60 of file cartesian_twist.cpp.
bool robot_controllers::CartesianTwistController::reset | ( | ) | [virtual] |
Cleanly reset the controller to it's initial state. Some controllers may choose to stop themselves. This is mainly used in the case of the the robot exiting some fault condition.
Implements robot_controllers::Controller.
Definition at line 158 of file cartesian_twist.cpp.
bool robot_controllers::CartesianTwistController::start | ( | ) | [virtual] |
Attempt to start the controller. This should be called only by the ControllerManager instance.
Implements robot_controllers::Controller.
Definition at line 133 of file cartesian_twist.cpp.
bool robot_controllers::CartesianTwistController::stop | ( | bool | force | ) | [virtual] |
Attempt to stop the controller. This should be called only by the ControllerManager instance.
force | Should we force the controller to stop? Some controllers may wish to continue running until they absolutely have to stop. |
Implements robot_controllers::Controller.
Definition at line 152 of file cartesian_twist.cpp.
void robot_controllers::CartesianTwistController::update | ( | const ros::Time & | now, |
const ros::Duration & | dt | ||
) | [virtual] |
This is the update loop for the controller.
time | The system time. |
dt | The timestep since last call to update. |
Implements robot_controllers::Controller.
Definition at line 164 of file cartesian_twist.cpp.
Definition at line 148 of file cartesian_twist.h.
bool robot_controllers::CartesianTwistController::enabled_ [private] |
Definition at line 138 of file cartesian_twist.h.
Definition at line 147 of file cartesian_twist.h.
boost::shared_ptr<KDL::ChainFkSolverPos_recursive> robot_controllers::CartesianTwistController::fksolver_ [private] |
Definition at line 142 of file cartesian_twist.h.
bool robot_controllers::CartesianTwistController::initialized_ [private] |
Definition at line 135 of file cartesian_twist.h.
bool robot_controllers::CartesianTwistController::is_active_ [private] |
Definition at line 156 of file cartesian_twist.h.
std::vector<JointHandlePtr> robot_controllers::CartesianTwistController::joints_ [private] |
Definition at line 150 of file cartesian_twist.h.
Definition at line 140 of file cartesian_twist.h.
Definition at line 155 of file cartesian_twist.h.
Definition at line 145 of file cartesian_twist.h.
Definition at line 136 of file cartesian_twist.h.
boost::mutex robot_controllers::CartesianTwistController::mutex_ [private] |
Definition at line 152 of file cartesian_twist.h.
boost::shared_ptr<KDL::ChainIkSolverVel_wdls> robot_controllers::CartesianTwistController::solver_ [private] |
Definition at line 141 of file cartesian_twist.h.
Definition at line 143 of file cartesian_twist.h.
Definition at line 144 of file cartesian_twist.h.
Definition at line 153 of file cartesian_twist.h.
std::string robot_controllers::CartesianTwistController::twist_command_frame_ [private] |
Definition at line 154 of file cartesian_twist.h.