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