#include <cob_twist_controller.h>
Definition at line 51 of file cob_twist_controller.h.
CobTwistController::CobTwistController | ( | ) | [inline] |
Definition at line 84 of file cob_twist_controller.h.
CobTwistController::~CobTwistController | ( | ) | [inline] |
Definition at line 88 of file cob_twist_controller.h.
void CobTwistController::checkSolverAndConstraints | ( | cob_twist_controller::TwistControllerConfig & | config | ) |
Definition at line 293 of file cob_twist_controller.cpp.
bool CobTwistController::initialize | ( | ) |
parse robot_description and generate KDL chains
parse robot_description and set velocity limits
initialize configuration control solver
Setting up dynamic_reconfigure server for the TwistControlerConfig parameters
initialize variables and current joint values and velocities
give tf_listener some time to fill tf-cache
initialize ROS interfaces
publisher for visualizing current twist direction
Definition at line 33 of file cob_twist_controller.cpp.
void CobTwistController::jointstateCallback | ( | const sensor_msgs::JointState::ConstPtr & | msg | ) |
Definition at line 560 of file cob_twist_controller.cpp.
void CobTwistController::odometryCallback | ( | const nav_msgs::Odometry::ConstPtr & | msg | ) |
Definition at line 589 of file cob_twist_controller.cpp.
void CobTwistController::reconfigureCallback | ( | cob_twist_controller::TwistControllerConfig & | config, |
uint32_t | level | ||
) |
Definition at line 228 of file cob_twist_controller.cpp.
Definition at line 198 of file cob_twist_controller.cpp.
void CobTwistController::solveTwist | ( | KDL::Twist | twist | ) |
Orientation of twist is with respect to chain_base coordinate system.
Definition at line 440 of file cob_twist_controller.cpp.
void CobTwistController::twistCallback | ( | const geometry_msgs::Twist::ConstPtr & | msg | ) |
Orientation of twist_msg is with respect to chain_base coordinate system.
Definition at line 432 of file cob_twist_controller.cpp.
void CobTwistController::twistStampedCallback | ( | const geometry_msgs::TwistStamped::ConstPtr & | msg | ) |
Orientation of twist_stamped_msg is with respect to coordinate system given in header.frame_id.
Definition at line 409 of file cob_twist_controller.cpp.
void CobTwistController::visualizeTwist | ( | KDL::Twist | twist | ) |
calculate rotation between twist-axis and z-axis of cylinder
Definition at line 471 of file cob_twist_controller.cpp.
Definition at line 79 of file cob_twist_controller.h.
KDL::Chain CobTwistController::chain_ [private] |
Definition at line 68 of file cob_twist_controller.h.
boost::shared_ptr<cob_twist_controller::ControllerInterfaceBase> CobTwistController::controller_interface_ [private] |
Definition at line 76 of file cob_twist_controller.h.
boost::shared_ptr<pluginlib::ClassLoader<cob_twist_controller::ControllerInterfaceBase> > CobTwistController::interface_loader_ [private] |
Definition at line 77 of file cob_twist_controller.h.
boost::shared_ptr<KDL::ChainFkSolverVel_recursive> CobTwistController::jntToCartSolver_vel_ [private] |
Definition at line 74 of file cob_twist_controller.h.
JointStates CobTwistController::joint_states_ [private] |
Definition at line 69 of file cob_twist_controller.h.
Definition at line 56 of file cob_twist_controller.h.
ros::NodeHandle CobTwistController::nh_ [private] |
Definition at line 54 of file cob_twist_controller.h.
Definition at line 66 of file cob_twist_controller.h.
Definition at line 61 of file cob_twist_controller.h.
boost::shared_ptr<InverseDifferentialKinematicsSolver> CobTwistController::p_inv_diff_kin_solver_ [private] |
Definition at line 75 of file cob_twist_controller.h.
boost::recursive_mutex CobTwistController::reconfig_mutex_ |
Definition at line 111 of file cob_twist_controller.h.
boost::shared_ptr< dynamic_reconfigure::Server<cob_twist_controller::TwistControllerConfig> > CobTwistController::reconfigure_server_ |
Definition at line 112 of file cob_twist_controller.h.
Definition at line 65 of file cob_twist_controller.h.
Definition at line 81 of file cob_twist_controller.h.
Definition at line 72 of file cob_twist_controller.h.
Definition at line 63 of file cob_twist_controller.h.
Definition at line 70 of file cob_twist_controller.h.
Definition at line 59 of file cob_twist_controller.h.
Definition at line 58 of file cob_twist_controller.h.