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
00037 #include <trajectory_filter_server/trajectory_filter_server.h>
00038 #include <pluginlib/class_loader.h>
00039
00040 namespace trajectory_filter_server
00041 {
00042 TrajectoryFilterServer::TrajectoryFilterServer() : private_handle_("~"),
00043 filter_chain_("arm_navigation_msgs::FilterJointTrajectory"),
00044 filter_constraints_chain_("arm_navigation_msgs::FilterJointTrajectoryWithConstraints")
00045 {
00046 std::string service_type_string;
00047 private_handle_.param<std::string>("service_type",service_type_string,"FilterJointTrajectory");
00048 private_handle_.param<bool>("use_safety_limits",use_safety_limits_,true);
00049
00050 ros::WallDuration(5.0).sleep();
00051
00052 if(service_type_string == std::string("FilterJointTrajectory"))
00053 service_type_ = FILTER_JOINT_TRAJECTORY;
00054 else if(service_type_string == std::string("FilterJointTrajectoryWithConstraints"))
00055 {
00056 ROS_INFO("Filtering joint trajectories with constraints");
00057 service_type_ = FILTER_JOINT_TRAJECTORY_WITH_CONSTRAINTS;
00058 }
00059 else
00060 service_type_ = FILTER_JOINT_TRAJECTORY;
00061
00062 }
00063
00064 TrajectoryFilterServer::~TrajectoryFilterServer()
00065 {
00066 }
00067
00068 bool TrajectoryFilterServer::init()
00069 {
00070 bool print = false;
00071 if(service_type_ == FILTER_JOINT_TRAJECTORY) {
00072 if (!filter_chain_.configure("filter_chain",private_handle_)) {
00073 pluginlib::ClassLoader<arm_navigation_msgs::FilterJointTrajectory> con_loader("filters",
00074 "filters::FilterBase<arm_navigation_msgs::FilterJointTrajectory>");
00075 std::vector<std::string> available = con_loader.getDeclaredClasses();
00076 for(unsigned int i = 0; i < available.size(); i++) {
00077 ROS_INFO_STREAM("Available class name " << available[i]);
00078 }
00079 return false;
00080 }
00081 } else {
00082 if (!filter_constraints_chain_.configure("filter_chain",private_handle_)) {
00083 pluginlib::ClassLoader<arm_navigation_msgs::FilterJointTrajectoryWithConstraints> con_loader("filters",
00084 "filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>");
00085 std::vector<std::string> available = con_loader.getDeclaredClasses();
00086 for(unsigned int i = 0; i < available.size(); i++) {
00087 ROS_INFO_STREAM("Available class name " << available[i]);
00088 }
00089 return false;
00090 }
00091 }
00092
00093 if(!loadURDF())
00094 return false;
00095
00096 if(service_type_ == FILTER_JOINT_TRAJECTORY)
00097 filter_service_ = private_handle_.advertiseService("filter_trajectory", &TrajectoryFilterServer::filter, this);
00098 if(service_type_ == FILTER_JOINT_TRAJECTORY_WITH_CONSTRAINTS)
00099 filter_constraints_service_ = private_handle_.advertiseService("filter_trajectory_with_constraints", &TrajectoryFilterServer::filterConstraints, this);
00100 return true;
00101 }
00102
00103 bool TrajectoryFilterServer::filter(arm_navigation_msgs::FilterJointTrajectory::Request &req,
00104 arm_navigation_msgs::FilterJointTrajectory::Response &resp)
00105 {
00106 ROS_DEBUG("TrajectoryFilter::Got trajectory with %d points and %d joints",
00107 (int)req.trajectory.points.size(),
00108 (int)req.trajectory.joint_names.size());
00109 getLimits(req.trajectory,req.limits);
00110 arm_navigation_msgs::FilterJointTrajectory orig_request;
00111 orig_request.request = req;
00112 arm_navigation_msgs::FilterJointTrajectory chain_response;
00113 if (!filter_chain_.update(orig_request,chain_response))
00114 {
00115 ROS_WARN("Filter chain failed to process trajectory");
00116 resp.error_code.val = chain_response.response.error_code.val;
00117 return true;
00118 }
00119 resp.trajectory = chain_response.request.trajectory;
00120 resp.error_code.val = resp.error_code.SUCCESS;
00121 return true;
00122 }
00123
00124 bool TrajectoryFilterServer::filterConstraints(arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request &req,
00125 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response &resp)
00126 {
00127 ROS_DEBUG("TrajectoryFilter::Got trajectory with %d points and %d joints",
00128 (int)req.trajectory.points.size(),
00129 (int)req.trajectory.joint_names.size());
00130 arm_navigation_msgs::FilterJointTrajectoryWithConstraints filter_response;
00131 getLimits(req.trajectory,req.limits);
00132 arm_navigation_msgs::FilterJointTrajectoryWithConstraints orig_request;
00133 orig_request.request = req;
00134 if (!filter_constraints_chain_.update(orig_request,filter_response))
00135 {
00136 ROS_WARN("Filter chain failed to process trajectory");
00137 resp.error_code.val = filter_response.response.error_code.val;
00138 return true;
00139 }
00140 resp.trajectory = filter_response.request.trajectory;
00141 resp.error_code.val = resp.error_code.SUCCESS;
00142 return true;
00143 }
00144
00145 void TrajectoryFilterServer::getLimits(const trajectory_msgs::JointTrajectory& trajectory,
00146 std::vector<arm_navigation_msgs::JointLimits>& limits_out)
00147 {
00148 int num_joints = trajectory.joint_names.size();
00149 limits_out.resize(num_joints);
00150 for (int i=0; i<num_joints; ++i)
00151 {
00152 std::map<std::string, arm_navigation_msgs::JointLimits>::const_iterator limit_it = joint_limits_.find(trajectory.joint_names[i]);
00153 arm_navigation_msgs::JointLimits limits;
00154
00155 if (limit_it == joint_limits_.end())
00156 {
00157 limits.joint_name = trajectory.joint_names[i];
00158 limits.has_position_limits = false;
00159 limits.has_velocity_limits = false;
00160 limits.has_acceleration_limits = false;
00161
00162 std::map<std::string, boost::shared_ptr<urdf::Joint> >::const_iterator urdf_joint_iterator = urdf_model_.joints_.find(trajectory.joint_names[i]);
00163 if(urdf_joint_iterator != urdf_model_.joints_.end())
00164 {
00165 boost::shared_ptr<urdf::Joint> urdf_joint = urdf_joint_iterator->second;
00166 if(use_safety_limits_ && urdf_joint->safety)
00167 {
00168 limits.min_position = urdf_joint->safety->soft_lower_limit;
00169 limits.max_position = urdf_joint->safety->soft_upper_limit;
00170 }
00171 else
00172 {
00173 limits.min_position = urdf_joint->limits->lower;
00174 limits.max_position = urdf_joint->limits->upper;
00175 }
00176 limits.max_velocity = urdf_joint->limits->velocity;
00177 limits.has_velocity_limits = true;
00178
00179 if(urdf_joint->type == urdf::Joint::CONTINUOUS)
00180 limits.has_position_limits = false;
00181 else
00182 limits.has_position_limits = true;
00183
00184 limits.has_acceleration_limits = false;
00185 }
00186
00187
00188 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/min_position",limits.min_position);
00189 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/max_position",limits.max_position);
00190 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/max_velocity",limits.max_velocity);
00191 private_handle_.param("joint_limits/"+trajectory.joint_names[i]+"/max_acceleration",
00192 limits.max_acceleration,
00193 std::numeric_limits<double>::max());
00194 bool boolean;
00195 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_position_limits", boolean))
00196 limits.has_position_limits = boolean?1:0;
00197 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_velocity_limits", boolean))
00198 limits.has_velocity_limits = boolean?1:0;
00199 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_acceleration_limits", boolean))
00200 limits.has_acceleration_limits = boolean?1:0;
00201
00202 joint_limits_.insert(make_pair(trajectory.joint_names[i], limits));
00203 }
00204 else
00205 {
00206 limits = limit_it->second;
00207 }
00208 limits_out[i] = limits;
00209 }
00210 }
00211
00212 bool TrajectoryFilterServer::loadURDF()
00213 {
00214 std::string robot_desc_string;
00215 root_handle_.param("robot_description", robot_desc_string, std::string());
00216 if (!urdf_model_.initString(robot_desc_string)){
00217 ROS_ERROR("Failed to parse urdf string");
00218 return false;
00219 }
00220 ROS_INFO("Successfully parsed urdf file");
00221 return true;
00222 }
00223
00224 }
00225
00226 int main(int argc, char** argv)
00227 {
00228 ros::init(argc, argv, "trajectory_filter_server");
00229
00230 ros::AsyncSpinner spinner(1);
00231 spinner.start();
00232
00233 trajectory_filter_server::TrajectoryFilterServer traj_filter_server;
00234 if(traj_filter_server.init())
00235 {
00236 ROS_INFO("Started trajectory filter server");
00237 ros::waitForShutdown();
00238 }
00239 else
00240 {
00241 ROS_ERROR("Could not start trajectory filter server");
00242 ROS_ERROR("Check your filters.yaml file to make sure it is configured correctly");
00243 ROS_ERROR("Also check the ROS parameter: service_type in your launch file");
00244 ROS_ERROR("The message type in the service request must match the type that the filters are acting on");
00245 ros::shutdown();
00246 }
00247 return 0;
00248 }