From c0c9a8d8a428199749f0bd8753834be5734e87ea Mon Sep 17 00:00:00 2001 From: Shaun Edwards Date: Thu, 26 Dec 2013 19:15:45 -0500 Subject: [PATCH] Added comments to header files, corrected formating --- .../filter_base.h | 341 ++++++++-------- .../n_point_filter.h | 110 +++--- .../uniform_sample_filter.h | 132 ++++--- .../src/n_point_filter.cpp | 181 ++++----- .../src/uniform_sample_filter.cpp | 369 +++++++++--------- 5 files changed, 587 insertions(+), 546 deletions(-) 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 f4d2ffa0..3e76acb9 100644 --- a/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h +++ b/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h @@ -1,33 +1,33 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Southwest Research Institute, nor the names + * of its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #ifndef FILTER_BASE_H_ #define FILTER_BASE_H_ @@ -39,11 +39,28 @@ #include #include -/** Trajectory filter interface: +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. * - * Description: - * This version of the FilterBase can be used to encapsulate the functionality from an arm navigation + */ +struct MessageAdapter { + + /** + * @brief Arm navigation message type for trajectory filter + * + */ + struct Request { + trajectory_msgs::JointTrajectory trajectory; + } request; +}; + +/** + * @brief This version of the FilterBase can be used to encapsulate the functionality from an arm navigation * trajectory filter into a PlanningRequestAdapter that is used by MoveIt. + * * Since this version of the FilterBase class only provides some degree of backwards compatibility * with the original FilterBase interface, then some parts of the arm navigation filter must be modified in order * to accommodate the differences introduced by this new interface class. @@ -99,141 +116,153 @@ * MessageAdapter structure and the planning interface objects in the argument list. The filter's implementation * should override this method whenever a custom adapter structure is used. */ - -namespace industrial_trajectory_filters -{ - -/* - * Message Adapter structure serves as a stand-in for the arm navigation message type that - * the filter was specialized with. - * - */ -struct MessageAdapter -{ - struct Request - { - trajectory_msgs::JointTrajectory trajectory; - } request; -}; - -/* - * Filter Base Interface - * Replaces the original arm navigation FilterBase. - */ template -class FilterBase: public planning_request_adapter::PlanningRequestAdapter -{ +class FilterBase: public planning_request_adapter::PlanningRequestAdapter { public: - FilterBase(): planning_request_adapter::PlanningRequestAdapter(), - nh_("~"), - configured_(false), - filter_type_("FilterBase"), - filter_name_("Unimplemented") - { - - } - - virtual ~FilterBase(){}; - - - /** update: Original FilterBase method - * Update the filter and return the data separately - * - param data_in A reference to the data to be input to the filter - * - param data_out A reference to the data output location - * This function must be implemented in the derived class. - */ - virtual bool update(const T& data_in, T& data_out)=0; - - - std::string getType() {return filter_type_;}; // original FilterBase method - inline const std::string& getName(){return filter_name_;}; // original FilterBase method - + /** + * @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: - /** configure: Original FilterBase method - * Pure virtual function for the sub class to configure the filter - * This function must be implemented in the derived class. - */ - virtual bool configure()=0; - - ///The name of the filter - std::string filter_name_; - - ///The type of the filter (Used by FilterChain for Factory construction) - std::string filter_type_; - - /// Whether the filter has been configured. - bool configured_; - - // node handle that can be used for param lookup - ros::NodeHandle nh_; - + /** + * @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: - /* - * 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< std::size_t > &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; - } - - virtual std::string getDescription() const - { - // non const pointer to this - FilterBase *p = const_cast* >(this); - - std::stringstream ss; - ss<<"Trajectory filter '"<getName()<<"' of type '"<getType()<<"'"; - return ss.str(); - } + /** + * @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(); + } }; } - #endif /* FILTER_BASE_H_ */ 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 a9fae83e..ec53c8cd 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 @@ -1,67 +1,85 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Southwest Research Institute, nor the names + * of its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #ifndef N_POINT_FILTER_H_ #define N_POINT_FILTER_H_ #include +namespace industrial_trajectory_filters { +/** + * @brief This is a simple filter which reduces a trajectory to N points or less + */ +template -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 -{ +class NPointFilter: public industrial_trajectory_filters::FilterBase { public: + /** + * @brief Default constructor + */ NPointFilter(); - ~NPointFilter(); + /** + * @brief Default destructor + */ + ~NPointFilter(); + ; + + virtual bool configure(); - /// \brief Configures the filter - 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). - 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: - int n_points_; // @brief number of points to reduce trajectory to + /** + * @brief number of points to reduce trajectory to + */ + int n_points_; }; -// Specializing trajectory filter implementation +/** + * @brief Specializing trajectory filter implementation + */ typedef NPointFilter NPointFilterAdapter; } 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 85cff110..c3c33054 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 @@ -1,81 +1,103 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Southwest Research Institute, nor the names + * of its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #ifndef UNIFORM_SAMPLE_FILTER_H_ #define UNIFORM_SAMPLE_FILTER_H_ - #include /* * These headers were part of the trajectory filter interface from the * arm navigation system. These can no longer be included in a catkin based * package. -#include -#include -*/ + #include + #include + */ -namespace industrial_trajectory_filters -{ +namespace industrial_trajectory_filters { -// \brief This is a simple filter which performs a uniforming sampling of -// a trajectory using linear interpolation. -template +/** + * \brief This is a simple filter which performs a uniforming sampling of + * a trajectory using linear interpolation. + * + */ +template -class UniformSampleFilter: public industrial_trajectory_filters::FilterBase -{ +class UniformSampleFilter: public industrial_trajectory_filters::FilterBase { public: + /** + * @brief Default constructor + */ UniformSampleFilter(); - ~UniformSampleFilter(); - - /// \brief Configures the filter - virtual bool configure(); + /** + * @brief Default destructor + */ + ~UniformSampleFilter(); - // \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). - bool update(const T& trajectory_in, T& trajectory_out); + virtual bool configure(); - // \brief Perform interpolation between p1 and p2. Time from start must be - // in between p1 and p2 times. - bool interpolatePt( - trajectory_msgs::JointTrajectoryPoint & p1, - trajectory_msgs::JointTrajectoryPoint & p2, - double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); + /** + * 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); private: - double sample_duration_; // @brief uniform sample duration (sec) + /** + * @brief uniform sample duration (sec) + */ + double sample_duration_; }; +/** + * @brief Specializing trajectory filter implementation + */ typedef UniformSampleFilter UniformSampleFilterAdapter; } diff --git a/industrial_trajectory_filters/src/n_point_filter.cpp b/industrial_trajectory_filters/src/n_point_filter.cpp index cb67e526..0f7c8977 100644 --- a/industrial_trajectory_filters/src/n_point_filter.cpp +++ b/industrial_trajectory_filters/src/n_point_filter.cpp @@ -1,33 +1,33 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Southwest Research Institute, nor the names + * of its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ //#include #include @@ -35,64 +35,57 @@ using namespace industrial_trajectory_filters; -const int DEFAULT_N=2; +const int DEFAULT_N = 2; - -template -NPointFilter::NPointFilter(): - FilterBase() -{ +template +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() -{} - -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; +template +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; +} -template -bool NPointFilter::update(const T& trajectory_in, - T& trajectory_out) -{ - bool success = false; - int size_in = trajectory_in.request.trajectory.points.size(); +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(); + // 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_) - { + if (size_in > n_points_) { //Add first point to output trajectory - trajectory_out.request.trajectory.points.push_back( - trajectory_in.request.trajectory.points.front()); + 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); + 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 @@ -100,31 +93,29 @@ bool NPointFilter::update(const T& trajectory_in, // 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++) - { + 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]); + 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()); + 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()); + 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; + } 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 @@ -133,9 +124,9 @@ CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, /* * Old plugin declaration for arm navigation trajectory filters -PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, - IndustrialNPointFilterJointTrajectoryWithConstraints, - industrial_trajectory_filters::NPointFilter, - filters::FilterBase); + PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, + IndustrialNPointFilterJointTrajectoryWithConstraints, + industrial_trajectory_filters::NPointFilter, + filters::FilterBase); -*/ + */ diff --git a/industrial_trajectory_filters/src/uniform_sample_filter.cpp b/industrial_trajectory_filters/src/uniform_sample_filter.cpp index 73459a84..2bfb6ea9 100644 --- a/industrial_trajectory_filters/src/uniform_sample_filter.cpp +++ b/industrial_trajectory_filters/src/uniform_sample_filter.cpp @@ -1,33 +1,33 @@ /* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Southwest Research Institute + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Southwest Research Institute, nor the names + * of its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #include #include @@ -35,85 +35,76 @@ using namespace industrial_trajectory_filters; -const double DEFAULT_SAMPLE_DURATION=0.050; //seconds +const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds - -template -UniformSampleFilter::UniformSampleFilter(): - industrial_trajectory_filters::FilterBase() -{ +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"; } -template -UniformSampleFilter::~UniformSampleFilter() -{} +template +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_); +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_); - 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_; + + } -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 ); + 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 @@ -121,122 +112,112 @@ bool UniformSampleFilter::update(const T& trajectory_in, // 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()); + 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 +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; + 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); } - - } - else - { - ROS_ERROR_STREAM("Trajectory point not fully defined, pos: " << p1.positions.size() << - " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); + 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("Time: " << time_from_start << " not between interpolation point times[" << - p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); + + } 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; - } - - + return rtn; +} // registering planner adapter -CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::UniformSampleFilterAdapter, +CLASS_LOADER_REGISTER_CLASS( + industrial_trajectory_filters::UniformSampleFilterAdapter, planning_request_adapter::PlanningRequestAdapter); /* * Old plugin declaration for arm navigation trajectory filters -PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, - IndustrialNPointFilterJointTrajectoryWithConstraints, - industrial_trajectory_filters::NPointFilter, - filters::FilterBase); + PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, + IndustrialNPointFilterJointTrajectoryWithConstraints, + industrial_trajectory_filters::NPointFilter, + filters::FilterBase); -*/ + */