Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef ARM_KINEMATICS_CONSTRAINT_AWARE_H
00039 #define ARM_KINEMATICS_CONSTRAINT_AWARE_H
00040
00041
00042 #include <boost/shared_ptr.hpp>
00043
00044
00045 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00046 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00047 #include <kinematics_msgs/GetPositionFK.h>
00048
00049 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00050 #include <arm_navigation_msgs/DisplayTrajectory.h>
00051 #include <arm_navigation_msgs/LinkPadding.h>
00052
00053
00054 #include <planning_environment/models/collision_models_interface.h>
00055 #include <arm_kinematics_constraint_aware/arm_kinematics_constraint_aware_utils.h>
00056 #include <kdl/jntarray.hpp>
00057 #include <angles/angles.h>
00058 #include <urdf/model.h>
00059
00060
00061 #include <pluginlib/class_loader.h>
00062 #include <kinematics_base/kinematics_base.h>
00063 #include <arm_kinematics_constraint_aware/arm_kinematics_solver_constraint_aware.h>
00064
00065
00066 namespace arm_kinematics_constraint_aware
00067 {
00068 class ArmKinematicsConstraintAware
00069 {
00070 public:
00071
00082 ArmKinematicsConstraintAware();
00083
00084 virtual ~ArmKinematicsConstraintAware()
00085 {
00086 if (collision_models_interface_)
00087 delete collision_models_interface_;
00088 };
00089
00100 bool getConstraintAwarePositionIK(kinematics_msgs::GetConstraintAwarePositionIK::Request &request,
00101 kinematics_msgs::GetConstraintAwarePositionIK::Response &response);
00102
00103 bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00104 kinematics_msgs::GetPositionIK::Response &response);
00105
00106 bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00107 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00108
00109 bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00110 kinematics_msgs::GetKinematicSolverInfo::Response &response);
00111
00112 bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00113 kinematics_msgs::GetPositionFK::Response &response);
00114
00115 bool isActive(){ return active_;}
00116 private:
00117
00118 pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00119 arm_kinematics_constraint_aware::ArmKinematicsSolverConstraintAware* solver_;
00120 bool active_;
00121
00122 ros::NodeHandle node_handle_,root_handle_;
00123 ros::ServiceServer ik_collision_service_, ik_service_, fk_service_, ik_solver_info_service_, fk_solver_info_service_;
00124 planning_environment::CollisionModelsInterface *collision_models_interface_;
00125 std::string group_;
00126 ros::Publisher vis_marker_publisher_;
00127 ros::Publisher vis_marker_array_publisher_;
00128 void printStringVec(const std::string &prefix, const std::vector<std::string> &string_vector);
00129 ros::Publisher display_trajectory_publisher_;
00130 bool visualize_solution_;
00131
00132 void advertiseBaseKinematicsServices();
00133 void advertiseConstraintIKService();
00134
00135 bool isReady(arm_navigation_msgs::ArmNavigationErrorCodes &error_code);
00136 void sendEndEffectorPose(const planning_models::KinematicState* state, bool valid);
00137
00138 kinematics_msgs::KinematicSolverInfo chain_info_;
00139
00140 };
00141 }
00142 #endif