Go to the documentation of this file.
69 #ifndef CARTESIAN_POSE_CONTROLLER_H
70 #define CARTESIAN_POSE_CONTROLLER_H
73 #include <boost/scoped_ptr.hpp>
77 #include <geometry_msgs/PoseStamped.h>
78 #include <geometry_msgs/Twist.h>
81 #include <kdl/chainfksolver.hpp>
82 #include <kdl/chain.hpp>
83 #include <kdl/chainjnttojacsolver.hpp>
84 #include <kdl/frames.hpp>
106 void command(
const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
boost::scoped_ptr< tf::MessageFilter< geometry_msgs::PoseStamped > > command_filter_
CartesianPoseController()
tf::TransformListener tf_
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_command_
std::string controller_name_
pr2_mechanism_model::RobotState * robot_state_
pr2_mechanism_model::Chain chain_
~CartesianPoseController()
boost::scoped_ptr< KDL::ChainFkSolverPos > jnt_to_pose_solver_
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > > state_pose_publisher_
boost::scoped_ptr< KDL::ChainJntToJacSolver > jac_solver_
std::vector< control_toolbox::Pid > pid_controller_
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::Twist > > state_error_publisher_
void command(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)