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_("arm_navigation_msgs::FilterJointTrajectory::Request"),
00043 filter_constraints_chain_("arm_navigation_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 }
00060
00061 TrajectoryFilterServer::~TrajectoryFilterServer()
00062 {
00063 }
00064
00065 bool TrajectoryFilterServer::init()
00066 {
00067 if(service_type_ == FILTER_JOINT_TRAJECTORY)
00068 if (!filter_chain_.configure("filter_chain",private_handle_))
00069 return false;
00070 if(service_type_ == FILTER_JOINT_TRAJECTORY_WITH_CONSTRAINTS)
00071 if (!filter_constraints_chain_.configure("filter_chain",private_handle_))
00072 return false;
00073
00074 if(!loadURDF())
00075 return false;
00076
00077 if(service_type_ == FILTER_JOINT_TRAJECTORY)
00078 filter_service_ = private_handle_.advertiseService("filter_trajectory", &TrajectoryFilterServer::filter, this);
00079 if(service_type_ == FILTER_JOINT_TRAJECTORY_WITH_CONSTRAINTS)
00080 filter_constraints_service_ = private_handle_.advertiseService("filter_trajectory_with_constraints", &TrajectoryFilterServer::filterConstraints, this);
00081 return true;
00082 }
00083
00084 bool TrajectoryFilterServer::filter(arm_navigation_msgs::FilterJointTrajectory::Request &req,
00085 arm_navigation_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 arm_navigation_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(arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request &req,
00103 arm_navigation_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 arm_navigation_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<arm_navigation_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, arm_navigation_msgs::JointLimits>::const_iterator limit_it = joint_limits_.find(trajectory.joint_names[i]);
00128 arm_navigation_msgs::JointLimits limits;
00129
00130 if (limit_it == joint_limits_.end())
00131 {
00132 limits.joint_name = trajectory.joint_names[i];
00133 limits.has_position_limits = false;
00134 limits.has_velocity_limits = false;
00135 limits.has_acceleration_limits = false;
00136
00137 std::map<std::string, boost::shared_ptr<urdf::Joint> >::const_iterator urdf_joint_iterator = urdf_model_.joints_.find(trajectory.joint_names[i]);
00138 if(urdf_joint_iterator != urdf_model_.joints_.end())
00139 {
00140 boost::shared_ptr<urdf::Joint> urdf_joint = urdf_joint_iterator->second;
00141 if(use_safety_limits_ && urdf_joint->safety)
00142 {
00143 limits.min_position = urdf_joint->safety->soft_lower_limit;
00144 limits.max_position = urdf_joint->safety->soft_upper_limit;
00145 }
00146 else
00147 {
00148 limits.min_position = urdf_joint->limits->lower;
00149 limits.max_position = urdf_joint->limits->upper;
00150 }
00151 limits.max_velocity = urdf_joint->limits->velocity;
00152 limits.has_velocity_limits = true;
00153
00154 if(urdf_joint->type == urdf::Joint::CONTINUOUS)
00155 limits.has_position_limits = false;
00156 else
00157 limits.has_position_limits = true;
00158
00159 limits.has_acceleration_limits = false;
00160 }
00161
00162
00163 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/min_position",limits.min_position);
00164 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/max_position",limits.max_position);
00165 private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/max_velocity",limits.max_velocity);
00166 private_handle_.param("joint_limits/"+trajectory.joint_names[i]+"/max_acceleration",
00167 limits.max_acceleration,
00168 std::numeric_limits<double>::max());
00169 bool boolean;
00170 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_position_limits", boolean))
00171 limits.has_position_limits = boolean?1:0;
00172 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_velocity_limits", boolean))
00173 limits.has_velocity_limits = boolean?1:0;
00174 if(private_handle_.getParam("joint_limits/"+trajectory.joint_names[i]+"/has_acceleration_limits", boolean))
00175 limits.has_acceleration_limits = boolean?1:0;
00176
00177 joint_limits_.insert(make_pair(trajectory.joint_names[i], limits));
00178 }
00179 else
00180 {
00181 limits = limit_it->second;
00182 }
00183 limits_out[i] = limits;
00184 }
00185 }
00186
00187 bool TrajectoryFilterServer::loadURDF()
00188 {
00189 std::string robot_desc_string;
00190 root_handle_.param("robot_description", robot_desc_string, std::string());
00191 if (!urdf_model_.initString(robot_desc_string)){
00192 ROS_ERROR("Failed to parse urdf string");
00193 return false;
00194 }
00195 ROS_INFO("Successfully parsed urdf file");
00196 return true;
00197 }
00198
00199 }
00200
00201 int main(int argc, char** argv)
00202 {
00203 ros::init(argc, argv, "trajectory_filter_server");
00204
00205 ros::AsyncSpinner spinner(1);
00206 spinner.start();
00207
00208 trajectory_filter_server::TrajectoryFilterServer traj_filter_server;
00209 if(traj_filter_server.init())
00210 {
00211 ROS_INFO("Started trajectory filter server");
00212 ros::waitForShutdown();
00213 }
00214 else
00215 {
00216 ROS_ERROR("Could not start trajectory filter server");
00217 ROS_ERROR("Check your filters.yaml file to make sure it is configured correctly");
00218 ROS_ERROR("Also check the ROS parameter: service_type in your launch file");
00219 ROS_ERROR("The message type in the service request must match the type that the filters are acting on");
00220 ros::shutdown();
00221 }
00222 return 0;
00223 }
