diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h index 3e76acb9..5591d337 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h @@ -39,22 +39,25 @@ #include #include -namespace industrial_trajectory_filters { +namespace industrial_trajectory_filters +{ /** * @brief Message Adapter structure serves as a stand-in for the arm navigation message type with * which the filter was specialized. * */ -struct MessageAdapter { - - /** - * @brief Arm navigation message type for trajectory filter - * - */ - struct Request { - trajectory_msgs::JointTrajectory trajectory; - } request; +struct MessageAdapter +{ + + /** + * @brief Arm navigation message type for trajectory filter + * + */ + struct Request + { + trajectory_msgs::JointTrajectory trajectory; + } request; }; /** @@ -117,151 +120,152 @@ struct MessageAdapter { * should override this method whenever a custom adapter structure is used. */ template -class FilterBase: public planning_request_adapter::PlanningRequestAdapter { -public: - - /** - * @brief Default constructor - */ - FilterBase() : - planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_( - false), filter_type_("FilterBase"), filter_name_( - "Unimplemented") { - - } - - /** - * Default destructor - */ - virtual ~FilterBase() { - } - ; - - /** - * @brief Update the filter and return the data separately. This function - * must be implemented in the derived class. - * - * @param data_in A reference to the data to be input to the filter - * @param data_out A reference to the data output location - * @return true on success, otherwise false. - */ - virtual bool update(const T& data_in, T& data_out)=0; - - /** - * @brief Original FilterBase method, return filter type - * @return filter type (as string) - */ - std::string getType() { - return filter_type_; - } - ; - - /** - * @brief Original FitlerBase Method - * @return filter name (as string). - */ - inline const std::string& getName() { - return filter_name_; - } - ; // original FilterBase method - -protected: - - /** - * @brief FilterBase method for the sub class to configure the filter - * This function must be implemented in the derived class. - * @return true if successful, otherwise false. - */ - virtual bool configure()=0; - - /** - * @brief filter name - */ - std::string filter_name_; - - /** - * The type of the filter (Used by FilterChain for Factory construction) - */ - std::string filter_type_; - - /** - * @brief Holds filter configuration state. - */ - bool configured_; - - /** - * @brief Internal node handle (used for parameter lookup) - */ - ros::NodeHandle nh_; - -protected: - - /** - * @brief Moveit Planning Request Adapter method. This basic implementation of the - * adaptAndPlan method calls the planner and then maps the trajectory data from the - * MotionPlanResponse object into the MessageAdapter mapping structure. The - * MessageAdapter object is then passed to the "update" method that resembles that - * from the old FilterBase interface class. The filtered trajectory is finally - * saved in the MotionPlanResponse object. - */ - virtual bool adaptAndPlan(const PlannerFn &planner, - const planning_scene::PlanningSceneConstPtr &planning_scene, - const planning_interface::MotionPlanRequest &req, - planning_interface::MotionPlanResponse &res, - std::vector &added_path_index) const { - - // non const pointer to this - FilterBase *p = - const_cast*>(this); - - // calling the configure method - if (!configured_ && p->configure()) { - p->configured_ = true; - } - - // moveit messages for saving trajectory data - moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out; - MessageAdapter trajectory_in, trajectory_out; // mapping structures - - // calling planner first - bool result = planner(planning_scene, req, res); - - // applying filter to planned trajectory - if (result && res.trajectory_) { - // mapping arguments into message adapter struct - res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in); - trajectory_in.request.trajectory = - robot_trajectory_in.joint_trajectory; - - // applying arm navigation filter to planned trajectory - p->update(trajectory_in, trajectory_out); - - // saving filtered trajectory into moveit message. - robot_trajectory_out.joint_trajectory = - trajectory_out.request.trajectory; - res.trajectory_->setRobotTrajectoryMsg( - planning_scene->getCurrentState(), robot_trajectory_out); - - } - - return result; - } - - /** - * @brief Return description string - * @return description (as a string) - */ - virtual std::string getDescription() const { - // non const pointer to this - FilterBase *p = - const_cast*>(this); - - std::stringstream ss; - ss << "Trajectory filter '" << p->getName() << "' of type '" - << p->getType() << "'"; - return ss.str(); - } -}; + class FilterBase : public planning_request_adapter::PlanningRequestAdapter + { + public: + + /** + * @brief Default constructor + */ + FilterBase() : + planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_( + "Unimplemented") + { + + } + + /** + * Default destructor + */ + virtual ~FilterBase() + { + } + ; + + /** + * @brief Update the filter and return the data separately. This function + * must be implemented in the derived class. + * + * @param data_in A reference to the data to be input to the filter + * @param data_out A reference to the data output location + * @return true on success, otherwise false. + */ + virtual bool update(const T& data_in, T& data_out)=0; + + /** + * @brief Original FilterBase method, return filter type + * @return filter type (as string) + */ + std::string getType() + { + return filter_type_; + } + ; + + /** + * @brief Original FitlerBase Method + * @return filter name (as string). + */ + inline const std::string& getName() + { + return filter_name_; + } + ; // original FilterBase method + + protected: + + /** + * @brief FilterBase method for the sub class to configure the filter + * This function must be implemented in the derived class. + * @return true if successful, otherwise false. + */ + virtual bool configure()=0; + + /** + * @brief filter name + */ + std::string filter_name_; + + /** + * The type of the filter (Used by FilterChain for Factory construction) + */ + std::string filter_type_; + + /** + * @brief Holds filter configuration state. + */ + bool configured_; + + /** + * @brief Internal node handle (used for parameter lookup) + */ + ros::NodeHandle nh_; + + protected: + + /** + * @brief Moveit Planning Request Adapter method. This basic implementation of the + * adaptAndPlan method calls the planner and then maps the trajectory data from the + * MotionPlanResponse object into the MessageAdapter mapping structure. The + * MessageAdapter object is then passed to the "update" method that resembles that + * from the old FilterBase interface class. The filtered trajectory is finally + * saved in the MotionPlanResponse object. + */ + virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, + const planning_interface::MotionPlanRequest &req, + planning_interface::MotionPlanResponse &res, + std::vector &added_path_index) const + { + + // non const pointer to this + FilterBase *p = const_cast*>(this); + + // calling the configure method + if (!configured_ && p->configure()) + { + p->configured_ = true; + } + + // moveit messages for saving trajectory data + moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out; + MessageAdapter trajectory_in, trajectory_out; // mapping structures + + // calling planner first + bool result = planner(planning_scene, req, res); + + // applying filter to planned trajectory + if (result && res.trajectory_) + { + // mapping arguments into message adapter struct + res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in); + trajectory_in.request.trajectory = robot_trajectory_in.joint_trajectory; + + // applying arm navigation filter to planned trajectory + p->update(trajectory_in, trajectory_out); + + // saving filtered trajectory into moveit message. + robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory; + res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out); + + } + + return result; + } + + /** + * @brief Return description string + * @return description (as a string) + */ + virtual std::string getDescription() const + { + // non const pointer to this + FilterBase *p = const_cast*>(this); + + std::stringstream ss; + ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'"; + return ss.str(); + } + }; } diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h index ec53c8cd..612d6597 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h @@ -34,48 +34,50 @@ #include -namespace industrial_trajectory_filters { +namespace industrial_trajectory_filters +{ /** * @brief This is a simple filter which reduces a trajectory to N points or less */ template -class NPointFilter: public industrial_trajectory_filters::FilterBase { -public: - /** - * @brief Default constructor - */ - NPointFilter(); - /** - * @brief Default destructor - */ - ~NPointFilter(); - ; + class NPointFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + NPointFilter(); + /** + * @brief Default destructor + */ + ~NPointFilter(); + ; - virtual bool configure(); + virtual bool configure(); - // \brief Reduces a trajectory to N points or less. The resulting trajectory - // contains only point within the original trajectory (no interpolation is done - // between points). + // \brief Reduces a trajectory to N points or less. The resulting trajectory + // contains only point within the original trajectory (no interpolation is done + // between points). - /** - * \brief Reduces a trajectory to N points or less. The resulting trajectory - * contains only point within the original trajectory (no interpolation is done - * between points). - * @param trajectory_in input trajectory - * @param trajectory_out filtered trajectory (N points or less - * @return true if successful - */ - bool update(const T& trajectory_in, T& trajectory_out); + /** + * \brief Reduces a trajectory to N points or less. The resulting trajectory + * contains only point within the original trajectory (no interpolation is done + * between points). + * @param trajectory_in input trajectory + * @param trajectory_out filtered trajectory (N points or less + * @return true if successful + */ + bool update(const T& trajectory_in, T& trajectory_out); -private: - /** - * @brief number of points to reduce trajectory to - */ - int n_points_; + private: + /** + * @brief number of points to reduce trajectory to + */ + int n_points_; -}; + }; /** * @brief Specializing trajectory filter implementation diff --git a/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h index c3c33054..9469c066 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h @@ -42,7 +42,8 @@ #include */ -namespace industrial_trajectory_filters { +namespace industrial_trajectory_filters +{ /** * \brief This is a simple filter which performs a uniforming sampling of @@ -51,49 +52,49 @@ namespace industrial_trajectory_filters { */ template -class UniformSampleFilter: public industrial_trajectory_filters::FilterBase { -public: - /** - * @brief Default constructor - */ - UniformSampleFilter(); - /** - * @brief Default destructor - */ - ~UniformSampleFilter(); + class UniformSampleFilter : public industrial_trajectory_filters::FilterBase + { + public: + /** + * @brief Default constructor + */ + UniformSampleFilter(); + /** + * @brief Default destructor + */ + ~UniformSampleFilter(); - virtual bool configure(); + virtual bool configure(); - /** - * Uniformly samples(in terms of time) the input trajectory based upon the - * sample duration. Sampling is performed via interpolation ensuring smooth - * velocity and acceleration. NOTE: For this reason trajectories must be - * fully defined before using this filter. - * @param trajectory_in non uniform trajectory - * @param trajectory_out uniform(in terms of time) trajectory - * @return - */ - bool update(const T& trajectory_in, T& trajectory_out); + /** + * Uniformly samples(in terms of time) the input trajectory based upon the + * sample duration. Sampling is performed via interpolation ensuring smooth + * velocity and acceleration. NOTE: For this reason trajectories must be + * fully defined before using this filter. + * @param trajectory_in non uniform trajectory + * @param trajectory_out uniform(in terms of time) trajectory + * @return + */ + bool update(const T& trajectory_in, T& trajectory_out); - /** - * @brief Perform interpolation between p1 and p2. Time from start must be - * in between p1 and p2 times. - * @param p1 prior trajectory point - * @param p2 subsequent trajectory point - * @param time_from_start time from start of trajectory (i.e. p0). - * @param interp_pt resulting interpolated point - * @return true if successful, otherwise false. - */ - bool interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, - trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start, - trajectory_msgs::JointTrajectoryPoint & interp_pt); + /** + * @brief Perform interpolation between p1 and p2. Time from start must be + * in between p1 and p2 times. + * @param p1 prior trajectory point + * @param p2 subsequent trajectory point + * @param time_from_start time from start of trajectory (i.e. p0). + * @param interp_pt resulting interpolated point + * @return true if successful, otherwise false. + */ + bool interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, trajectory_msgs::JointTrajectoryPoint & p2, + double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); -private: - /** - * @brief uniform sample duration (sec) - */ - double sample_duration_; -}; + private: + /** + * @brief uniform sample duration (sec) + */ + double sample_duration_; + }; /** * @brief Specializing trajectory filter implementation diff --git a/industrial_trajectory_filters/src/n_point_filter.cpp b/industrial_trajectory_filters/src/n_point_filter.cpp index 0f7c8977..41d86dbe 100644 --- a/industrial_trajectory_filters/src/n_point_filter.cpp +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -38,89 +38,93 @@ using namespace industrial_trajectory_filters; const int DEFAULT_N = 2; template -NPointFilter::NPointFilter() : - FilterBase() { - ROS_INFO_STREAM("Constructing N point filter"); - n_points_ = DEFAULT_N; - this->filter_name_ = "NPointFilter"; - this->filter_type_ = "NPointFilter"; -} + NPointFilter::NPointFilter() : + FilterBase() + { + ROS_INFO_STREAM("Constructing N point filter"); + n_points_ = DEFAULT_N; + this->filter_name_ = "NPointFilter"; + this->filter_type_ = "NPointFilter"; + } template -NPointFilter::~NPointFilter() { -} + NPointFilter::~NPointFilter() + { + } template -bool NPointFilter::configure() { - //if (!filters::FilterBase::getParam("n_points", n_points_)) - if (!this->nh_.getParam("n_points", n_points_)) { - ROS_WARN_STREAM("NPointFilter, params has no attribute n_points."); - } - if (n_points_ < 2) { - ROS_WARN_STREAM( - "n_points attribute less than min(2), setting to minimum"); - n_points_ = 2; - } - ROS_INFO_STREAM("Using a n_points value of " << n_points_); - - return true; -} + bool NPointFilter::configure() + { + //if (!filters::FilterBase::getParam("n_points", n_points_)) + if (!this->nh_.getParam("n_points", n_points_)) + { + ROS_WARN_STREAM("NPointFilter, params has no attribute n_points."); + } + if (n_points_ < 2) + { + ROS_WARN_STREAM( "n_points attribute less than min(2), setting to minimum"); + n_points_ = 2; + } + ROS_INFO_STREAM("Using a n_points value of " << n_points_); + + return true; + } template -bool NPointFilter::update(const T& trajectory_in, T& trajectory_out) { - bool success = false; - int size_in = trajectory_in.request.trajectory.points.size(); - - // Copy non point related data - trajectory_out.request.trajectory = trajectory_in.request.trajectory; - // Clear out the trajectory points - trajectory_out.request.trajectory.points.clear(); - - if (size_in > n_points_) { - //Add first point to output trajectory - trajectory_out.request.trajectory.points.push_back( - trajectory_in.request.trajectory.points.front()); - - int intermediate_points = n_points_ - 2; //subtract the first and last elements - double int_point_increment = double(size_in) - / double(intermediate_points + 1.0); - ROS_INFO_STREAM( - "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment); - - // The intermediate point index is determined by the following equation: - // int_point_index = i * int_point_increment - // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 -> - // int_point_increment = 3.66667, - // i = 1: int_point_index = 3 - // i = 2: int_point_index = 7 - for (int i = 1; i <= intermediate_points; i++) { - int int_point_index = int(double(i) * int_point_increment); - ROS_INFO_STREAM("Intermediate point index: " << int_point_index); - trajectory_out.request.trajectory.points.push_back( - trajectory_in.request.trajectory.points[int_point_index]); - } - - //Add last point to output trajectory - trajectory_out.request.trajectory.points.push_back( - trajectory_in.request.trajectory.points.back()); - - ROS_INFO_STREAM( - "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size()); - - success = true; - } else { - ROS_WARN_STREAM( - "Trajectory size less than n: " << n_points_ << ", pass through"); - trajectory_out.request.trajectory = trajectory_in.request.trajectory; - success = true; - } - - return success; -} + bool NPointFilter::update(const T& trajectory_in, T& trajectory_out) + { + bool success = false; + int size_in = trajectory_in.request.trajectory.points.size(); + + // Copy non point related data + trajectory_out.request.trajectory = trajectory_in.request.trajectory; + // Clear out the trajectory points + trajectory_out.request.trajectory.points.clear(); + + if (size_in > n_points_) + { + //Add first point to output trajectory + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front()); + + int intermediate_points = n_points_ - 2; //subtract the first and last elements + double int_point_increment = double(size_in) / double(intermediate_points + 1.0); + ROS_INFO_STREAM( + "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment); + + // The intermediate point index is determined by the following equation: + // int_point_index = i * int_point_increment + // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 -> + // int_point_increment = 3.66667, + // i = 1: int_point_index = 3 + // i = 2: int_point_index = 7 + for (int i = 1; i <= intermediate_points; i++) + { + int int_point_index = int(double(i) * int_point_increment); + ROS_INFO_STREAM("Intermediate point index: " << int_point_index); + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]); + } + + //Add last point to output trajectory + trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back()); + + ROS_INFO_STREAM( + "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size()); + + success = true; + } + else + { + ROS_WARN_STREAM( "Trajectory size less than n: " << n_points_ << ", pass through"); + trajectory_out.request.trajectory = trajectory_in.request.trajectory; + success = true; + } + + return success; + } // registering planner adapter CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, - planning_request_adapter::PlanningRequestAdapter); + planning_request_adapter::PlanningRequestAdapter); /* * Old plugin declaration for arm navigation trajectory filters diff --git a/industrial_trajectory_filters/src/uniform_sample_filter.cpp b/industrial_trajectory_filters/src/uniform_sample_filter.cpp index 2bfb6ea9..157c32cc 100644 --- a/industrial_trajectory_filters/src/uniform_sample_filter.cpp +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -38,179 +38,183 @@ using namespace industrial_trajectory_filters; const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds template -UniformSampleFilter::UniformSampleFilter() : - industrial_trajectory_filters::FilterBase() { - ROS_INFO_STREAM("Constructing N point filter"); - sample_duration_ = DEFAULT_SAMPLE_DURATION; - this->filter_name_ = "UniformSampleFilter"; - this->filter_type_ = "UniformSampleFilter"; -} + UniformSampleFilter::UniformSampleFilter() : + industrial_trajectory_filters::FilterBase() + { + ROS_INFO_STREAM("Constructing N point filter"); + sample_duration_ = DEFAULT_SAMPLE_DURATION; + this->filter_name_ = "UniformSampleFilter"; + this->filter_type_ = "UniformSampleFilter"; + } template -UniformSampleFilter::~UniformSampleFilter() { -} + UniformSampleFilter::~UniformSampleFilter() + { + } template -bool UniformSampleFilter::configure() { - if (!this->nh_.getParam("sample_duration", sample_duration_)) { - ROS_WARN_STREAM( - "UniformSampleFilter, params has no attribute sample_duration."); - } - ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_); + bool UniformSampleFilter::configure() + { + if (!this->nh_.getParam("sample_duration", sample_duration_)) + { + ROS_WARN_STREAM( "UniformSampleFilter, params has no attribute sample_duration."); + } + ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_); - return true; -} + return true; + } template -bool UniformSampleFilter::update(const T& trajectory_in, T& trajectory_out) { - bool success = false; - size_t size_in = trajectory_in.request.trajectory.points.size(); - double duration_in = - trajectory_in.request.trajectory.points.back().time_from_start.toSec(); - double interpolated_time = 0.0; - size_t index_in = 0; - - trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt; - - trajectory_out = trajectory_in; - - // Clear out the trajectory points - trajectory_out.request.trajectory.points.clear(); - - while (interpolated_time < duration_in) { - ROS_INFO_STREAM("Interpolated time: " << interpolated_time); - // Increment index until the interpolated time is past the start time. - while (interpolated_time - > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()) { - ROS_INFO_STREAM( - "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())); - ROS_INFO_STREAM("Incrementing index"); - index_in++; - if (index_in >= size_in) { - ROS_ERROR_STREAM( - "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time); - return false; - } - } - p1 = trajectory_in.request.trajectory.points[index_in]; - p2 = trajectory_in.request.trajectory.points[index_in + 1]; - if (!interpolatePt(p1, p2, interpolated_time, interp_pt)) { - ROS_ERROR_STREAM("Failed to interpolate point"); - return false; - } - trajectory_out.request.trajectory.points.push_back(interp_pt); - interpolated_time += sample_duration_; - - } - - ROS_INFO_STREAM( - "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time); - p2 = trajectory_in.request.trajectory.points.back(); - p2.time_from_start = ros::Duration(interpolated_time); - // TODO: Really should check that appending the last point doesn't result in - // really slow motion at the end. This could happen if the sample duration is a - // large percentage of the trajectory duration (not likely). - trajectory_out.request.trajectory.points.push_back(p2); - - ROS_INFO_STREAM( - "Uniform sampling, resample duraction: " << sample_duration_ << " input traj. size: " << trajectory_in.request.trajectory.points.size() << " output traj. size: " << trajectory_out.request.trajectory.points.size()); - - success = true; - return success; -} + bool UniformSampleFilter::update(const T& trajectory_in, T& trajectory_out) + { + bool success = false; + size_t size_in = trajectory_in.request.trajectory.points.size(); + double duration_in = trajectory_in.request.trajectory.points.back().time_from_start.toSec(); + double interpolated_time = 0.0; + size_t index_in = 0; + + trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt; + + trajectory_out = trajectory_in; + + // Clear out the trajectory points + trajectory_out.request.trajectory.points.clear(); + + while (interpolated_time < duration_in) + { + ROS_INFO_STREAM("Interpolated time: " << interpolated_time); + // Increment index until the interpolated time is past the start time. + while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()) + { + ROS_INFO_STREAM( + "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())); + ROS_INFO_STREAM("Incrementing index"); + index_in++; + if (index_in >= size_in) + { + ROS_ERROR_STREAM( + "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time); + return false; + } + } + p1 = trajectory_in.request.trajectory.points[index_in]; + p2 = trajectory_in.request.trajectory.points[index_in + 1]; + if (!interpolatePt(p1, p2, interpolated_time, interp_pt)) + { + ROS_ERROR_STREAM("Failed to interpolate point"); + return false; + } + trajectory_out.request.trajectory.points.push_back(interp_pt); + interpolated_time += sample_duration_; + + } + + ROS_INFO_STREAM( + "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time); + p2 = trajectory_in.request.trajectory.points.back(); + p2.time_from_start = ros::Duration(interpolated_time); + // TODO: Really should check that appending the last point doesn't result in + // really slow motion at the end. This could happen if the sample duration is a + // large percentage of the trajectory duration (not likely). + trajectory_out.request.trajectory.points.push_back(p2); + + ROS_INFO_STREAM( + "Uniform sampling, resample duraction: " << sample_duration_ << " input traj. size: " << trajectory_in.request.trajectory.points.size() << " output traj. size: " << trajectory_out.request.trajectory.points.size()); + + success = true; + return success; + } template -bool UniformSampleFilter::interpolatePt( - trajectory_msgs::JointTrajectoryPoint & p1, - trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start, - trajectory_msgs::JointTrajectoryPoint & interp_pt) { - bool rtn = false; - double p1_time_from_start = p1.time_from_start.toSec(); - double p2_time_from_start = p2.time_from_start.toSec(); - - ROS_INFO_STREAM("time from start: " << time_from_start); - - if (time_from_start >= p1_time_from_start - && time_from_start <= p2_time_from_start) { - if (p1.positions.size() == p1.velocities.size() - && p1.positions.size() == p1.accelerations.size()) { - if (p1.positions.size() == p2.positions.size() - && p1.velocities.size() == p2.velocities.size() - && p1.accelerations.size() == p2.accelerations.size()) { - // Copy p1 to ensure the interp_pt has the correct size vectors - interp_pt = p1; - // TODO: Creating a new spline calculator in this function means that - // it may be created multiple times for the same points (assuming the - // resample duration is less that the actual duration, which it might - // be sometimes) - KDL::VelocityProfile_Spline spline_calc; - ROS_INFO_STREAM( - "---------------Begin interpolating joint point---------------"); - - for (size_t i = 0; i < p1.positions.size(); ++i) { - // Calculated relative times for spline calculation - double time_from_p1 = time_from_start - - p1.time_from_start.toSec(); - double time_from_p1_to_p2 = p2_time_from_start - - p1_time_from_start; - - ROS_INFO_STREAM("time from p1: " << time_from_p1); - ROS_INFO_STREAM( - "time_from_p1_to_p2: " << time_from_p1_to_p2); - - spline_calc.SetProfileDuration(p1.positions[i], - p1.velocities[i], p1.accelerations[i], - p2.positions[i], p2.velocities[i], - p2.accelerations[i], time_from_p1_to_p2); - - ros::Duration time_from_start_dur(time_from_start); - ROS_INFO_STREAM( - "time from start_dur: " << time_from_start_dur); - - interp_pt.time_from_start = time_from_start_dur; - interp_pt.positions[i] = spline_calc.Pos(time_from_p1); - interp_pt.velocities[i] = spline_calc.Vel(time_from_p1); - interp_pt.accelerations[i] = spline_calc.Acc(time_from_p1); - - ROS_INFO_STREAM( - "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start); - - ROS_INFO_STREAM( - "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start); - - ROS_INFO_STREAM( - "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start); - } - ROS_INFO_STREAM( - "---------------End interpolating joint point---------------"); - rtn = true; - } else { - ROS_ERROR_STREAM("Trajectory point size mismatch"); - ROS_ERROR_STREAM( - "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); - ROS_ERROR_STREAM( - "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size()); - rtn = false; - } - - } else { - ROS_ERROR_STREAM( - "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); - rtn = false; - } - } else { - ROS_ERROR_STREAM( - "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); - rtn = false; - } - - return rtn; -} + bool UniformSampleFilter::interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, + trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start, + trajectory_msgs::JointTrajectoryPoint & interp_pt) + { + bool rtn = false; + double p1_time_from_start = p1.time_from_start.toSec(); + double p2_time_from_start = p2.time_from_start.toSec(); + + ROS_INFO_STREAM("time from start: " << time_from_start); + + if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start) + { + if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size()) + { + if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size() + && p1.accelerations.size() == p2.accelerations.size()) + { + // Copy p1 to ensure the interp_pt has the correct size vectors + interp_pt = p1; + // TODO: Creating a new spline calculator in this function means that + // it may be created multiple times for the same points (assuming the + // resample duration is less that the actual duration, which it might + // be sometimes) + KDL::VelocityProfile_Spline spline_calc; + ROS_INFO_STREAM( "---------------Begin interpolating joint point---------------"); + + for (size_t i = 0; i < p1.positions.size(); ++i) + { + // Calculated relative times for spline calculation + double time_from_p1 = time_from_start - p1.time_from_start.toSec(); + double time_from_p1_to_p2 = p2_time_from_start - p1_time_from_start; + + ROS_INFO_STREAM("time from p1: " << time_from_p1); + ROS_INFO_STREAM( "time_from_p1_to_p2: " << time_from_p1_to_p2); + + spline_calc.SetProfileDuration(p1.positions[i], p1.velocities[i], p1.accelerations[i], p2.positions[i], + p2.velocities[i], p2.accelerations[i], time_from_p1_to_p2); + + ros::Duration time_from_start_dur(time_from_start); + ROS_INFO_STREAM( "time from start_dur: " << time_from_start_dur); + + interp_pt.time_from_start = time_from_start_dur; + interp_pt.positions[i] = spline_calc.Pos(time_from_p1); + interp_pt.velocities[i] = spline_calc.Vel(time_from_p1); + interp_pt.accelerations[i] = spline_calc.Acc(time_from_p1); + + ROS_INFO_STREAM( + "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start); + + ROS_INFO_STREAM( + "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start); + + ROS_INFO_STREAM( + "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start); + } + ROS_INFO_STREAM( "---------------End interpolating joint point---------------"); + rtn = true; + } + else + { + ROS_ERROR_STREAM("Trajectory point size mismatch"); + ROS_ERROR_STREAM( + "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + ROS_ERROR_STREAM( + "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size()); + rtn = false; + } + + } + else + { + ROS_ERROR_STREAM( + "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + rtn = false; + } + } + else + { + ROS_ERROR_STREAM( + "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); + rtn = false; + } + + return rtn; + } // registering planner adapter -CLASS_LOADER_REGISTER_CLASS( - industrial_trajectory_filters::UniformSampleFilterAdapter, - planning_request_adapter::PlanningRequestAdapter); +CLASS_LOADER_REGISTER_CLASS( industrial_trajectory_filters::UniformSampleFilterAdapter, + planning_request_adapter::PlanningRequestAdapter); /* * Old plugin declaration for arm navigation trajectory filters