#include <cartesian_trajectory_controller.h>
Public Member Functions | |
CartesianTrajectoryController () | |
bool | checkMoving (simple_Jtranspose_controller::CheckMoving::Request &req, simple_Jtranspose_controller::CheckMoving::Response &res) |
bool | init (pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n) |
bool | moveTo (const geometry_msgs::PoseStamped &pose, const geometry_msgs::Twist &tolerance=geometry_msgs::Twist(), double duration=0) |
void | starting () |
void | update () |
~CartesianTrajectoryController () | |
Private Member Functions | |
void | command (const tf::MessageFilter< geometry_msgs::PoseStamped >::MConstPtr &pose_msg) |
KDL::Frame | getPose () |
bool | moveTo (simple_Jtranspose_controller::MoveToPose::Request &req, simple_Jtranspose_controller::MoveToPose::Response &resp) |
bool | preempt (std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) |
void | TransformToFrame (const tf::Transform &trans, KDL::Frame &frame) |
Private Attributes | |
pr2_mechanism_model::Chain | chain_ |
ros::ServiceServer | check_moving_srv_ |
std::string | controller_name_ |
bool | exceed_tolerance_ |
bool | is_moving_ |
KDL::JntArray | jnt_pos_ |
boost::scoped_ptr < KDL::ChainFkSolverPos > | jnt_to_pose_solver_ |
KDL::Chain | kdl_chain_ |
ros::Time | last_time_ |
double | max_duration_ |
std::vector < KDL::VelocityProfile_Trap > | motion_profile_ |
ros::ServiceServer | move_to_srv_ |
ros::NodeHandle | node_ |
KDL::Frame | pose_begin_ |
CartesianPoseController * | pose_controller_ |
KDL::Frame | pose_current_ |
KDL::Frame | pose_end_ |
ros::ServiceServer | preempt_srv_ |
bool | request_preempt_ |
pr2_mechanism_model::RobotState * | robot_state_ |
std::string | root_name_ |
tf::TransformListener | tf_ |
double | time_passed_ |
ros::Time | time_started_ |
KDL::Twist | tolerance_ |
KDL::Twist | twist_current_ |
Definition at line 44 of file cartesian_trajectory_controller.h.
controller::CartesianTrajectoryController::CartesianTrajectoryController | ( | ) |
controller::CartesianTrajectoryController::~CartesianTrajectoryController | ( | ) |
bool controller::CartesianTrajectoryController::checkMoving | ( | simple_Jtranspose_controller::CheckMoving::Request & | req, | |
simple_Jtranspose_controller::CheckMoving::Response & | res | |||
) |
void controller::CartesianTrajectoryController::command | ( | const tf::MessageFilter< geometry_msgs::PoseStamped >::MConstPtr & | pose_msg | ) | [private] |
KDL::Frame controller::CartesianTrajectoryController::getPose | ( | ) | [private] |
bool controller::CartesianTrajectoryController::init | ( | pr2_mechanism_model::RobotState * | robot_state, | |
ros::NodeHandle & | n | |||
) |
bool controller::CartesianTrajectoryController::moveTo | ( | simple_Jtranspose_controller::MoveToPose::Request & | req, | |
simple_Jtranspose_controller::MoveToPose::Response & | resp | |||
) | [private] |
bool controller::CartesianTrajectoryController::moveTo | ( | const geometry_msgs::PoseStamped & | pose, | |
const geometry_msgs::Twist & | tolerance = geometry_msgs::Twist() , |
|||
double | duration = 0 | |||
) |
bool controller::CartesianTrajectoryController::preempt | ( | std_srvs::Empty::Request & | req, | |
std_srvs::Empty::Response & | resp | |||
) | [private] |
void controller::CartesianTrajectoryController::starting | ( | ) |
void controller::CartesianTrajectoryController::TransformToFrame | ( | const tf::Transform & | trans, | |
KDL::Frame & | frame | |||
) | [private] |
void controller::CartesianTrajectoryController::update | ( | ) |
pr2_mechanism_model::Chain controller::CartesianTrajectoryController::chain_ [private] |
Definition at line 78 of file cartesian_trajectory_controller.h.
ros::ServiceServer controller::CartesianTrajectoryController::check_moving_srv_ [private] |
Definition at line 66 of file cartesian_trajectory_controller.h.
std::string controller::CartesianTrajectoryController::controller_name_ [private] |
Definition at line 68 of file cartesian_trajectory_controller.h.
bool controller::CartesianTrajectoryController::exceed_tolerance_ [private] |
Definition at line 71 of file cartesian_trajectory_controller.h.
bool controller::CartesianTrajectoryController::is_moving_ [private] |
Definition at line 71 of file cartesian_trajectory_controller.h.
KDL::JntArray controller::CartesianTrajectoryController::jnt_pos_ [private] |
Definition at line 83 of file cartesian_trajectory_controller.h.
boost::scoped_ptr<KDL::ChainFkSolverPos> controller::CartesianTrajectoryController::jnt_to_pose_solver_ [private] |
Definition at line 82 of file cartesian_trajectory_controller.h.
KDL::Chain controller::CartesianTrajectoryController::kdl_chain_ [private] |
Definition at line 81 of file cartesian_trajectory_controller.h.
ros::Time controller::CartesianTrajectoryController::last_time_ [private] |
Definition at line 69 of file cartesian_trajectory_controller.h.
double controller::CartesianTrajectoryController::max_duration_ [private] |
Definition at line 70 of file cartesian_trajectory_controller.h.
std::vector<KDL::VelocityProfile_Trap> controller::CartesianTrajectoryController::motion_profile_ [private] |
Definition at line 86 of file cartesian_trajectory_controller.h.
ros::ServiceServer controller::CartesianTrajectoryController::move_to_srv_ [private] |
Definition at line 66 of file cartesian_trajectory_controller.h.
ros::NodeHandle controller::CartesianTrajectoryController::node_ [private] |
Definition at line 65 of file cartesian_trajectory_controller.h.
KDL::Frame controller::CartesianTrajectoryController::pose_begin_ [private] |
Definition at line 73 of file cartesian_trajectory_controller.h.
CartesianPoseController* controller::CartesianTrajectoryController::pose_controller_ [private] |
Definition at line 89 of file cartesian_trajectory_controller.h.
KDL::Frame controller::CartesianTrajectoryController::pose_current_ [private] |
Definition at line 73 of file cartesian_trajectory_controller.h.
KDL::Frame controller::CartesianTrajectoryController::pose_end_ [private] |
Definition at line 73 of file cartesian_trajectory_controller.h.
ros::ServiceServer controller::CartesianTrajectoryController::preempt_srv_ [private] |
Definition at line 66 of file cartesian_trajectory_controller.h.
bool controller::CartesianTrajectoryController::request_preempt_ [private] |
Definition at line 71 of file cartesian_trajectory_controller.h.
pr2_mechanism_model::RobotState* controller::CartesianTrajectoryController::robot_state_ [private] |
Definition at line 77 of file cartesian_trajectory_controller.h.
std::string controller::CartesianTrajectoryController::root_name_ [private] |
Definition at line 95 of file cartesian_trajectory_controller.h.
tf::TransformListener controller::CartesianTrajectoryController::tf_ [private] |
Definition at line 91 of file cartesian_trajectory_controller.h.
double controller::CartesianTrajectoryController::time_passed_ [private] |
Definition at line 70 of file cartesian_trajectory_controller.h.
ros::Time controller::CartesianTrajectoryController::time_started_ [private] |
Definition at line 69 of file cartesian_trajectory_controller.h.
KDL::Twist controller::CartesianTrajectoryController::tolerance_ [private] |
Definition at line 74 of file cartesian_trajectory_controller.h.
KDL::Twist controller::CartesianTrajectoryController::twist_current_ [private] |
Definition at line 74 of file cartesian_trajectory_controller.h.