00001 #include <pr2_controller_interface/controller.h> 00002 #include <pr2_mechanism_model/joint.h> 00003 #include <pr2_mechanism_model/chain.h> 00004 #include <ros/ros.h> 00005 #include <pr2_mechanism_msgs/LoadController.h> 00006 #include <realtime_tools/realtime_publisher.h> 00007 #include <boost/scoped_ptr.hpp> 00008 #include <sensor_msgs/JointState.h> 00009 #include <kdl/chain.hpp> 00010 00011 00012 namespace my_controller_ns{ 00013 00014 class MyControllerClass: public pr2_controller_interface::Controller 00015 { 00016 private: 00017 bool serviceCallback(pr2_mechanism_msgs::LoadController::Request& req, 00018 pr2_mechanism_msgs::LoadController::Response& resp); 00019 00020 ros::ServiceServer srv_; 00021 boost::scoped_ptr<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > pub_; 00022 pr2_mechanism_model::RobotState *robot_; 00023 pr2_mechanism_model::JointState *joint_state_; 00024 pr2_mechanism_model::Chain chain_; 00025 KDL::Chain kdl_chain_; 00026 ros::Time time_of_last_cycle_; 00027 double max_effort_; 00028 unsigned int counter_; 00029 00030 public: 00031 bool init(pr2_mechanism_model::RobotState *robot, 00032 ros::NodeHandle &n); 00033 void starting(); 00034 void update(); 00035 void stopping(); 00036 }; 00037 }