18 #ifndef COB_FRAME_TRACKER_H 19 #define COB_FRAME_TRACKER_H 27 #include <std_msgs/String.h> 28 #include <std_msgs/Float64.h> 29 #include <std_msgs/Float64MultiArray.h> 30 #include <sensor_msgs/JointState.h> 31 #include <geometry_msgs/Twist.h> 32 #include <std_srvs/Trigger.h> 33 #include <cob_srvs/SetString.h> 36 #include <cob_frame_tracker/FrameTrackerConfig.h> 37 #include <cob_frame_tracker/FrameTrackingAction.h> 43 #include <kdl/chainiksolvervel_pinv.hpp> 44 #include <kdl/chainfksolvervel_recursive.hpp> 45 #include <kdl/frames.hpp> 46 #include <kdl/jntarray.hpp> 47 #include <kdl/jntarrayvel.hpp> 50 #include <dynamic_reconfigure/server.h> 51 #include <dynamic_reconfigure/Reconfigure.h> 52 #include <boost/thread/mutex.hpp> 74 jntToCartSolver_vel_.reset();
76 reconfigure_server_.reset();
82 void jointstateCallback(
const sensor_msgs::JointState::ConstPtr& msg);
84 bool startTrackingCallback(cob_srvs::SetString::Request& request, cob_srvs::SetString::Response& response);
85 bool startLookatCallback(cob_srvs::SetString::Request& request, cob_srvs::SetString::Response& response);
86 bool stopCallback(std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response);
88 bool getTransform(
const std::string& from,
const std::string& to,
tf::StampedTransform& stamped_tf);
90 void publishZeroTwist();
91 void publishTwist(
ros::Duration period,
bool do_publish =
true);
97 void action_success();
158 void reconfigureCallback(cob_frame_tracker::FrameTrackerConfig& config, uint32_t level);
162 bool checkInfinitesimalTwist(
const KDL::Twist current);
163 bool checkCartDistanceViolation(
const double dist,
const double rot);
166 int checkServiceCallStatus();
std::string chain_tip_link_
std::string chain_base_link_
double twist_dead_threshold_rot_
ros::Time tracking_start_time_
KDL::JntArray last_q_dot_
actionlib::SimpleActionServer< cob_frame_tracker::FrameTrackingAction > SAS_FrameTrackingAction_t
ROSCONSOLE_DECL void initialize()
std::string tracking_frame_
boost::shared_ptr< KDL::ChainFkSolverVel_recursive > jntToCartSolver_vel_
ros::ServiceServer start_tracking_server_
double tracking_duration_
boost::shared_ptr< dynamic_reconfigure::Server< cob_frame_tracker::FrameTrackerConfig > > reconfigure_server_
ros::ServiceServer stop_server_
control_toolbox::Pid pid_controller_rot_z_
cob_frame_tracker::FrameTrackingResult action_result_
double twist_dead_threshold_lin_
double twist_deviation_threshold_lin_
KDL::Twist current_twist_
boost::recursive_mutex reconfig_mutex_
double twist_deviation_threshold_rot_
unsigned int abortion_counter_
control_toolbox::Pid pid_controller_trans_z_
control_toolbox::Pid pid_controller_rot_y_
cob_frame_tracker::FrameTrackingFeedback action_feedback_
tf::TransformListener tf_listener_
tf::StampedTransform transform_tf
boost::shared_ptr< SAS_FrameTrackingAction_t > as_
KDL::Chain chain_
KDL Conversion.
std::string target_frame_
std::string lookat_focus_frame_
std::vector< std::string > joints_
double cart_min_dist_threshold_lin_
ros::Publisher error_pub_
control_toolbox::Pid pid_controller_trans_y_
unsigned int max_abortions_
control_toolbox::Pid pid_controller_trans_x_
ros::Subscriber jointstate_sub_
ros::ServiceClient reconfigure_client_
bool enable_abortion_checking_
void run(ClassLoader *loader)
control_toolbox::Pid pid_controller_rot_x_
std::string action_name_
Action interface.
ros::ServiceServer start_lookat_server_
ros::Publisher twist_pub_
double cart_min_dist_threshold_rot_