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
00037 #ifndef TRAJECTORY_FILTER_SERVER_H_
00038 #define TRAJECTORY_FILTER_SERVER_H_
00039
00040 #include <ros/ros.h>
00041 #include <filters/filter_chain.h>
00042 #include <motion_planning_msgs/FilterJointTrajectory.h>
00043 #include <motion_planning_msgs/FilterJointTrajectoryWithConstraints.h>
00044 #include <urdf/model.h>
00045
00046 namespace trajectory_filter_server
00047 {
00048
00049 static const int FILTER_JOINT_TRAJECTORY = 0;
00050 static const int FILTER_JOINT_TRAJECTORY_WITH_CONSTRAINTS = 1;
00051
00052 class TrajectoryFilterServer
00053 {
00054
00055 public:
00056 TrajectoryFilterServer();
00057 virtual ~TrajectoryFilterServer();
00058 bool init();
00059
00060 private:
00061 bool filter(motion_planning_msgs::FilterJointTrajectory::Request &req,
00062 motion_planning_msgs::FilterJointTrajectory::Response &resp);
00063
00064 bool filterConstraints(motion_planning_msgs::FilterJointTrajectoryWithConstraints::Request &req,
00065 motion_planning_msgs::FilterJointTrajectoryWithConstraints::Response &resp);
00066
00067
00068
00069
00070 void getLimits(const trajectory_msgs::JointTrajectory& trajectory,
00071 std::vector<motion_planning_msgs::JointLimits>& limits_out);
00072
00073 ros::NodeHandle private_handle_, root_handle_;
00074 ros::ServiceServer filter_service_, filter_constraints_service_;
00075 std::map<std::string, motion_planning_msgs::JointLimits> joint_limits_;
00076
00077 int service_type_;
00078
00079 filters::FilterChain<motion_planning_msgs::FilterJointTrajectory::Request> filter_chain_;
00080 filters::FilterChain<motion_planning_msgs::FilterJointTrajectoryWithConstraints::Request> filter_constraints_chain_;
00081
00082 urdf::Model urdf_model_;
00083
00084 bool loadURDF();
00085 bool use_safety_limits_;
00086
00087 };
00088
00089 }
00090
00091 #endif