diff --git a/include/eband_local_planner/conversions_and_types.h b/include/eband_local_planner/conversions_and_types.h index e1c70cd..d493684 100644 --- a/include/eband_local_planner/conversions_and_types.h +++ b/include/eband_local_planner/conversions_and_types.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #ifndef EBAND_CONVERSIONS_AND_TYPES_H_ #define EBAND_CONVERSIONS_AND_TYPES_H_ @@ -58,54 +58,54 @@ namespace eband_local_planner{ -// typedefs - -///<@brief defines a bubble - pose of center & radius of according hypersphere (expansion) -struct Bubble -{ - geometry_msgs::PoseStamped center; - double expansion; -}; - -enum AddAtPosition {add_front, add_back}; - -// functions - -/** - * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from quaternions to euler angles) - * @param Pose which shall be converted - * @param References to converted ROS Pose2D frmae - */ -void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D); - - -/** - * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from euler angles to quaternions, -> z-coordinate is set to zero) - * @param References to converted ROS Pose2D frame - * @param Pose2D which shall be converted - */ -void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D); - - -/** - * @brief Transforms the global plan of the robot from the planner frame to the local frame. This replaces the transformGlobalPlan as defined in the base_local_planner/goal_functions.h main difference is that it additionally outputs counter indicating which part of the plan has been transformed. - * @param tf A reference to a transform listener - * @param global_plan The plan to be transformed - * @param costmap A reference to the costmap being used so the window size for transforming can be computed - * @param global_frame The frame to transform the plan to - * @param transformed_plan Populated with the transformed plan - * @param number of start and end frame counted from the end of the global plan - */ -bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, - costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, - std::vector& transformed_plan, std::vector& start_end_counts_from_end); - -/** - * @brief Gets the footprint of the robot and computes the circumscribed radius for the eband approach - * @param costmap A reference to the costmap from which the radius is computed - * @return radius in meters - */ -double getCircumscribedRadius(costmap_2d::Costmap2DROS& costmap); + // typedefs + + ///<@brief defines a bubble - pose of center & radius of according hypersphere (expansion) + struct Bubble + { + geometry_msgs::PoseStamped center; + double expansion; + }; + + enum AddAtPosition {add_front, add_back}; + + // functions + + /** + * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from quaternions to euler angles) + * @param Pose which shall be converted + * @param References to converted ROS Pose2D frmae + */ + void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D); + + + /** + * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from euler angles to quaternions, -> z-coordinate is set to zero) + * @param References to converted ROS Pose2D frame + * @param Pose2D which shall be converted + */ + void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D); + + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame. This replaces the transformGlobalPlan as defined in the base_local_planner/goal_functions.h main difference is that it additionally outputs counter indicating which part of the plan has been transformed. + * @param tf A reference to a transform listener + * @param global_plan The plan to be transformed + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param transformed_plan Populated with the transformed plan + * @param number of start and end frame counted from the end of the global plan + */ + bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, + costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, + std::vector& transformed_plan, std::vector& start_end_counts_from_end); + + /** + * @brief Gets the footprint of the robot and computes the circumscribed radius for the eband approach + * @param costmap A reference to the costmap from which the radius is computed + * @return radius in meters + */ + double getCircumscribedRadius(costmap_2d::Costmap2DROS& costmap); }; #endif diff --git a/include/eband_local_planner/eband_local_planner.h b/include/eband_local_planner/eband_local_planner.h index bcd6fb2..ab21f21 100644 --- a/include/eband_local_planner/eband_local_planner.h +++ b/include/eband_local_planner/eband_local_planner.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #ifndef EBAND_LOCAL_PLANNER_H_ #define EBAND_LOCAL_PLANNER_H_ @@ -71,292 +71,292 @@ namespace eband_local_planner{ -/** - * @class EBandPlanner - * @brief Implements the Elastic Band Method for SE2-Manifold (mobile Base) - */ -class EBandPlanner{ - - public: - /** - * @brief Default constructor - */ - EBandPlanner(); - - /** - * @brief Constructs the elastic band object - * @param name The name to give this instance of the elastic band local planner - * @param costmap The cost map to use for assigning costs to trajectories - */ - EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief Destructor - */ - ~EBandPlanner(); - - /** - * @brief Initializes the elastic band class by accesing costmap and loading parameters - * @param name The name to give this instance of the trajectory planner - * @param costmap The cost map to use for assigning costs to trajectories - */ - void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief passes a reference to the eband visualization object which can be used to visualize the band optimization - * @param pointer to visualization object - */ - void setVisualization(boost::shared_ptr eband_visual); - - /** - * @brief Set plan which shall be optimized to elastic band planner - * @param global_plan The plan which shall be optimized - * @return True if the plan was set successfully - */ - bool setPlan(const std::vector& global_plan); - - /** - * @brief This transforms the refined band to a path again and outputs it - * @param reference via which the path is passed to the caller - * @return true if path could be handed over - */ - bool getPlan(std::vector& global_plan); - - /** - * @brief This outputs the elastic_band - * @param reference via which the band is passed to the caller - * @return true if there was a band to pass - */ - bool getBand(std::vector& elastic_band); - - /** - * @brief converts robot_pose to a bubble and tries to connect to the path - * @param pose of the robot which shall be connected to the band - * @return true if pose could be connected - */ - bool addFrames(const std::vector& robot_pose, const AddAtPosition& add_frames_at); - - /** - * @brief cycles over the elastic band set before and optimizes it locally by minimizing an energy-function - * @return true if band is valid - */ - bool optimizeBand(); - - /** - * @brief cycles over the elastic band checks it for validity and optimizes it locally by minimizing an energy-function - * @param reference to band which shall be optimized - * @return true if band is valid - */ - bool optimizeBand(std::vector& band); - - private: - // pointer to external objects (do NOT delete object) - costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap - - // parameters - std::vector acc_lim_; ///<@brief acceleration limits for translational and rotational motion - int num_optim_iterations_; ///<@brief maximal number of iteration steps during optimization of band - double internal_force_gain_; ///<@brief gain for internal forces ("Elasticity of Band") - double external_force_gain_; ///<@brief gain for external forces ("Penalty on low distance to abstacles") - double tiny_bubble_distance_; ///<@brief internal forces between two bubbles are only calc. if there distance is bigger than this lower bound - double tiny_bubble_expansion_; ///<@brief lower bound for bubble expansion. below this bound bubble is considered as "in collision" - double min_bubble_overlap_; ///<@brief minimum relative overlap two bubbles must have to be treated as connected - int max_recursion_depth_approx_equi_; ///@brief maximum depth for recursive approximation to constrain computational burden - double equilibrium_relative_overshoot_; ///@brief percentage of old force for which a new force is considered significant when higher as this value - double significant_force_; ///@brief lower bound for absolute value of force below which it is treated as insignificant (no recursive approximation) - double costmap_weight_; // the costmap weight or scaling factor - - // pointer to locally created objects (delete - except for smart-ptrs:) - base_local_planner::CostmapModel* world_model_; // local world model - boost::shared_ptr eband_visual_; // pointer to visualization object - - // flags - bool initialized_, visualization_; - - // data - std::vector footprint_spec_; // specification of robot footprint as vector of corner points - costmap_2d::Costmap2D* costmap_; // pointer to underlying costmap - std::vector global_plan_; // copy of the plan that shall be optimized - std::vector elastic_band_; - - - // methods - - // band refinement (filling of gaps & removing redundant bubbles) - - /** - * @brief Cycles over an band and searches for gaps and redundant bubbles. Tries to close gaps and remove redundant bubbles. - * @param band is a reference to the band that shall be refined - * @return true if all gaps could be closed and band is valid - */ - bool refineBand(std::vector& band); - - - /** - * @brief recursively checks an intervall of a band whether gaps need to be filled or bubbles may be removed and fills or removes if possible. This exploits the sequential structure of the band. Therefore it can not detect redundant cycles which are closed over several on its own not redundant bubbles. - * @param reference to the elastic band that shall be checked - * @param reference to the iterator pointing to the start of the intervall that shall be checked - * @param reference to the iterator pointing to the end of the intervall that shall be checked - * @return true if segment is valid (all gaps filled), falls if band is broken within this segment - */ - bool removeAndFill(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter); - - - /** - * @brief recursively fills gaps between two bubbles (if possible) - * @param reference to the elastic band that is worked on - * @param reference to the iterator pointing to the start of the intervall that shall be filled - * @param reference to the iterator pointing to the end of the intervall that shall be filled - * @return true if gap was successfully filled otherwise false (band broken) - */ - bool fillGap(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter); - - - // optimization - - /** - * @brief calculates internal and external forces and applies changes to the band - * @param reference to band which shall be modified - * @return true if band could be modified, all steps finished cleanly - */ - bool modifyBandArtificialForce(std::vector& band); - - - /** - * @brief Applies forces to move bubbles and recalculates expansion of bubbles - * @param reference to number of bubble which shall be modified - number might be modified if additional bubbles are introduced to fill band - * @param reference to band which shall be modified - * @param forces and torques which shall be applied - * @return true if modified band valid, false if band is broken (one or more bubbles in collision) - */ - bool applyForces(int bubble_num, std::vector& band, std::vector forces); - - - /** - * @brief Checks for zero-crossings and large changes in force-vector after moving of bubble - if detected it tries to approximate the equilibrium point - * @param position of the checked bubble within the band - * @param band within which equilibrium position shall be approximated - * @param force calculated for last bubble pose - * @param current step width - * @param current recursion depth to constrain number of recurions - * @return true if recursion successfully - */ - bool moveApproximateEquilibrium(const int& bubble_num, const std::vector& band, Bubble& curr_bubble, - const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width, - const int& curr_recursion_depth); - - - // problem (geometry) dependant functions - - /** - * @brief interpolates between two bubbles by calculating the pose for the center of a bubble in the middle of the path connecting the bubbles [depends kinematics] - * @param center of first of the two bubbles between which shall be interpolated - * @param center of second of the two bubbles between which shall be interpolated - * @param reference to hand back the interpolated bubble's center - * @return true if interpolation was successful - */ - bool interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center); - - - /** - * @brief this checks whether two bubbles overlap - * @param band on which we want to check - * @param iterator to first bubble - * @param iterator to second bubble - * @return true if bubbles overlap - */ - bool checkOverlap(Bubble bubble1, Bubble bubble2); - - - /** - * @brief This calculates the distance between two bubbles [depends kinematics, shape] - * @param pose of the center of first bubbles - * @param pose of the center of second bubbles - * @param refernce to variable to pass distance to caller function - * @return true if distance was successfully calculated - */ - bool calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance); - - - /** - * @brief Calculates the difference between the pose of the center of two bubbles and outputs it as Twist (pointing from first to second bubble) [depends kinematic] - * @param pose of the first bubble - * @param pose of the second bubble - * @param reference to variable in wich the difference is stored as twist - * @return true if difference was successfully calculated - */ - bool calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference); - - - /** - * @brief Calculates the distance between the center of a bubble and the closest obstacle [depends kinematics, shape, environment] - * @param center of bubble as Pose - * @param reference to distance variable - * @return True if successfully calculated distance false otherwise - */ - bool calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance); - - - /** - * @brief Calculates all forces for a certain bubble at a specific position in the band [depends kinematic] - * @param position in band for which internal forces shall be calculated - * @param band for which forces shall be calculated - * @param Bubble for which internal forces shall be calculated - * @param reference to variable via which forces and torques are given back - * @return true if forces were calculated successfully - */ - bool getForcesAt(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); - - - /** - * @brief Calculates internal forces for bubbles along the band [depends kinematic] - * @param position in band for which internal forces shall be calculated - * @param band for which forces shall be calculated - * @param Bubble for which internal forces shall be calculated - * @param reference to variable via which forces and torques are given back - * @return true if forces were calculated successfully - */ - bool calcInternalForces(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); - - - /** - * @brief Calculates external forces for bubbles along the band [depends shape, environment] - * @param position in band for which internal forces shall be calculated - * @param Bubble for which external forces shall be calculated - * @param reference to variable via which forces and torques are given back - * @return true if forces were calculated successfully - */ - bool calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); - - - /** - * @brief Calculates tangential portion of forces - * @param number of bubble for which forces shall be recalculated - * @param reference to variable via which forces and torques are given back - * @return true if forces were calculated successfully - */ - bool suppressTangentialForces(int bubble_num, std::vector band, geometry_msgs::WrenchStamped& forces); - - - // type conversion - - /** - * @brief This converts a plan from a sequence of stamped poses into a band, a sequence of bubbles. Therefore it calculates distances to the surrounding obstacles and derives the expansion or radius of the bubble - * @param plan is the sequence of stamped Poses which shall be converted - * @param band is the resulting sequence of bubbles - * @return true if path was successfully converted - band did not break - */ - bool convertPlanToBand(std::vector plan, std::vector& band); - - - /** - * @brief This converts a band from a sequence of bubbles into a plan from, a sequence of stamped poses - * @param band is the sequence of bubbles which shall be converted - * @param plan is the resulting sequence of stamped Poses - * @return true if path was successfully converted - band did not break - */ - bool convertBandToPlan(std::vector& plan, std::vector band); - -}; + /** + * @class EBandPlanner + * @brief Implements the Elastic Band Method for SE2-Manifold (mobile Base) + */ + class EBandPlanner{ + + public: + /** + * @brief Default constructor + */ + EBandPlanner(); + + /** + * @brief Constructs the elastic band object + * @param name The name to give this instance of the elastic band local planner + * @param costmap The cost map to use for assigning costs to trajectories + */ + EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Destructor + */ + ~EBandPlanner(); + + /** + * @brief Initializes the elastic band class by accesing costmap and loading parameters + * @param name The name to give this instance of the trajectory planner + * @param costmap The cost map to use for assigning costs to trajectories + */ + void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief passes a reference to the eband visualization object which can be used to visualize the band optimization + * @param pointer to visualization object + */ + void setVisualization(boost::shared_ptr eband_visual); + + /** + * @brief Set plan which shall be optimized to elastic band planner + * @param global_plan The plan which shall be optimized + * @return True if the plan was set successfully + */ + bool setPlan(const std::vector& global_plan); + + /** + * @brief This transforms the refined band to a path again and outputs it + * @param reference via which the path is passed to the caller + * @return true if path could be handed over + */ + bool getPlan(std::vector& global_plan); + + /** + * @brief This outputs the elastic_band + * @param reference via which the band is passed to the caller + * @return true if there was a band to pass + */ + bool getBand(std::vector& elastic_band); + + /** + * @brief converts robot_pose to a bubble and tries to connect to the path + * @param pose of the robot which shall be connected to the band + * @return true if pose could be connected + */ + bool addFrames(const std::vector& robot_pose, const AddAtPosition& add_frames_at); + + /** + * @brief cycles over the elastic band set before and optimizes it locally by minimizing an energy-function + * @return true if band is valid + */ + bool optimizeBand(); + + /** + * @brief cycles over the elastic band checks it for validity and optimizes it locally by minimizing an energy-function + * @param reference to band which shall be optimized + * @return true if band is valid + */ + bool optimizeBand(std::vector& band); + + private: + // pointer to external objects (do NOT delete object) + costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap + + // parameters + std::vector acc_lim_; ///<@brief acceleration limits for translational and rotational motion + int num_optim_iterations_; ///<@brief maximal number of iteration steps during optimization of band + double internal_force_gain_; ///<@brief gain for internal forces ("Elasticity of Band") + double external_force_gain_; ///<@brief gain for external forces ("Penalty on low distance to abstacles") + double tiny_bubble_distance_; ///<@brief internal forces between two bubbles are only calc. if there distance is bigger than this lower bound + double tiny_bubble_expansion_; ///<@brief lower bound for bubble expansion. below this bound bubble is considered as "in collision" + double min_bubble_overlap_; ///<@brief minimum relative overlap two bubbles must have to be treated as connected + int max_recursion_depth_approx_equi_; ///@brief maximum depth for recursive approximation to constrain computational burden + double equilibrium_relative_overshoot_; ///@brief percentage of old force for which a new force is considered significant when higher as this value + double significant_force_; ///@brief lower bound for absolute value of force below which it is treated as insignificant (no recursive approximation) + double costmap_weight_; // the costmap weight or scaling factor + + // pointer to locally created objects (delete - except for smart-ptrs:) + base_local_planner::CostmapModel* world_model_; // local world model + boost::shared_ptr eband_visual_; // pointer to visualization object + + // flags + bool initialized_, visualization_; + + // data + std::vector footprint_spec_; // specification of robot footprint as vector of corner points + costmap_2d::Costmap2D* costmap_; // pointer to underlying costmap + std::vector global_plan_; // copy of the plan that shall be optimized + std::vector elastic_band_; + + + // methods + + // band refinement (filling of gaps & removing redundant bubbles) + + /** + * @brief Cycles over an band and searches for gaps and redundant bubbles. Tries to close gaps and remove redundant bubbles. + * @param band is a reference to the band that shall be refined + * @return true if all gaps could be closed and band is valid + */ + bool refineBand(std::vector& band); + + + /** + * @brief recursively checks an intervall of a band whether gaps need to be filled or bubbles may be removed and fills or removes if possible. This exploits the sequential structure of the band. Therefore it can not detect redundant cycles which are closed over several on its own not redundant bubbles. + * @param reference to the elastic band that shall be checked + * @param reference to the iterator pointing to the start of the intervall that shall be checked + * @param reference to the iterator pointing to the end of the intervall that shall be checked + * @return true if segment is valid (all gaps filled), falls if band is broken within this segment + */ + bool removeAndFill(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter); + + + /** + * @brief recursively fills gaps between two bubbles (if possible) + * @param reference to the elastic band that is worked on + * @param reference to the iterator pointing to the start of the intervall that shall be filled + * @param reference to the iterator pointing to the end of the intervall that shall be filled + * @return true if gap was successfully filled otherwise false (band broken) + */ + bool fillGap(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter); + + + // optimization + + /** + * @brief calculates internal and external forces and applies changes to the band + * @param reference to band which shall be modified + * @return true if band could be modified, all steps finished cleanly + */ + bool modifyBandArtificialForce(std::vector& band); + + + /** + * @brief Applies forces to move bubbles and recalculates expansion of bubbles + * @param reference to number of bubble which shall be modified - number might be modified if additional bubbles are introduced to fill band + * @param reference to band which shall be modified + * @param forces and torques which shall be applied + * @return true if modified band valid, false if band is broken (one or more bubbles in collision) + */ + bool applyForces(int bubble_num, std::vector& band, std::vector forces); + + + /** + * @brief Checks for zero-crossings and large changes in force-vector after moving of bubble - if detected it tries to approximate the equilibrium point + * @param position of the checked bubble within the band + * @param band within which equilibrium position shall be approximated + * @param force calculated for last bubble pose + * @param current step width + * @param current recursion depth to constrain number of recurions + * @return true if recursion successfully + */ + bool moveApproximateEquilibrium(const int& bubble_num, const std::vector& band, Bubble& curr_bubble, + const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width, + const int& curr_recursion_depth); + + + // problem (geometry) dependant functions + + /** + * @brief interpolates between two bubbles by calculating the pose for the center of a bubble in the middle of the path connecting the bubbles [depends kinematics] + * @param center of first of the two bubbles between which shall be interpolated + * @param center of second of the two bubbles between which shall be interpolated + * @param reference to hand back the interpolated bubble's center + * @return true if interpolation was successful + */ + bool interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center); + + + /** + * @brief this checks whether two bubbles overlap + * @param band on which we want to check + * @param iterator to first bubble + * @param iterator to second bubble + * @return true if bubbles overlap + */ + bool checkOverlap(Bubble bubble1, Bubble bubble2); + + + /** + * @brief This calculates the distance between two bubbles [depends kinematics, shape] + * @param pose of the center of first bubbles + * @param pose of the center of second bubbles + * @param refernce to variable to pass distance to caller function + * @return true if distance was successfully calculated + */ + bool calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance); + + + /** + * @brief Calculates the difference between the pose of the center of two bubbles and outputs it as Twist (pointing from first to second bubble) [depends kinematic] + * @param pose of the first bubble + * @param pose of the second bubble + * @param reference to variable in wich the difference is stored as twist + * @return true if difference was successfully calculated + */ + bool calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference); + + + /** + * @brief Calculates the distance between the center of a bubble and the closest obstacle [depends kinematics, shape, environment] + * @param center of bubble as Pose + * @param reference to distance variable + * @return True if successfully calculated distance false otherwise + */ + bool calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance); + + + /** + * @brief Calculates all forces for a certain bubble at a specific position in the band [depends kinematic] + * @param position in band for which internal forces shall be calculated + * @param band for which forces shall be calculated + * @param Bubble for which internal forces shall be calculated + * @param reference to variable via which forces and torques are given back + * @return true if forces were calculated successfully + */ + bool getForcesAt(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); + + + /** + * @brief Calculates internal forces for bubbles along the band [depends kinematic] + * @param position in band for which internal forces shall be calculated + * @param band for which forces shall be calculated + * @param Bubble for which internal forces shall be calculated + * @param reference to variable via which forces and torques are given back + * @return true if forces were calculated successfully + */ + bool calcInternalForces(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); + + + /** + * @brief Calculates external forces for bubbles along the band [depends shape, environment] + * @param position in band for which internal forces shall be calculated + * @param Bubble for which external forces shall be calculated + * @param reference to variable via which forces and torques are given back + * @return true if forces were calculated successfully + */ + bool calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces); + + + /** + * @brief Calculates tangential portion of forces + * @param number of bubble for which forces shall be recalculated + * @param reference to variable via which forces and torques are given back + * @return true if forces were calculated successfully + */ + bool suppressTangentialForces(int bubble_num, std::vector band, geometry_msgs::WrenchStamped& forces); + + + // type conversion + + /** + * @brief This converts a plan from a sequence of stamped poses into a band, a sequence of bubbles. Therefore it calculates distances to the surrounding obstacles and derives the expansion or radius of the bubble + * @param plan is the sequence of stamped Poses which shall be converted + * @param band is the resulting sequence of bubbles + * @return true if path was successfully converted - band did not break + */ + bool convertPlanToBand(std::vector plan, std::vector& band); + + + /** + * @brief This converts a band from a sequence of bubbles into a plan from, a sequence of stamped poses + * @param band is the sequence of bubbles which shall be converted + * @param plan is the resulting sequence of stamped Poses + * @return true if path was successfully converted - band did not break + */ + bool convertBandToPlan(std::vector& plan, std::vector band); + + }; }; #endif diff --git a/include/eband_local_planner/eband_local_planner_ros.h b/include/eband_local_planner/eband_local_planner_ros.h index 4f37a29..c71f48c 100644 --- a/include/eband_local_planner/eband_local_planner_ros.h +++ b/include/eband_local_planner/eband_local_planner_ros.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #ifndef EBAND_LOCAL_PLANNER_ROS_H_ #define EBAND_LOCAL_PLANNER_ROS_H_ @@ -74,106 +74,106 @@ namespace eband_local_planner{ -/** - * @class EBandPlannerROS - * @brief Plugin to the ros base_local_planner. Implements a wrapper for the Elastic Band Method - */ -class EBandPlannerROS : public nav_core::BaseLocalPlanner{ - - public: - /** - * @brief Default constructor for the ros wrapper - */ - EBandPlannerROS(); - - /** - * @brief Constructs the ros wrapper - * @param name The name to give this instance of the elastic band local planner - * @param tf A pointer to a transform listener - * @param costmap The cost map to use for assigning costs to trajectories - */ - EBandPlannerROS(std::string name, tf::TransformListener* tf, - costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief Destructor for the wrapper - */ - ~EBandPlannerROS(); - - /** - * @brief Initializes the ros wrapper - * @param name The name to give this instance of the trajectory planner - * @param tf A pointer to a transform listener - * @param costmap The cost map to use for assigning costs to trajectories - */ - void initialize(std::string name, tf::TransformListener* tf, - costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief Set the plan that the controller is following; also reset eband-planner - * @param orig_global_plan The plan to pass to the controller - * @return True if the plan was updated successfully, false otherwise - */ - bool setPlan(const std::vector& orig_global_plan); - - /** - * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base - * @param cmd_vel Will be filled with the velocity command to be passed to the robot base - * @return True if a valid trajectory was found, false otherwise - */ - bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); - - /** - * @brief Check if the goal pose has been achieved - * @return True if achieved, false otherwise - */ - bool isGoalReached(); - - private: - - // pointer to external objects (do NOT delete object) - costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap - tf::TransformListener* tf_; ///<@brief pointer to Transform Listener - - // parameters - double yaw_goal_tolerance_, xy_goal_tolerance_; ///<@brief parameters to define region in which goal is treated as reached - double rot_stopped_vel_, trans_stopped_vel_; ///<@brief lower bound for absolute value of velocity (with respect to stick-slip behaviour) - - // Topics & Services - ros::Publisher g_plan_pub_; ///<@brief publishes modified global plan - ros::Publisher l_plan_pub_; ///<@brief publishes prediction for local commands - ros::Subscriber odom_sub_; ///<@brief subscribes to the odometry topic in global namespace - - // data - nav_msgs::Odometry base_odom_; - std::vector global_plan_; // plan as handed over from move_base or global planner - std::vector transformed_plan_; // plan transformed into the map frame we are working in - std::vector plan_start_end_counter_; // stores which number start and end frame of the transformed plan have inside the global plan - - // pointer to locally created objects (delete - except for smart-ptrs:) - boost::shared_ptr eband_; - boost::shared_ptr eband_visual_; - boost::shared_ptr eband_trj_ctrl_; - - // hack introduced with diff drive plugin (mostly to test a theory) - int band_snapped_hack_count_; - bool band_snapped_hack_; - bool goal_reached_; - - // flags - bool initialized_; - boost::mutex odom_mutex_; // mutex to lock odometry-callback while data is read from topic - - - // methods - - /** - * @brief Odometry-Callback: function is called whenever a new odometry msg is published on that topic - * @param Pointer to the received message - */ - void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); - -}; + /** + * @class EBandPlannerROS + * @brief Plugin to the ros base_local_planner. Implements a wrapper for the Elastic Band Method + */ + class EBandPlannerROS : public nav_core::BaseLocalPlanner{ + + public: + /** + * @brief Default constructor for the ros wrapper + */ + EBandPlannerROS(); + + /** + * @brief Constructs the ros wrapper + * @param name The name to give this instance of the elastic band local planner + * @param tf A pointer to a transform listener + * @param costmap The cost map to use for assigning costs to trajectories + */ + EBandPlannerROS(std::string name, tf::TransformListener* tf, + costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Destructor for the wrapper + */ + ~EBandPlannerROS(); + + /** + * @brief Initializes the ros wrapper + * @param name The name to give this instance of the trajectory planner + * @param tf A pointer to a transform listener + * @param costmap The cost map to use for assigning costs to trajectories + */ + void initialize(std::string name, tf::TransformListener* tf, + costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Set the plan that the controller is following; also reset eband-planner + * @param orig_global_plan The plan to pass to the controller + * @return True if the plan was updated successfully, false otherwise + */ + bool setPlan(const std::vector& orig_global_plan); + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base + * @return True if a valid trajectory was found, false otherwise + */ + bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); + + /** + * @brief Check if the goal pose has been achieved + * @return True if achieved, false otherwise + */ + bool isGoalReached(); + + private: + + // pointer to external objects (do NOT delete object) + costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap + tf::TransformListener* tf_; ///<@brief pointer to Transform Listener + + // parameters + double yaw_goal_tolerance_, xy_goal_tolerance_; ///<@brief parameters to define region in which goal is treated as reached + double rot_stopped_vel_, trans_stopped_vel_; ///<@brief lower bound for absolute value of velocity (with respect to stick-slip behaviour) + + // Topics & Services + ros::Publisher g_plan_pub_; ///<@brief publishes modified global plan + ros::Publisher l_plan_pub_; ///<@brief publishes prediction for local commands + ros::Subscriber odom_sub_; ///<@brief subscribes to the odometry topic in global namespace + + // data + nav_msgs::Odometry base_odom_; + std::vector global_plan_; // plan as handed over from move_base or global planner + std::vector transformed_plan_; // plan transformed into the map frame we are working in + std::vector plan_start_end_counter_; // stores which number start and end frame of the transformed plan have inside the global plan + + // pointer to locally created objects (delete - except for smart-ptrs:) + boost::shared_ptr eband_; + boost::shared_ptr eband_visual_; + boost::shared_ptr eband_trj_ctrl_; + + // hack introduced with diff drive plugin (mostly to test a theory) + int band_snapped_hack_count_; + bool band_snapped_hack_; + bool goal_reached_; + + // flags + bool initialized_; + boost::mutex odom_mutex_; // mutex to lock odometry-callback while data is read from topic + + + // methods + + /** + * @brief Odometry-Callback: function is called whenever a new odometry msg is published on that topic + * @param Pointer to the received message + */ + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); + + }; }; #endif diff --git a/include/eband_local_planner/eband_trajectory_controller.h b/include/eband_local_planner/eband_trajectory_controller.h index e5262c6..3d4f067 100644 --- a/include/eband_local_planner/eband_trajectory_controller.h +++ b/include/eband_local_planner/eband_trajectory_controller.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #ifndef EBAND_TRAJECTORY_CONTROLLER_H_ #define EBAND_TRAJECTORY_CONTROLLER_H_ @@ -63,138 +63,138 @@ namespace eband_local_planner{ -/** - * @class EBandPlanner - * @brief Implements the Elastic Band Method for SE2-Manifold (mobile Base) - */ -class EBandTrajectoryCtrl{ - - public: - - /** - * @brief Default constructor - */ - EBandTrajectoryCtrl(); - - /** - * @brief Constructs the elastic band object - * @param name The name to give this instance of the elastic band local planner - * @param costmap The cost map to use for assigning costs to trajectories - */ - EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief Destructor - */ - ~EBandTrajectoryCtrl(); - - /** - * @brief Initializes the elastic band class by accesing costmap and loading parameters - * @param name The name to give this instance of the trajectory planner - * @param costmap The cost map to use for assigning costs to trajectories - */ - void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief passes a reference to the eband visualization object which can be used to visualize the band optimization - * @param pointer to visualization object - */ - void setVisualization(boost::shared_ptr target_visual); - - /** - * @brief This sets the elastic_band to the trajectory controller - * @param reference via which the band is passed - */ - bool setBand(const std::vector& elastic_band); - - /** - * @brief This sets the robot velocity as provided by odometry - * @param reference via which odometry is passed - */ - bool setOdometry(const nav_msgs::Odometry& odometry); - - /** - * @brief calculates a twist feedforward command from the current band - * @param refernce to the twist cmd - */ - bool getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached); - bool getTwistDifferentialDrive(geometry_msgs::Twist& twist_cmd, bool& goal_reached); - - private: - - // pointer to external objects (do NOT delete object) - costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap - boost::shared_ptr target_visual_; // pointer to visualization object - - control_toolbox::Pid pid_; - - // parameters - bool differential_drive_hack_; - double k_p_, k_nu_, k_int_, k_diff_, ctrl_freq_; - double acc_max_, virt_mass_; - double max_vel_lin_, max_vel_th_, min_vel_lin_, min_vel_th_; - double min_in_place_vel_th_; - double in_place_trans_vel_; - double tolerance_trans_, tolerance_rot_, tolerance_timeout_; - double acc_max_trans_, acc_max_rot_; - double rotation_correction_threshold_; // We'll do rotation correction if we're at least this far from the goal - - // diff drive only parameters - double bubble_velocity_multiplier_; - double rotation_threshold_multiplier_; - bool in_final_goal_turn_; - - // flags - bool initialized_, band_set_, visualization_; - - // data - std::vector elastic_band_; - geometry_msgs::Twist odom_vel_; - geometry_msgs::Twist last_vel_; - geometry_msgs::Pose ref_frame_band_; - - ///@brief defines sign of a double - inline double sign(double n) - { - return n < 0.0 ? -1.0 : 1.0; - } - - /** - * @brief Transforms Pose of frame 1 and 2 into reference frame and gets difference of frame 1 and 2 - * @param refernce to pose of frame1 - * @param reference to pose of frame2 - * @param reference to pose of reference frame - * @return vector from frame1 to frame2 in coordinates of the reference frame - */ - geometry_msgs::Twist getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame); - geometry_msgs::Twist getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame); - - /** - * @param Transforms twist into a given reference frame - * @param Twist that shall be transformed - * @param refernce to pose of frame1 - * @param reference to pose of frame2 - * @return transformed twist - */ - geometry_msgs::Twist transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist, const geometry_msgs::Pose& frame1, - const geometry_msgs::Pose& frame2); - - /** - * @brief limits the twist to the allowed range - * @param reference to unconstrained twist - * @return twist in allowed range - */ - geometry_msgs::Twist limitTwist(const geometry_msgs::Twist& twist); - - /** - * @brief gets the max velocity allowed within this bubble depending on size of the bubble and pose and size of the following bubble - * @param number of the bubble of interest within the band - * @param band in which the bubble is in - * @return absolute value of maximum allowed velocity within this bubble - */ - double getBubbleTargetVel(const int& target_bub_num, const std::vector& band, geometry_msgs::Twist& VelDir); - -}; + /** + * @class EBandPlanner + * @brief Implements the Elastic Band Method for SE2-Manifold (mobile Base) + */ + class EBandTrajectoryCtrl{ + + public: + + /** + * @brief Default constructor + */ + EBandTrajectoryCtrl(); + + /** + * @brief Constructs the elastic band object + * @param name The name to give this instance of the elastic band local planner + * @param costmap The cost map to use for assigning costs to trajectories + */ + EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Destructor + */ + ~EBandTrajectoryCtrl(); + + /** + * @brief Initializes the elastic band class by accesing costmap and loading parameters + * @param name The name to give this instance of the trajectory planner + * @param costmap The cost map to use for assigning costs to trajectories + */ + void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief passes a reference to the eband visualization object which can be used to visualize the band optimization + * @param pointer to visualization object + */ + void setVisualization(boost::shared_ptr target_visual); + + /** + * @brief This sets the elastic_band to the trajectory controller + * @param reference via which the band is passed + */ + bool setBand(const std::vector& elastic_band); + + /** + * @brief This sets the robot velocity as provided by odometry + * @param reference via which odometry is passed + */ + bool setOdometry(const nav_msgs::Odometry& odometry); + + /** + * @brief calculates a twist feedforward command from the current band + * @param refernce to the twist cmd + */ + bool getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached); + bool getTwistDifferentialDrive(geometry_msgs::Twist& twist_cmd, bool& goal_reached); + + private: + + // pointer to external objects (do NOT delete object) + costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap + boost::shared_ptr target_visual_; // pointer to visualization object + + control_toolbox::Pid pid_; + + // parameters + bool differential_drive_hack_; + double k_p_, k_nu_, k_int_, k_diff_, ctrl_freq_; + double acc_max_, virt_mass_; + double max_vel_lin_, max_vel_th_, min_vel_lin_, min_vel_th_; + double min_in_place_vel_th_; + double in_place_trans_vel_; + double tolerance_trans_, tolerance_rot_, tolerance_timeout_; + double acc_max_trans_, acc_max_rot_; + double rotation_correction_threshold_; // We'll do rotation correction if we're at least this far from the goal + + // diff drive only parameters + double bubble_velocity_multiplier_; + double rotation_threshold_multiplier_; + bool in_final_goal_turn_; + + // flags + bool initialized_, band_set_, visualization_; + + // data + std::vector elastic_band_; + geometry_msgs::Twist odom_vel_; + geometry_msgs::Twist last_vel_; + geometry_msgs::Pose ref_frame_band_; + + ///@brief defines sign of a double + inline double sign(double n) + { + return n < 0.0 ? -1.0 : 1.0; + } + + /** + * @brief Transforms Pose of frame 1 and 2 into reference frame and gets difference of frame 1 and 2 + * @param refernce to pose of frame1 + * @param reference to pose of frame2 + * @param reference to pose of reference frame + * @return vector from frame1 to frame2 in coordinates of the reference frame + */ + geometry_msgs::Twist getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame); + geometry_msgs::Twist getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame); + + /** + * @param Transforms twist into a given reference frame + * @param Twist that shall be transformed + * @param refernce to pose of frame1 + * @param reference to pose of frame2 + * @return transformed twist + */ + geometry_msgs::Twist transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist, const geometry_msgs::Pose& frame1, + const geometry_msgs::Pose& frame2); + + /** + * @brief limits the twist to the allowed range + * @param reference to unconstrained twist + * @return twist in allowed range + */ + geometry_msgs::Twist limitTwist(const geometry_msgs::Twist& twist); + + /** + * @brief gets the max velocity allowed within this bubble depending on size of the bubble and pose and size of the following bubble + * @param number of the bubble of interest within the band + * @param band in which the bubble is in + * @return absolute value of maximum allowed velocity within this bubble + */ + double getBubbleTargetVel(const int& target_bub_num, const std::vector& band, geometry_msgs::Twist& VelDir); + + }; }; #endif diff --git a/include/eband_local_planner/eband_visualization.h b/include/eband_local_planner/eband_visualization.h index 857e29e..8249d3e 100644 --- a/include/eband_local_planner/eband_visualization.h +++ b/include/eband_local_planner/eband_visualization.h @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #ifndef EBAND_VISUALIZATION_H_ #define EBAND_VISUALIZATION_H_ @@ -69,130 +69,130 @@ namespace eband_local_planner{ -/** - * @class ConversionsAndTypes - * @brief Implements type-convrsions and types used by elastic-band optimizer and according ros-wrapper class - */ -class EBandVisualization{ - - public: - - // typedefs - enum Color {blue, red, green}; - - - // methods - - /** - * @brief Default constructor - */ - EBandVisualization(); - - /** - * @brief Construct and initializes eband visualization - */ - EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief Default destructor - */ - ~EBandVisualization(); - - /** - * @brief Initializes the visualization class - * @param name The name to give this instance (important for publishing) - */ - void initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros); - - /** - * @brief publishes the bubbles (Position and Expansion) in a band as Marker-Array - * @param the name space under which the markers shall be bublished - * @param the shape of the markers - * @param the band which shall be published - */ - void publishBand(std::string marker_name_space, std::vector band); - - /** - * @brief publishes a single bubble as a Marker - * @param the name space under which the markers shall be bublished - * @param the shape of the markers - * @param the bubble which shall be published - */ - void publishBubble(std::string marker_name_space, int marker_id, Bubble bubble); - - /** - * @brief publishes a single bubble as a Marker - * @param the name space under which the markers shall be bublished - * @param the shape of the markers - * @param the bubble which shall be published - * @param color in which the bubble shall be displayed - */ - void publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble); - - /** - * @brief publishes the list of forces along the band as Marker-Array - * @param the name space under which the markers shall be bublished - * @param the shape of the markers - * @param the list of forces which shall be published - * @param the list of bubbles on which the forces act (needed to get origin of force-vector) - */ - void publishForceList(std::string marker_name_space, std::vector forces, std::vector band); - - /** - * @brief publishes a single force as a Marker - * @param the name space under which the markers shall be bublished - * @param the shape of the markers - * @param the force which shall be published - */ - void publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble); - - private: - - // external objects - costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap - needed to retrieve information about robot geometry - - // Topics & Services - ros::Publisher bubble_pub_; ///<@brief publishes markers to visualize bubbles of elastic band ("modified global plan") - ros::Publisher one_bubble_pub_; ///<@brief publishes markers to visualize bubbles of elastic band ("modified global plan") - - // flags - bool initialized_; - - // parameters - double marker_lifetime_; - - - // methods - - /** - * @brief converts a bubble into a Marker msg - this is visualization-specific - * @param the bubble to convert - * @param reference to hand back the marker - * @param name space under which the marker shall be published - * @param object id of the marker in its name space - */ - void bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); - - /** - * @brief converts the haeding of a bubble into a Arrow-Marker msg - this is visualization-specific - * @param the bubble to convert - * @param reference to hand back the marker - * @param name space under which the marker shall be published - * @param object id of the marker in its name space - */ - void bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); - - /** - * @brief converts a wrench into a Marker msg - this is visualization-specific - * @param the wrench to convert - * @param origin of force or wrench - * @param reference to hand back the marker - * @param name space under which the marker shall be published - * @param object id of the marker in its name space - * @param color in which the marker shall be displayed - */ - void forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); - -}; + /** + * @class ConversionsAndTypes + * @brief Implements type-convrsions and types used by elastic-band optimizer and according ros-wrapper class + */ + class EBandVisualization{ + + public: + + // typedefs + enum Color {blue, red, green}; + + + // methods + + /** + * @brief Default constructor + */ + EBandVisualization(); + + /** + * @brief Construct and initializes eband visualization + */ + EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Default destructor + */ + ~EBandVisualization(); + + /** + * @brief Initializes the visualization class + * @param name The name to give this instance (important for publishing) + */ + void initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief publishes the bubbles (Position and Expansion) in a band as Marker-Array + * @param the name space under which the markers shall be bublished + * @param the shape of the markers + * @param the band which shall be published + */ + void publishBand(std::string marker_name_space, std::vector band); + + /** + * @brief publishes a single bubble as a Marker + * @param the name space under which the markers shall be bublished + * @param the shape of the markers + * @param the bubble which shall be published + */ + void publishBubble(std::string marker_name_space, int marker_id, Bubble bubble); + + /** + * @brief publishes a single bubble as a Marker + * @param the name space under which the markers shall be bublished + * @param the shape of the markers + * @param the bubble which shall be published + * @param color in which the bubble shall be displayed + */ + void publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble); + + /** + * @brief publishes the list of forces along the band as Marker-Array + * @param the name space under which the markers shall be bublished + * @param the shape of the markers + * @param the list of forces which shall be published + * @param the list of bubbles on which the forces act (needed to get origin of force-vector) + */ + void publishForceList(std::string marker_name_space, std::vector forces, std::vector band); + + /** + * @brief publishes a single force as a Marker + * @param the name space under which the markers shall be bublished + * @param the shape of the markers + * @param the force which shall be published + */ + void publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble); + + private: + + // external objects + costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap - needed to retrieve information about robot geometry + + // Topics & Services + ros::Publisher bubble_pub_; ///<@brief publishes markers to visualize bubbles of elastic band ("modified global plan") + ros::Publisher one_bubble_pub_; ///<@brief publishes markers to visualize bubbles of elastic band ("modified global plan") + + // flags + bool initialized_; + + // parameters + double marker_lifetime_; + + + // methods + + /** + * @brief converts a bubble into a Marker msg - this is visualization-specific + * @param the bubble to convert + * @param reference to hand back the marker + * @param name space under which the marker shall be published + * @param object id of the marker in its name space + */ + void bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); + + /** + * @brief converts the haeding of a bubble into a Arrow-Marker msg - this is visualization-specific + * @param the bubble to convert + * @param reference to hand back the marker + * @param name space under which the marker shall be published + * @param object id of the marker in its name space + */ + void bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); + + /** + * @brief converts a wrench into a Marker msg - this is visualization-specific + * @param the wrench to convert + * @param origin of force or wrench + * @param reference to hand back the marker + * @param name space under which the marker shall be published + * @param object id of the marker in its name space + * @param color in which the marker shall be displayed + */ + void forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color); + + }; }; #endif diff --git a/src/conversions_and_types.cpp b/src/conversions_and_types.cpp index d2d1462..e6bde9f 100644 --- a/src/conversions_and_types.cpp +++ b/src/conversions_and_types.cpp @@ -1,228 +1,225 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ - + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #include - namespace eband_local_planner{ - -void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D) -{ - // use tf-pkg to convert angles + void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D) + { + // use tf-pkg to convert angles tf::Pose pose_tf; - - // convert geometry_msgs::PoseStamped to tf::Pose - tf::poseMsgToTF(pose, pose_tf); - // now get Euler-Angles from pose_tf + // convert geometry_msgs::PoseStamped to tf::Pose + tf::poseMsgToTF(pose, pose_tf); + + // now get Euler-Angles from pose_tf double useless_pitch, useless_roll, yaw; pose_tf.getBasis().getEulerYPR(yaw, useless_pitch, useless_roll); - // normalize angle - yaw = angles::normalize_angle(yaw); + // normalize angle + yaw = angles::normalize_angle(yaw); - // and set to pose2D - pose2D.x = pose.position.x; - pose2D.y = pose.position.y; - pose2D.theta = yaw; + // and set to pose2D + pose2D.x = pose.position.x; + pose2D.y = pose.position.y; + pose2D.theta = yaw; - return; -} + return; + } -void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D) -{ - // use tf-pkg to convert angles - tf::Quaternion frame_quat; + void Pose2DToPose(geometry_msgs::Pose& pose, const geometry_msgs::Pose2D pose2D) + { + // use tf-pkg to convert angles + tf::Quaternion frame_quat; - // transform angle from euler-angle to quaternion representation - frame_quat = tf::createQuaternionFromYaw(pose2D.theta); + // transform angle from euler-angle to quaternion representation + frame_quat = tf::createQuaternionFromYaw(pose2D.theta); - // set position - pose.position.x = pose2D.x; - pose.position.y = pose2D.y; - pose.position.z = 0.0; + // set position + pose.position.x = pose2D.x; + pose.position.y = pose2D.y; + pose.position.z = 0.0; - // set quaternion - pose.orientation.x = frame_quat.x(); - pose.orientation.y = frame_quat.y(); - pose.orientation.z = frame_quat.z(); - pose.orientation.w = frame_quat.w(); + // set quaternion + pose.orientation.x = frame_quat.x(); + pose.orientation.y = frame_quat.y(); + pose.orientation.z = frame_quat.z(); + pose.orientation.w = frame_quat.w(); - return; -} + return; + } -bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, - costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, - std::vector& transformed_plan, std::vector& start_end_counts) -{ - const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; - - // initiate refernce variables - transformed_plan.clear(); - - try - { - if (!global_plan.size() > 0) - { - ROS_ERROR("Recieved plan with zero length"); - return false; - } - - tf::StampedTransform transform; - tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, - plan_pose.header.frame_id, transform); - - //let's get the pose of the robot in the frame of the plan - tf::Stamped robot_pose; - robot_pose.setIdentity(); - robot_pose.frame_id_ = costmap.getBaseFrameID(); - robot_pose.stamp_ = ros::Time(); - tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose); - - //we'll keep points on the plan that are within the window that we're looking at - - double dist_threshold = std::max( - costmap.getCostmap()->getSizeInMetersX() / 2.0, - costmap.getCostmap()->getSizeInMetersY() / 2.0 - ); - - unsigned int i = 0; - double sq_dist_threshold = dist_threshold * dist_threshold; - double sq_dist = DBL_MAX; - - // --- start - modification w.r.t. base_local_planner - // initiate start_end_count - std::vector start_end_count; - start_end_count.assign(2, 0); - - // we know only one direction and that is forward! - initiate search with previous start_end_counts - // this is neccesserry to work with the sampling based planners - path may severall time enter and leave moving window - ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) ); - i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0); - // --- end - modification w.r.t. base_local_planner - - //we need to loop to a point on the plan that is within a certain distance of the robot - while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold) - { - double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; - double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; - sq_dist = x_diff * x_diff + y_diff * y_diff; - - // --- start - modification w.r.t. base_local_planner - // not yet in reach - get next frame - if( sq_dist > sq_dist_threshold ) - ++i; - else - // set counter for start of transformed intervall - from back as beginning of plan might be prunned - start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i); - // --- end - modification w.r.t. base_local_planner - } - - - tf::Stamped tf_pose; - geometry_msgs::PoseStamped newer_pose; - - //now we'll transform until points are outside of our distance threshold - while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold) - { - double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; - double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; - sq_dist = x_diff * x_diff + y_diff * y_diff; - - const geometry_msgs::PoseStamped& pose = global_plan[i]; - poseStampedMsgToTF(pose, tf_pose); - tf_pose.setData(transform * tf_pose); - tf_pose.stamp_ = transform.stamp_; - tf_pose.frame_id_ = global_frame; - poseStampedTFToMsg(tf_pose, newer_pose); - - transformed_plan.push_back(newer_pose); - - // --- start - modification w.r.t. base_local_planner - // set counter for end of transformed intervall - from back as beginning of plan might be prunned - start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i); - // --- end - modification w.r.t. base_local_planner - - ++i; - } - - // --- start - modification w.r.t. base_local_planner - // write to reference variable - start_end_counts = start_end_count; - // --- end - modification w.r.t. base_local_planner + bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, + costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, + std::vector& transformed_plan, std::vector& start_end_counts) + { + const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; + + // initiate refernce variables + transformed_plan.clear(); + + try + { + if (!global_plan.size() > 0) + { + ROS_ERROR("Recieved plan with zero length"); + return false; + } + + tf::StampedTransform transform; + tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, transform); + + //let's get the pose of the robot in the frame of the plan + tf::Stamped robot_pose; + robot_pose.setIdentity(); + robot_pose.frame_id_ = costmap.getBaseFrameID(); + robot_pose.stamp_ = ros::Time(); + tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose); + + //we'll keep points on the plan that are within the window that we're looking at + + double dist_threshold = std::max( + costmap.getCostmap()->getSizeInMetersX() / 2.0, + costmap.getCostmap()->getSizeInMetersY() / 2.0 + ); + + unsigned int i = 0; + double sq_dist_threshold = dist_threshold * dist_threshold; + double sq_dist = DBL_MAX; + + // --- start - modification w.r.t. base_local_planner + // initiate start_end_count + std::vector start_end_count; + start_end_count.assign(2, 0); + + // we know only one direction and that is forward! - initiate search with previous start_end_counts + // this is neccesserry to work with the sampling based planners - path may severall time enter and leave moving window + ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) ); + i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0); + // --- end - modification w.r.t. base_local_planner + + //we need to loop to a point on the plan that is within a certain distance of the robot + while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold) + { + double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // --- start - modification w.r.t. base_local_planner + // not yet in reach - get next frame + if( sq_dist > sq_dist_threshold ) + ++i; + else + // set counter for start of transformed intervall - from back as beginning of plan might be prunned + start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i); + // --- end - modification w.r.t. base_local_planner + } + + + tf::Stamped tf_pose; + geometry_msgs::PoseStamped newer_pose; + + //now we'll transform until points are outside of our distance threshold + while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold) + { + double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + const geometry_msgs::PoseStamped& pose = global_plan[i]; + poseStampedMsgToTF(pose, tf_pose); + tf_pose.setData(transform * tf_pose); + tf_pose.stamp_ = transform.stamp_; + tf_pose.frame_id_ = global_frame; + poseStampedTFToMsg(tf_pose, newer_pose); + + transformed_plan.push_back(newer_pose); + + // --- start - modification w.r.t. base_local_planner + // set counter for end of transformed intervall - from back as beginning of plan might be prunned + start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i); + // --- end - modification w.r.t. base_local_planner + + ++i; + } + + // --- start - modification w.r.t. base_local_planner + // write to reference variable + start_end_counts = start_end_count; + // --- end - modification w.r.t. base_local_planner } - catch(tf::LookupException& ex) - { - ROS_ERROR("No Transform available Error: %s\n", ex.what()); - return false; - } - catch(tf::ConnectivityException& ex) - { - ROS_ERROR("Connectivity Error: %s\n", ex.what()); - return false; - } - catch(tf::ExtrapolationException& ex) - { - ROS_ERROR("Extrapolation Error: %s\n", ex.what()); - if (global_plan.size() > 0) - ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str()); - - return false; - } - - return true; -} + catch(tf::LookupException& ex) + { + ROS_ERROR("No Transform available Error: %s\n", ex.what()); + return false; + } + catch(tf::ConnectivityException& ex) + { + ROS_ERROR("Connectivity Error: %s\n", ex.what()); + return false; + } + catch(tf::ExtrapolationException& ex) + { + ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + if (global_plan.size() > 0) + ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str()); + + return false; + } + + return true; + } -double getCircumscribedRadius(costmap_2d::Costmap2DROS& costmap) { - std::vector footprint(costmap.getRobotFootprint()); - double max_distance_sqr = 0; - for (size_t i = 0; i < footprint.size(); ++i) { - geometry_msgs::Point& p = footprint[i]; - double distance_sqr = p.x*p.x + p.y*p.y; - if (distance_sqr > max_distance_sqr) { - max_distance_sqr = distance_sqr; + double getCircumscribedRadius(costmap_2d::Costmap2DROS& costmap) { + std::vector footprint(costmap.getRobotFootprint()); + double max_distance_sqr = 0; + for (size_t i = 0; i < footprint.size(); ++i) { + geometry_msgs::Point& p = footprint[i]; + double distance_sqr = p.x*p.x + p.y*p.y; + if (distance_sqr > max_distance_sqr) { + max_distance_sqr = distance_sqr; + } } + return sqrt(max_distance_sqr); } - return sqrt(max_distance_sqr); -} } diff --git a/src/eband_local_planner.cpp b/src/eband_local_planner.cpp index 855841c..6ea9c3d 100644 --- a/src/eband_local_planner.cpp +++ b/src/eband_local_planner.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #include @@ -41,1894 +41,1894 @@ namespace eband_local_planner{ -EBandPlanner::EBandPlanner() : costmap_ros_(NULL), initialized_(false) {} + EBandPlanner::EBandPlanner() : costmap_ros_(NULL), initialized_(false) {} -EBandPlanner::EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) - : costmap_ros_(NULL), initialized_(false) -{ - // initialize planner - initialize(name, costmap_ros); -} + EBandPlanner::EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) + : costmap_ros_(NULL), initialized_(false) + { + // initialize planner + initialize(name, costmap_ros); + } -EBandPlanner::~EBandPlanner() -{ - delete world_model_; -} + EBandPlanner::~EBandPlanner() + { + delete world_model_; + } -void EBandPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) -{ - // check if the plugin is already initialized - if(!initialized_) - { - // copy adress of costmap (handed over from move_base via eband wrapper) - costmap_ros_ = costmap_ros; + void EBandPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) + { + // check if the plugin is already initialized + if(!initialized_) + { + // copy adress of costmap (handed over from move_base via eband wrapper) + costmap_ros_ = costmap_ros; - // get a pointer to the underlying costmap - costmap_ = costmap_ros_->getCostmap(); + // get a pointer to the underlying costmap + costmap_ = costmap_ros_->getCostmap(); - // create world model from costmap - world_model_ = new base_local_planner::CostmapModel(*costmap_); + // create world model from costmap + world_model_ = new base_local_planner::CostmapModel(*costmap_); - // get footprint of the robot - footprint_spec_ = costmap_ros_->getRobotFootprint(); + // get footprint of the robot + footprint_spec_ = costmap_ros_->getRobotFootprint(); - // create Node Handle with name of plugin (as used in move_base for loading) - ros::NodeHandle pn("~/" + name); + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle pn("~/" + name); - // read parameters from parameter server - // connectivity checking - pn.param("eband_min_relative_bubble_overlap_", min_bubble_overlap_, 0.7); + // read parameters from parameter server + // connectivity checking + pn.param("eband_min_relative_bubble_overlap_", min_bubble_overlap_, 0.7); - // bubble geometric bounds - pn.param("eband_tiny_bubble_distance", tiny_bubble_distance_, 0.01); - pn.param("eband_tiny_bubble_expansion", tiny_bubble_expansion_, 0.01); + // bubble geometric bounds + pn.param("eband_tiny_bubble_distance", tiny_bubble_distance_, 0.01); + pn.param("eband_tiny_bubble_expansion", tiny_bubble_expansion_, 0.01); - // optimization - force calculation - pn.param("eband_internal_force_gain", internal_force_gain_, 1.0); - pn.param("eband_external_force_gain", external_force_gain_, 2.0); - pn.param("num_iterations_eband_optimization", num_optim_iterations_, 3); + // optimization - force calculation + pn.param("eband_internal_force_gain", internal_force_gain_, 1.0); + pn.param("eband_external_force_gain", external_force_gain_, 2.0); + pn.param("num_iterations_eband_optimization", num_optim_iterations_, 3); - // recursive approximation of bubble equilibrium position based - pn.param("eband_equilibrium_approx_max_recursion_depth", max_recursion_depth_approx_equi_, 4); - pn.param("eband_equilibrium_relative_overshoot", equilibrium_relative_overshoot_, 0.75); - pn.param("eband_significant_force_lower_bound", significant_force_, 0.15); + // recursive approximation of bubble equilibrium position based + pn.param("eband_equilibrium_approx_max_recursion_depth", max_recursion_depth_approx_equi_, 4); + pn.param("eband_equilibrium_relative_overshoot", equilibrium_relative_overshoot_, 0.75); + pn.param("eband_significant_force_lower_bound", significant_force_, 0.15); - // use this parameter if a different weight is supplied to the costmap in dyn reconfigure - pn.param("costmap_weight", costmap_weight_, 10.0); + // use this parameter if a different weight is supplied to the costmap in dyn reconfigure + pn.param("costmap_weight", costmap_weight_, 10.0); - // clean up band - elastic_band_.clear(); + // clean up band + elastic_band_.clear(); - // set initialized flag - initialized_ = true; + // set initialized flag + initialized_ = true; - // set flag whether visualization availlable to false by default - visualization_ = false; - } - else - { - ROS_WARN("This planner has already been initialized, doing nothing."); - } -} + // set flag whether visualization availlable to false by default + visualization_ = false; + } + else + { + ROS_WARN("This planner has already been initialized, doing nothing."); + } + } -void EBandPlanner::setVisualization(boost::shared_ptr eband_visual) -{ - eband_visual_ = eband_visual; + void EBandPlanner::setVisualization(boost::shared_ptr eband_visual) + { + eband_visual_ = eband_visual; + + visualization_ = true; + } - visualization_ = true; -} + bool EBandPlanner::setPlan(const std::vector& global_plan) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + + // check if plan valid (minimum 2 frames) + if(global_plan.size() < 2) + { + ROS_ERROR("Attempt to pass empty path to optimization. Valid path needs to have at least 2 Frames. This one has %d.", ((int) global_plan.size()) ); + return false; + } + // copy plan to local member variable + global_plan_ = global_plan; -bool EBandPlanner::setPlan(const std::vector& global_plan) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - - // check if plan valid (minimum 2 frames) - if(global_plan.size() < 2) - { - ROS_ERROR("Attempt to pass empty path to optimization. Valid path needs to have at least 2 Frames. This one has %d.", ((int) global_plan.size()) ); - return false; - } - // copy plan to local member variable - global_plan_ = global_plan; - - - // check whether plan and costmap are in the same frame - if(global_plan.front().header.frame_id != costmap_ros_->getGlobalFrameID()) - { + + // check whether plan and costmap are in the same frame + if(global_plan.front().header.frame_id != costmap_ros_->getGlobalFrameID()) + { ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the plan was sent in the %s frame.", costmap_ros_->getGlobalFrameID().c_str(), global_plan.front().header.frame_id.c_str()); return false; } - - // convert frames in path into bubbles in band -> sets center of bubbles and calculates expansion - ROS_DEBUG("Converting Plan to Band"); - if(!convertPlanToBand(global_plan_, elastic_band_)) - { - ROS_WARN("Conversion from plan to elastic band failed. Plan probably not collision free. Plan not set for optimization"); - // TODO try to do local repairs of band - return false; - } + // convert frames in path into bubbles in band -> sets center of bubbles and calculates expansion + ROS_DEBUG("Converting Plan to Band"); + if(!convertPlanToBand(global_plan_, elastic_band_)) + { + ROS_WARN("Conversion from plan to elastic band failed. Plan probably not collision free. Plan not set for optimization"); + // TODO try to do local repairs of band + return false; + } + + + // close gaps and remove redundant bubbles + ROS_DEBUG("Refining Band"); + if(!refineBand(elastic_band_)) + { + ROS_WARN("Band is broken. Could not close gaps in converted path. Path not set. Global replanning needed"); + return false; + } - // close gaps and remove redundant bubbles - ROS_DEBUG("Refining Band"); - if(!refineBand(elastic_band_)) - { - ROS_WARN("Band is broken. Could not close gaps in converted path. Path not set. Global replanning needed"); - return false; - } + ROS_DEBUG("Refinement done - Band set."); + return true; + } - ROS_DEBUG("Refinement done - Band set."); - return true; -} + bool EBandPlanner::getPlan(std::vector& global_plan) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } -bool EBandPlanner::getPlan(std::vector& global_plan) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // check if there is a band - if(elastic_band_.empty()) - { - ROS_WARN("Band is empty. There was no path successfully set so far."); - return false; - } - - // convert band to plan - if(!convertBandToPlan(global_plan, elastic_band_)) - { - ROS_WARN("Conversion from Elastic Band to path failed."); - return false; - } - - return true; -} + // check if there is a band + if(elastic_band_.empty()) + { + ROS_WARN("Band is empty. There was no path successfully set so far."); + return false; + } + + // convert band to plan + if(!convertBandToPlan(global_plan, elastic_band_)) + { + ROS_WARN("Conversion from Elastic Band to path failed."); + return false; + } + return true; + } -bool EBandPlanner::getBand(std::vector& elastic_band) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - elastic_band = elastic_band_; + bool EBandPlanner::getBand(std::vector& elastic_band) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } - // check if there is a band - if(elastic_band_.empty()) - { - ROS_WARN("Band is empty."); - return false; - } + elastic_band = elastic_band_; - return true; -} + // check if there is a band + if(elastic_band_.empty()) + { + ROS_WARN("Band is empty."); + return false; + } + + return true; + } + + + bool EBandPlanner::addFrames(const std::vector& plan_to_add, const AddAtPosition& add_frames_at) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + // check that there is a plan at all (minimum 1 frame in this case, as robot + goal = plan) + if(elastic_band_.size() < 1) + { + ROS_WARN("Attempt to connect path to empty band. path not connected. Use SetPath instead"); + return false; + } -bool EBandPlanner::addFrames(const std::vector& plan_to_add, const AddAtPosition& add_frames_at) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // check that there is a plan at all (minimum 1 frame in this case, as robot + goal = plan) - if(elastic_band_.size() < 1) - { - ROS_WARN("Attempt to connect path to empty band. path not connected. Use SetPath instead"); - return false; - } - - //check that plan which shall be added is not empty - if(plan_to_add.empty()) - { - ROS_WARN("Attempt to connect empty path to band. Nothing to do here."); - return false; - } - - // check whether plan and costmap are in the same frame - if(plan_to_add.at(0).header.frame_id != costmap_ros_->getGlobalFrameID()) - { + //check that plan which shall be added is not empty + if(plan_to_add.empty()) + { + ROS_WARN("Attempt to connect empty path to band. Nothing to do here."); + return false; + } + + // check whether plan and costmap are in the same frame + if(plan_to_add.at(0).header.frame_id != costmap_ros_->getGlobalFrameID()) + { ROS_ERROR("Elastic Band expects robot pose for optimization in the %s frame, the pose was sent in the %s frame.", costmap_ros_->getGlobalFrameID().c_str(), plan_to_add.at(0).header.frame_id.c_str()); return false; } - // convert plan to band - std::vector band_to_add; - if(!convertPlanToBand(plan_to_add, band_to_add)) - { - ROS_DEBUG("Conversion from plan to elastic band failed. Plan not appended"); - // TODO try to do local repairs of band - return false; - } - - - // connect frames to existing band - ROS_DEBUG("Checking for connections between current band and new bubbles"); - bool connected = false; - int bubble_connect = -1; - if(add_frames_at == add_front) - { - // add frames at the front of the current band - // - for instance to connect band and current robot position - for(int i = ((int) elastic_band_.size() - 1); i >= 0; i--) - { - // cycle over bubbles from End - connect to bubble furthest away but overlapping - if(checkOverlap(band_to_add.back(), elastic_band_.at(i))) - { - bubble_connect = i; - connected = true; - break; - } - } - } - else - { - // add frames at the end of the current band - // - for instance to connect new frames entering the moving window - for(int i = 0; i < ((int) elastic_band_.size() - 1); i++) - { - // cycle over bubbles from Start - connect to bubble furthest away but overlapping - if(checkOverlap(band_to_add.front(), elastic_band_.at(i))) - { - bubble_connect = i; - connected = true; - break; - } - } - } - - // intanstiate local copy of band - std::vector tmp_band; - std::vector::iterator tmp_iter1, tmp_iter2; - // copy new frames to tmp_band - tmp_band.assign(band_to_add.begin(), band_to_add.end()); - - if(connected) - { - ROS_DEBUG("Connections found - composing new band by connecting new frames to bubble %d", bubble_connect); - if(add_frames_at == add_front) - { - // compose new vector by appending elastic_band to new frames - tmp_iter1 = elastic_band_.begin() + bubble_connect; - ROS_ASSERT( (tmp_iter1 >= elastic_band_.begin()) && (tmp_iter1 < elastic_band_.end()) ); - tmp_band.insert(tmp_band.end(), tmp_iter1, elastic_band_.end()); - } - else - { - // compose new vector by pre-appending elastic_band to new frames - tmp_iter1 = elastic_band_.begin() + bubble_connect + 1; // +1 - as insert only appends [start, end) - ROS_ASSERT( (tmp_iter1 > elastic_band_.begin()) && (tmp_iter1 <= elastic_band_.end()) ); - tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), tmp_iter1); - } - - // done - elastic_band_ = tmp_band; - return true; - } - - // otherwise, we need to do some more work - add complete band to tmp_band - ROS_DEBUG("No direct connection found - Composing tmp band and trying to fill gap"); - if(add_frames_at == add_front) - { - // compose new vector by appending elastic_band to new frames - tmp_band.insert(tmp_band.end(), elastic_band_.begin(), elastic_band_.end()); - // and get iterators to connecting bubbles - tmp_iter1 = tmp_band.begin() + ((int) band_to_add.size()) - 1; - tmp_iter2 = tmp_iter1 + 1; - } - else - { - // compose new vector by pre-appending elastic_band to new frames - tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), elastic_band_.end()); - // and get iterators to connecting bubbles - tmp_iter1 = tmp_band.begin() + ((int) elastic_band_.size()) - 1; - tmp_iter2 = tmp_iter1 + 1; - } - - // just in case - ROS_ASSERT( tmp_iter1 >= tmp_band.begin() ); - ROS_ASSERT( tmp_iter2 < tmp_band.end() ); - ROS_ASSERT( tmp_iter1 < tmp_iter2 ); - if(!fillGap(tmp_band, tmp_iter1, tmp_iter2)) - { - // we could not connect band and robot at its current position - ROS_DEBUG("Could not connect robot pose to band - Failed to fill gap."); - return false; - } - - // otherwise - done - elastic_band_ = tmp_band; - - return true; -} + // convert plan to band + std::vector band_to_add; + if(!convertPlanToBand(plan_to_add, band_to_add)) + { + ROS_DEBUG("Conversion from plan to elastic band failed. Plan not appended"); + // TODO try to do local repairs of band + return false; + } -bool EBandPlanner::optimizeBand() -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // check if there is a band - if(elastic_band_.empty()) - { - ROS_ERROR("Band is empty. Probably Band has not been set yet"); - return false; - } - - // call optimization with member elastic_band_ - ROS_DEBUG("Starting optimization of band"); - if(!optimizeBand(elastic_band_)) - { - ROS_DEBUG("Aborting Optimization. Changes discarded."); - return false; - } - - ROS_DEBUG("Elastic Band - Optimization successfull!"); - return true; -} + // connect frames to existing band + ROS_DEBUG("Checking for connections between current band and new bubbles"); + bool connected = false; + int bubble_connect = -1; + if(add_frames_at == add_front) + { + // add frames at the front of the current band + // - for instance to connect band and current robot position + for(int i = ((int) elastic_band_.size() - 1); i >= 0; i--) + { + // cycle over bubbles from End - connect to bubble furthest away but overlapping + if(checkOverlap(band_to_add.back(), elastic_band_.at(i))) + { + bubble_connect = i; + connected = true; + break; + } + } + } + else + { + // add frames at the end of the current band + // - for instance to connect new frames entering the moving window + for(int i = 0; i < ((int) elastic_band_.size() - 1); i++) + { + // cycle over bubbles from Start - connect to bubble furthest away but overlapping + if(checkOverlap(band_to_add.front(), elastic_band_.at(i))) + { + bubble_connect = i; + connected = true; + break; + } + } + } + + // intanstiate local copy of band + std::vector tmp_band; + std::vector::iterator tmp_iter1, tmp_iter2; + // copy new frames to tmp_band + tmp_band.assign(band_to_add.begin(), band_to_add.end()); + + if(connected) + { + ROS_DEBUG("Connections found - composing new band by connecting new frames to bubble %d", bubble_connect); + if(add_frames_at == add_front) + { + // compose new vector by appending elastic_band to new frames + tmp_iter1 = elastic_band_.begin() + bubble_connect; + ROS_ASSERT( (tmp_iter1 >= elastic_band_.begin()) && (tmp_iter1 < elastic_band_.end()) ); + tmp_band.insert(tmp_band.end(), tmp_iter1, elastic_band_.end()); + } + else + { + // compose new vector by pre-appending elastic_band to new frames + tmp_iter1 = elastic_band_.begin() + bubble_connect + 1; // +1 - as insert only appends [start, end) + ROS_ASSERT( (tmp_iter1 > elastic_band_.begin()) && (tmp_iter1 <= elastic_band_.end()) ); + tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), tmp_iter1); + } + + // done + elastic_band_ = tmp_band; + return true; + } + + // otherwise, we need to do some more work - add complete band to tmp_band + ROS_DEBUG("No direct connection found - Composing tmp band and trying to fill gap"); + if(add_frames_at == add_front) + { + // compose new vector by appending elastic_band to new frames + tmp_band.insert(tmp_band.end(), elastic_band_.begin(), elastic_band_.end()); + // and get iterators to connecting bubbles + tmp_iter1 = tmp_band.begin() + ((int) band_to_add.size()) - 1; + tmp_iter2 = tmp_iter1 + 1; + } + else + { + // compose new vector by pre-appending elastic_band to new frames + tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), elastic_band_.end()); + // and get iterators to connecting bubbles + tmp_iter1 = tmp_band.begin() + ((int) elastic_band_.size()) - 1; + tmp_iter2 = tmp_iter1 + 1; + } + + // just in case + ROS_ASSERT( tmp_iter1 >= tmp_band.begin() ); + ROS_ASSERT( tmp_iter2 < tmp_band.end() ); + ROS_ASSERT( tmp_iter1 < tmp_iter2 ); + if(!fillGap(tmp_band, tmp_iter1, tmp_iter2)) + { + // we could not connect band and robot at its current position + ROS_DEBUG("Could not connect robot pose to band - Failed to fill gap."); + return false; + } + + // otherwise - done + elastic_band_ = tmp_band; + return true; + } + + + bool EBandPlanner::optimizeBand() + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // check if there is a band + if(elastic_band_.empty()) + { + ROS_ERROR("Band is empty. Probably Band has not been set yet"); + return false; + } + + // call optimization with member elastic_band_ + ROS_DEBUG("Starting optimization of band"); + if(!optimizeBand(elastic_band_)) + { + ROS_DEBUG("Aborting Optimization. Changes discarded."); + return false; + } -bool EBandPlanner::optimizeBand(std::vector& band) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } + ROS_DEBUG("Elastic Band - Optimization successfull!"); + return true; + } + + + bool EBandPlanner::optimizeBand(std::vector& band) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } - // check whether band and costmap are in the same frame - if(band.front().center.header.frame_id != costmap_ros_->getGlobalFrameID()) - { + // check whether band and costmap are in the same frame + if(band.front().center.header.frame_id != costmap_ros_->getGlobalFrameID()) + { ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the plan was sent in the %s frame.", costmap_ros_->getGlobalFrameID().c_str(), band.front().center.header.frame_id.c_str()); return false; } - double distance; - for(int i = 0; i < ((int) band.size()); i++) - { - // update Size of Bubbles in band by calculating Dist to nearest Obstacle [depends kinematic, environment] - if(!calcObstacleKinematicDistance(band.at(i).center.pose, distance)) - { - ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d Probably outside map coordinates.", - i, ((int) band.size()) ); - return false; - } - - if(distance == 0.0) - { - // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid - ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Trying to refine band.", - i, ((int) band.size()) ); - // TODO if frame in collision try to repair band instead of aborting everything - return false; - } - - band.at(i).expansion = distance; - } - - // close gaps and remove redundant bubbles - if(!refineBand(band)) - { - ROS_DEBUG("Elastic Band is broken. Could not close gaps in band. Global replanning needed."); - return false; - } - - // get a copy of current (valid) band - std::vector tmp_band = band; - - // now optimize iteratively (for instance by miminizing the energy-function of the full system) - for(int i = 0; i < num_optim_iterations_; i++) - { - ROS_DEBUG("Inside optimization: Cycle no %d", i); - - // calculate forces and apply changes - if(!modifyBandArtificialForce(tmp_band)) - { - ROS_DEBUG("Optimization failed while trying to modify Band."); - // something went wrong -> discard changes and stop process - return false; - } - - // check whether band still valid - refine if neccesarry - if(!refineBand(tmp_band)) - { - ROS_DEBUG("Optimization failed while trying to refine modified band"); - // modified band is not valid anymore -> discard changes and stop process - return false; - } - } - - // copy changes back to band - band = tmp_band; - return true; -} + double distance; + for(int i = 0; i < ((int) band.size()); i++) + { + // update Size of Bubbles in band by calculating Dist to nearest Obstacle [depends kinematic, environment] + if(!calcObstacleKinematicDistance(band.at(i).center.pose, distance)) + { + ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d Probably outside map coordinates.", + i, ((int) band.size()) ); + return false; + } + + if(distance == 0.0) + { + // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid + ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Trying to refine band.", + i, ((int) band.size()) ); + // TODO if frame in collision try to repair band instead of aborting everything + return false; + } + + band.at(i).expansion = distance; + } + // close gaps and remove redundant bubbles + if(!refineBand(band)) + { + ROS_DEBUG("Elastic Band is broken. Could not close gaps in band. Global replanning needed."); + return false; + } -// private methods - -bool EBandPlanner::refineBand(std::vector& band) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // check if band valid (minimum 2 bubbles) - if(band.size() < 2) - { - ROS_WARN("Attempt to convert empty band to plan. Valid band needs to have at least 2 Frames. This one has %d.", ((int) band.size()) ); - return false; - } - - // instantiate local variables - bool success; - std::vector tmp_band; - std::vector::iterator start_iter, end_iter; - - // remove redundant Bubbles and fill gabs recursively - tmp_band = band; - start_iter = tmp_band.begin(); - end_iter = (tmp_band.end() - 1); // -1 because .end() points "past the end"! - - success = removeAndFill(tmp_band, start_iter, end_iter); - - if(!success) - ROS_DEBUG("Band is broken. Could not close gaps."); - else - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Recursive filling and removing DONE"); - #endif - band = tmp_band; - } - - return success; -} + // get a copy of current (valid) band + std::vector tmp_band = band; + + // now optimize iteratively (for instance by miminizing the energy-function of the full system) + for(int i = 0; i < num_optim_iterations_; i++) + { + ROS_DEBUG("Inside optimization: Cycle no %d", i); + + // calculate forces and apply changes + if(!modifyBandArtificialForce(tmp_band)) + { + ROS_DEBUG("Optimization failed while trying to modify Band."); + // something went wrong -> discard changes and stop process + return false; + } + + // check whether band still valid - refine if neccesarry + if(!refineBand(tmp_band)) + { + ROS_DEBUG("Optimization failed while trying to refine modified band"); + // modified band is not valid anymore -> discard changes and stop process + return false; + } + } + // copy changes back to band + band = tmp_band; + return true; + } -bool EBandPlanner::removeAndFill(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter) -{ - // instantiate local variables - bool overlap; - std::vector::iterator tmp_iter; - int mid_int, diff_int; - - #ifdef DEBUG_EBAND_ - int debug_dist_start, debug_dist_iters; - debug_dist_start = std::distance(band.begin(), start_iter); - debug_dist_iters = std::distance(start_iter, end_iter); - ROS_DEBUG("Refining Recursive - Check if Bubbles %d and %d overlapp. Total size of band %d.", debug_dist_start, (debug_dist_start + debug_dist_iters), ((int) band.size()) ); - #endif - - // check that iterators are still valid - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( start_iter < end_iter ); - - // check whether start and end bubbles of this intervall overlap - overlap = checkOverlap(*start_iter, *end_iter); - - if(overlap) - { - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Bubbles overlapp, check for redundancies"); - #endif - - // if there are bubbles between start and end of intervall remove them (they are redundant as start and end of intervall do overlap) - if((start_iter + 1) < end_iter) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Bubbles overlapp, removing Bubbles %d to %d.", (debug_dist_start + 1), (debug_dist_start + debug_dist_iters -1)); - #endif - - // erase bubbles between start and end (but not start and end themself) and get new iterator to end (old one is invalid) - tmp_iter = band.erase((start_iter+1), end_iter); - - // write back changed iterator pointing to the end of the intervall - end_iter = tmp_iter; - } - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Bubbles overlapp - DONE"); - #endif - - // we are done here (leaf of this branch is reached) - return true; - } - - - // if bubbles do not overlap -> check whether there are still bubbles between start and end - if((start_iter + 1) < end_iter) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper"); - #endif - - // split remaining sequence of bubbles - // get distance between start and end iterator for this intervall - mid_int = std::distance(start_iter, end_iter); - mid_int = mid_int/2; // division by integer implies floor (round down) - - // now get iterator pointing to the middle (roughly) - tmp_iter = start_iter + mid_int; - // and realative position of end_iter to tmp_iter - diff_int = (int) std::distance(tmp_iter, end_iter); - - // after all this arithmetics - check that iterators are still valid - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( start_iter < end_iter ); - - - // and call removeAndFill recursively for the left intervall - if(!removeAndFill(band, start_iter, tmp_iter)) - { - // band is broken in this intervall and could not be fixed - return false; - } - - // carefull at this point!!! if we filled in or removed bubbles end_iter is not valid anymore - // but the relative position towards tmp_iter is still the same and tmp_iter was kept valid in the lower recursion steps - end_iter = tmp_iter + diff_int; - - // check that iterators are still valid - one more time - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); - - - // o.k. we are done with left hand intervall now do the same for the right hand intervall - // but first get relative position of start and tmp iter - diff_int = (int) std::distance(start_iter, tmp_iter); - if(!removeAndFill(band, tmp_iter, end_iter)) - { - // band is broken in this intervall and could not be fixed - return false; - } - - // if we filled in bubbles vector might have been reallocated -> start_iter might be invalid - start_iter = tmp_iter - diff_int; - - // check that iterators are still valid - almost done - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); - - - // we reached the leaf but we are not yet done - // -> we know that there are no redundant elements in the left intervall taken on its own - // -> and we know the same holds for the right intervall - // but the middle bubble itself might be redundant -> check it - if(checkOverlap(*(tmp_iter-1), *(tmp_iter+1))) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Removing middle bubble"); - #endif - - // again: get distance between (tmp_iter + 1) and end_iter, (+1 because we will erase tmp_iter itself) - diff_int = (int) std::distance((tmp_iter + 1), end_iter); - - // remove middle bubble and correct end_iter - tmp_iter = band.erase(tmp_iter); - end_iter = tmp_iter + diff_int; - } - - // check that iterators are still valid - almost almost - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( start_iter < end_iter ); - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper DONE"); - #endif - - //now we are done with this case - return true; - } - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Gap detected, fill recursive"); - #endif - - // last possible case -> bubbles do not overlap AND there are nor bubbles in between -> try to fill gap recursively - if(!fillGap(band, start_iter, end_iter)) - { - // band is broken in this intervall and could not be fixed (this should only be called on a leaf, so we put a log_out here;) - ROS_DEBUG("Failed to fill gap between bubble %d and %d.", (int) distance(band.begin(), start_iter), (int) distance(band.begin(), end_iter) ); - return false; - } - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Refining Recursive - Gap detected, fill recursive DONE"); - #endif - - // we could fill the gap (reached leaf of this branch) - return true; -} + // private methods -bool EBandPlanner::fillGap(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter) -{ - // insert bubbles in the middle between not-overlapping bubbles (e.g. (Dist > Size Bub1) && (Dist > Size Bub2) ) - // repeat until gaps are closed - - // instantiate local variables - double distance = 0.0; - Bubble interpolated_bubble; - geometry_msgs::PoseStamped interpolated_center; - std::vector::iterator tmp_iter; - int diff_int, start_num, end_num; - - // make sure this method was called for a valid element in the forces or bubbles vector - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( start_iter < end_iter ); - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - interpolate"); - #endif - - // interpolate between bubbles [depends kinematic] - if(!interpolateBubbles(start_iter->center, end_iter->center, interpolated_center)) - { - // interpolation failed (for whatever reason), so return with false - start_num = std::distance(band.begin(), start_iter); - end_num = std::distance(band.begin(), end_iter); - ROS_DEBUG("Interpolation failed while trying to fill gap between bubble %d and %d.", start_num, end_num); - return false; - } - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - calc expansion of interpolated bubble"); - #endif - - // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] - if(!calcObstacleKinematicDistance(interpolated_center.pose, distance)) - { - // pose probably outside map coordinates - start_num = std::distance(band.begin(), start_iter); - end_num = std::distance(band.begin(), end_iter); - ROS_DEBUG("Calculation of Distance failed for interpolated bubble - failed to fill gap between bubble %d and %d.", start_num, end_num); - return false; - } - - if(distance <= tiny_bubble_expansion_) - { - // band broken! frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid - start_num = std::distance(band.begin(), start_iter); - end_num = std::distance(band.begin(), end_iter); - ROS_DEBUG("Interpolated Bubble in Collision - failed to fill gap between bubble %d and %d.", start_num, end_num); - // TODO this means only that there is an obstacle on the direct interconnection between the bubbles - think about repair or rescue strategies - - return false; - } - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - inserting interpolated bubble at (%f, %f), with expansion %f", interpolated_center.pose.position.x, interpolated_center.pose.position.y, distance); - #endif - - // insert bubble and assign center and expansion - interpolated_bubble.center = interpolated_center; - interpolated_bubble.expansion = distance; - // insert bubble (vector.insert() inserts elements before given iterator) and get iterator pointing to it - tmp_iter = band.insert(end_iter, interpolated_bubble); - // insert is a little bit more tricky than erase, as it may require reallocation of the vector -> start and end iter could be invalid - start_iter = tmp_iter - 1; - end_iter = tmp_iter + 1; - - // check that iterators are still valid - just in case :) - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - check overlap interpolated bubble and first bubble"); - #endif - - // we have now two intervalls (left and right of inserted bubble) which need to be checked again and filled if neccessary - if(!checkOverlap(*start_iter, *tmp_iter)) - { - - #ifdef DEBUG_EBAND - ROS_DEBUG("Fill recursive - gap btw. interpolated and first bubble - fill recursive"); - #endif - - // gap in left intervall -> try to fill - if(!fillGap(band, start_iter, tmp_iter)) - { - // band is broken in this intervall and could not be fixed - return false; - } - // bubbles were inserted -> make sure to keep end_iter iterator valid - end_iter = tmp_iter + 1; - } - - // check that iterators are still valid - just in case :) - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - check overlap interpolated bubble and second bubble"); - #endif - - if(!checkOverlap(*tmp_iter, *end_iter)) - { - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - gap btw. interpolated and second bubble - fill recursive"); - #endif - - // get distance between start_iter and tmp_iter before filling right intervall (in case of reallocation of vector) - diff_int = (int) std::distance(start_iter, tmp_iter); - - // gap in left intervall -> try to fill - if(!fillGap(band, tmp_iter, end_iter)) - { - // band is broken in this intervall and could not be fixed - return false; - } - // bubbles were inserted -> make sure to keep start_iter iterator valid - start_iter = tmp_iter - diff_int; - } - - // check that iterators are still valid - just in case :) - ROS_ASSERT( start_iter >= band.begin() ); - ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector - ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Fill recursive - gap closed"); - #endif - - // bubbles overlap, iterators are kept valid -> done - return true; -} + bool EBandPlanner::refineBand(std::vector& band) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + // check if band valid (minimum 2 bubbles) + if(band.size() < 2) + { + ROS_WARN("Attempt to convert empty band to plan. Valid band needs to have at least 2 Frames. This one has %d.", ((int) band.size()) ); + return false; + } -// optimization - -bool EBandPlanner::modifyBandArtificialForce(std::vector& band) -{ - if(band.empty()) - { - ROS_ERROR("Trying to modify an empty band."); - return false; - } - - if(band.size() <= 2) - { - // nothing to do here -> we can stop right away - return true; - } - - std::vector internal_forces, external_forces, forces; - geometry_msgs::WrenchStamped wrench; - - #ifdef DEBUG_EBAND_ - //publish original band - if(visualization_) - eband_visual_->publishBand("bubbles", band); - #endif - - // init variables to calm down debug warnings - wrench.header.stamp = ros::Time::now(); - wrench.header.frame_id = band[0].center.header.frame_id; - wrench.wrench.force.x = 0.0; - wrench.wrench.force.y = 0.0; - wrench.wrench.force.z = 0.0; - wrench.wrench.torque.x = 0.0; - wrench.wrench.torque.y = 0.0; - wrench.wrench.torque.z = 0.0; - internal_forces.assign(band.size(), wrench); - external_forces = internal_forces; - forces = internal_forces; - - // TODO log timigs of planner - // instantiate variables for timing - //ros::Time time_stamp1, time_stamp2; - //ros::Duration duration; - //time_stamp1 = ros::Time::now(); - - // due to refinement band might change its size -> use while loop - int i = 1; - bool forward = true; // cycle 1xforwards and 1xbackwards through band - while( (i>0) && (i < ((int) band.size() - 1)) ) - { - ROS_DEBUG("Modifying bubble %d.", i); - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Calculating internal force for bubble %d.", i); - #endif - - if(!calcInternalForces(i, band, band.at(i), internal_forces.at(i))) - { - // calculation of internal forces failed - stopping optimization - ROS_DEBUG("Calculation of internal forces failed"); - return false; - } - - #ifdef DEBUG_EBAND_ - if(visualization_) - // publish internal forces - eband_visual_->publishForce("internal_forces", i, eband_visual_->blue, internal_forces[i], band[i]); - // Log out debug info about next step - ROS_DEBUG("Calculating external force for bubble %d.", i); - #endif - - - //if(!calcExternalForces(i, band, external_forces)) - if(!calcExternalForces(i, band.at(i), external_forces.at(i))) - { - // calculation of External Forces failed - stopping optimization - ROS_DEBUG("Calculation of external forces failed"); - return false; - } - - #ifdef DEBUG_EBAND_ - if(visualization_) - //publish external forces - eband_visual_->publishForce("external_forces", i, eband_visual_->red, external_forces[i], band[i]); - // Log out debug info about next step - ROS_DEBUG("Superposing internal and external forces"); - #endif - - - // sum up external and internal forces over all bubbles - forces.at(i).wrench.force.x = internal_forces.at(i).wrench.force.x + external_forces.at(i).wrench.force.x; - forces.at(i).wrench.force.y = internal_forces.at(i).wrench.force.y + external_forces.at(i).wrench.force.y; - forces.at(i).wrench.force.z = internal_forces.at(i).wrench.force.z + external_forces.at(i).wrench.force.z; - - forces.at(i).wrench.torque.x = internal_forces.at(i).wrench.torque.x + external_forces.at(i).wrench.torque.x; - forces.at(i).wrench.torque.y = internal_forces.at(i).wrench.torque.y + external_forces.at(i).wrench.torque.y; - forces.at(i).wrench.torque.z = internal_forces.at(i).wrench.torque.z + external_forces.at(i).wrench.torque.z; - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Superpose forces: (x, y, theta) = (%f, %f, %f)", forces.at(i).wrench.force.x, forces.at(i).wrench.force.y, forces.at(i).wrench.torque.z); - ROS_DEBUG("Supressing tangential forces"); - #endif - - if(!suppressTangentialForces(i, band, forces.at(i))) - { - // suppression of tangential forces failed - ROS_DEBUG("Supression of tangential forces failed"); - return false; - } - - #ifdef DEBUG_EBAND_ - if(visualization_) - //publish resulting forces - eband_visual_->publishForce("resulting_forces", i, eband_visual_->green, forces[i], band[i]); - #endif - - - ROS_DEBUG("Applying forces to modify band"); - if(!applyForces(i, band, forces)) - { - // band invalid - ROS_DEBUG("Band is invalid - Stopping Modification"); - return false; - } - - #ifdef DEBUG_EBAND_ - if(visualization_) - { - // publish band with changed bubble at resulting position - eband_visual_->publishBand("bubbles", band); - ros::Duration(0.01).sleep(); - } - #endif - - - //next bubble - if(forward) - { - i++; - if(i == ((int) band.size() - 1)) - { - // reached end of band - start backwards cycle until at start again - then stop - forward = false; - i--; - ROS_DEBUG("Optimization Elastic Band - Forward cycle done, starting backward cycle"); - } - } - else - { - i--; - } - } - - return true; -} + // instantiate local variables + bool success; + std::vector tmp_band; + std::vector::iterator start_iter, end_iter; + + // remove redundant Bubbles and fill gabs recursively + tmp_band = band; + start_iter = tmp_band.begin(); + end_iter = (tmp_band.end() - 1); // -1 because .end() points "past the end"! + + success = removeAndFill(tmp_band, start_iter, end_iter); + + if(!success) + ROS_DEBUG("Band is broken. Could not close gaps."); + else + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Recursive filling and removing DONE"); +#endif + band = tmp_band; + } + return success; + } -bool EBandPlanner::applyForces(int bubble_num, std::vector& band, std::vector forces) -{ - //cycle over all bubbles except first and last (these are fixed) - if(band.size() <= 2) - { - // nothing to do here -> we can stop right away - no forces calculated - return true; - } - - geometry_msgs::Pose2D bubble_pose2D, new_bubble_pose2D; - geometry_msgs::Pose bubble_pose, new_bubble_pose; - geometry_msgs::Twist bubble_jump; - Bubble new_bubble = band.at(bubble_num); - double distance; - - - // move bubble - bubble_pose = band.at(bubble_num).center.pose; - PoseToPose2D(bubble_pose, bubble_pose2D); - - // move according to bubble_new = bubble_old + alpha*force -> we choose alpha to be the current expansion of the modified bubble - bubble_jump.linear.x = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.x; - bubble_jump.linear.y = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.y; - bubble_jump.linear.z = 0.0; - bubble_jump.angular.x = 0.0; - bubble_jump.angular.y = 0.0; - bubble_jump.angular.z = band.at(bubble_num).expansion/getCircumscribedRadius(*costmap_ros_) * forces.at(bubble_num).wrench.torque.z; - bubble_jump.angular.z = angles::normalize_angle(bubble_jump.angular.z); - - // apply changes to calc tmp bubble position - new_bubble_pose2D.x = bubble_pose2D.x + bubble_jump.linear.x; - new_bubble_pose2D.y = bubble_pose2D.y + bubble_jump.linear.y; - new_bubble_pose2D.theta = bubble_pose2D.theta + bubble_jump.angular.z; - new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta); - - // apply changes to local copy - Pose2DToPose(new_bubble_pose, new_bubble_pose2D); - new_bubble.center.pose = new_bubble_pose; - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f).", bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta, - bubble_jump.linear.x, bubble_jump.linear.y, bubble_jump.angular.z); - #endif - - - // check validity of moved bubble - - // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] - if(!calcObstacleKinematicDistance(new_bubble_pose, distance)) - { - ROS_DEBUG("Calculation of Distance failed. Frame %d of %d Probably outside map. Discarding Changes", bubble_num, ((int) band.size()) ); - - #ifdef DEBUG_EBAND_ - if(visualization_) - eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble); - #endif - - // this bubble must not be changed, but band is still valid -> continue with other bubbles - return true; - } - - if(distance <= tiny_bubble_expansion_) - { - // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid - ROS_DEBUG("Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Discarding Changes", bubble_num, ((int) band.size()) ); - - #ifdef DEBUG_EBAND_ - if(visualization_) - eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble); - #endif - - // this bubble must not be changed, but band is still valid -> continue with other bubbles - return true; - } - - // so far o.k. -> assign distance to new bubble - new_bubble.expansion = distance; - - - // check whether step was reasonable - - geometry_msgs::WrenchStamped new_bubble_force = forces.at(bubble_num); - - // check whether we get a valid force calculation here - if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force)) - { - // error during calculation of forces for the new position - discard changes - ROS_DEBUG("Cannot calculate forces on bubble %d at new position - discarding changes", bubble_num); - return true; - } - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Check for zero-crossings in force on bubble %d", bubble_num); - #endif - - // check for zero-crossings in the force-vector - double checksum_zero, abs_new_force, abs_old_force; - - // project force-vectors onto each other - checksum_zero = (new_bubble_force.wrench.force.x * forces.at(bubble_num).wrench.force.x) + - (new_bubble_force.wrench.force.y * forces.at(bubble_num).wrench.force.y) + - (new_bubble_force.wrench.torque.z * forces.at(bubble_num).wrench.torque.z); - - // if sign changes and ... - if(checksum_zero < 0.0) - { - ROS_DEBUG("Detected zero-crossings in force on bubble %d. Checking total change in force.", bubble_num); - // check the absolute values of the two vectors - abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) + - (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) + - (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) ); - abs_old_force = sqrt( (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) + - (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) + - (forces.at(bubble_num).wrench.torque.z * forces.at(bubble_num).wrench.torque.z) ); - - // force still has a significant high value (> ~75% of old force by default) - if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) ) - { - ROS_DEBUG("Detected significante change in force (%f to %f) on bubble %d. Entering Recursive Approximation.", abs_old_force, abs_new_force, bubble_num); - // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point - int curr_recursion_depth = 0; - geometry_msgs::Twist new_step_width; - Bubble curr_bubble = band.at(bubble_num); - geometry_msgs::WrenchStamped curr_bubble_force = forces.at(bubble_num); - - // half step size - new_step_width.linear.x = 0.5*bubble_jump.linear.x; - new_step_width.linear.y = 0.5*bubble_jump.linear.y; - new_step_width.linear.z = 0.5*bubble_jump.linear.z; - new_step_width.angular.x = 0.5*bubble_jump.angular.x; - new_step_width.angular.y = 0.5*bubble_jump.angular.y; - new_step_width.angular.z = 0.5*bubble_jump.angular.z; - - // one step deeper into the recursion - if(moveApproximateEquilibrium(bubble_num, band, curr_bubble, curr_bubble_force, new_step_width, curr_recursion_depth)) - { - // done with recursion - change bubble and hand it back - new_bubble = curr_bubble; - - #ifdef DEBUG_EBAND_ - geometry_msgs::Pose2D curr_bubble_pose2D; - PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D); - ROS_DEBUG("Instead - Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f) to (%f, %f, %f).", - bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta, - new_step_width.linear.x, new_step_width.linear.y, new_step_width.angular.z, - curr_bubble_pose2D.x, curr_bubble_pose2D.y, curr_bubble_pose2D.theta); - #endif - } - } - } - - - // check validity of resulting band (given the moved bubble) - - // TODO use this routine not only to check whether gap can be filled but also to fill gap (if possible) - // get local copy of band, set new position of moved bubble and init iterators - std::vector tmp_band = band; - std::vector::iterator start_iter, end_iter; - tmp_band.at(bubble_num) = new_bubble; - start_iter = tmp_band.begin(); - - // check left connection (bubble and bubble-1) - start_iter = start_iter + bubble_num - 1; - end_iter = start_iter + 1; - - // check Overlap - if bubbles do not overlap try to fill gap - if(!checkOverlap(*start_iter, *end_iter)) - { - if(!fillGap(tmp_band, start_iter, end_iter)) - { - ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes."); - // this bubble must not be changed, but band is still valid -> continue with other bubbles - return true; - } - } - - - // get fresh copy of band, set new position of bubble again and reinit iterators - tmp_band = band; - tmp_band.at(bubble_num) = new_bubble; - start_iter = tmp_band.begin(); - - // check right connection (bubble and bubble +1) - start_iter = start_iter + bubble_num; - end_iter = start_iter + 1; - - // check Overlap - if bubbles do not overlap try to fill gap - if(!checkOverlap(*start_iter, *end_iter)) - { - if(!fillGap(tmp_band, start_iter, end_iter)) - { - ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes."); - // this bubble must not be changed, but band is still valid -> continue with other bubbles - return true; - } - } - - - // check successful - bubble and band valid apply changes - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Frame %d of %d: Check successful - bubble and band valid. Applying Changes", bubble_num, ((int) band.size()) ); - #endif - - band.at(bubble_num) = new_bubble; - - return true; -} + bool EBandPlanner::removeAndFill(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter) + { + // instantiate local variables + bool overlap; + std::vector::iterator tmp_iter; + int mid_int, diff_int; -bool EBandPlanner::moveApproximateEquilibrium(const int& bubble_num, const std::vector& band, Bubble& curr_bubble, - const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width, const int& curr_recursion_depth) -{ - - double distance; - Bubble new_bubble = curr_bubble; - geometry_msgs::Pose2D new_bubble_pose2D, curr_bubble_pose2D; - geometry_msgs::WrenchStamped new_bubble_force = curr_bubble_force; - - // move bubble - PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D); - PoseToPose2D(new_bubble.center.pose, new_bubble_pose2D); - - // apply changes to calculate tmp bubble position - new_bubble_pose2D.x = curr_bubble_pose2D.x + curr_step_width.linear.x; - new_bubble_pose2D.y = curr_bubble_pose2D.y + curr_step_width.linear.y; - new_bubble_pose2D.theta = curr_bubble_pose2D.theta + curr_step_width.angular.z; - new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta); - - // apply changes to local copy - Pose2DToPose(new_bubble.center.pose, new_bubble_pose2D); - - - // check validity of moved bubble - - // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] - if(!calcObstacleKinematicDistance(new_bubble.center.pose, distance)) - return false; - - // we wont be able to calculate forces later on - if(distance == 0.0) - return false; - - - // so far o.k. -> assign distance to new bubble - new_bubble.expansion = distance; - - // check whether we get a valid force calculation here - if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force)) - return false; - - // great - lets store this - this is better then what we had so far - curr_bubble = new_bubble; - - // if everything is fine and we reached our maximum recursion depth - if(curr_recursion_depth >= max_recursion_depth_approx_equi_) - // break recursion at this point - return true; - - - // now - let's check for zero-crossing - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Check for zero-crossings in force on bubble %d - Recursion %d", bubble_num, curr_recursion_depth); - #endif - - double checksum_zero, abs_new_force, abs_old_force; - int new_recursion_depth; - geometry_msgs::Twist new_step_width; - - // check zero-crossing by projecting force-vectors onto each other - checksum_zero = (new_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + - (new_bubble_force.wrench.force.y * curr_bubble_force.wrench.force.y) + - (new_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z); - - if(checksum_zero < 0.0) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Detected zero-crossings in force on bubble %d - Recursion %d. Checking total change in force.", bubble_num, curr_recursion_depth); - #endif - - // check the absolute values of the two vectors - abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) + - (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) + - (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) ); - abs_old_force = sqrt( (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + - (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + - (curr_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z) ); - - if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) ) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Detected significant change in force (%f to %f) on bubble %d - Recursion %d. Going one Recursion deeper.", abs_old_force, abs_new_force, bubble_num, curr_recursion_depth); - #endif - - // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point - new_recursion_depth = curr_recursion_depth + 1; - // half step size - backward direction - new_step_width.linear.x = -0.5*curr_step_width.linear.x; - new_step_width.linear.y = -0.5*curr_step_width.linear.y; - new_step_width.linear.z = -0.5*curr_step_width.linear.z; - new_step_width.angular.x = -0.5*curr_step_width.angular.x; - new_step_width.angular.y = -0.5*curr_step_width.angular.y; - new_step_width.angular.z = -0.5*curr_step_width.angular.z; - - // one step deeper into the recursion - if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth)) - // done with recursion - change bubble and hand it back - curr_bubble = new_bubble; - - // otherwise - could not get a better value - return without change (curr_bubble as asigned above) - } - - // otherwise - this is good enough for us - return with this value (curr_bubble as asigned above) - } - else - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("No zero-crossings in force on bubble %d - Recursion %d. Continue walk in same direction. Going one recursion deeper.", bubble_num, curr_recursion_depth); - #endif - - // continue walk in same direction - new_recursion_depth = curr_recursion_depth + 1; - // half step size - backward direction - new_step_width.linear.x = 0.5*curr_step_width.linear.x; - new_step_width.linear.y = 0.5*curr_step_width.linear.y; - new_step_width.linear.z = 0.5*curr_step_width.linear.z; - new_step_width.angular.x = 0.5*curr_step_width.angular.x; - new_step_width.angular.y = 0.5*curr_step_width.angular.y; - new_step_width.angular.z = 0.5*curr_step_width.angular.z; - - // one step deeper into the recursion - if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth)) - // done with recursion - change bubble and hand it back - curr_bubble = new_bubble; - - // otherwise - could not get a better value - return without change (curr_bubble as asigned above) - } - - // done - return true; -} +#ifdef DEBUG_EBAND_ + int debug_dist_start, debug_dist_iters; + debug_dist_start = std::distance(band.begin(), start_iter); + debug_dist_iters = std::distance(start_iter, end_iter); + ROS_DEBUG("Refining Recursive - Check if Bubbles %d and %d overlapp. Total size of band %d.", debug_dist_start, (debug_dist_start + debug_dist_iters), ((int) band.size()) ); +#endif + // check that iterators are still valid + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( start_iter < end_iter ); -bool EBandPlanner::getForcesAt(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) -{ - geometry_msgs::WrenchStamped internal_force, external_force; - - if(!calcInternalForces(bubble_num, band, curr_bubble, internal_force)) - { - // calculation of internal forces failed - stopping optimization - ROS_DEBUG("Calculation of internal forces failed"); - return false; - } - - if(!calcExternalForces(bubble_num, curr_bubble, external_force)) - { - // calculation of External Forces failed - stopping optimization - ROS_DEBUG("Calculation of external forces failed"); - return false; - } - - // sum up external and internal forces over all bubbles - forces.wrench.force.x = internal_force.wrench.force.x + external_force.wrench.force.x; - forces.wrench.force.y = internal_force.wrench.force.y + external_force.wrench.force.y; - forces.wrench.force.z = internal_force.wrench.force.z + external_force.wrench.force.z; - - forces.wrench.torque.x = internal_force.wrench.torque.x + external_force.wrench.torque.x; - forces.wrench.torque.y = internal_force.wrench.torque.y + external_force.wrench.torque.y; - forces.wrench.torque.z = internal_force.wrench.torque.z + external_force.wrench.torque.z; - - if(!suppressTangentialForces(bubble_num, band, forces)) - { - // suppression of tangential forces failed - ROS_DEBUG("Supression of tangential forces failed"); - return false; - } - - return true; -} + // check whether start and end bubbles of this intervall overlap + overlap = checkOverlap(*start_iter, *end_iter); + if(overlap) + { -bool EBandPlanner::calcInternalForces(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - //cycle over all bubbles except first and last (these are fixed) - if(band.size() <= 2) - { - // nothing to do here -> we can stop right away - no forces calculated - return true; - } - - // init tmp variables - double distance1, distance2; - geometry_msgs::Twist difference1, difference2; - geometry_msgs::Wrench wrench; - - // make sure this method was called for a valid element in the forces or bubbles vector - ROS_ASSERT( bubble_num > 0 ); - ROS_ASSERT( bubble_num < ((int) band.size() - 1) ); - - - // get distance between bubbles - if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num-1].center.pose, distance1)) - { - ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!"); - return false; - } - - if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num+1].center.pose, distance2)) - { - ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!"); - return false; - } - - // get (elementwise) difference bewtween bubbles - if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num-1].center.pose, difference1)) - { - ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!"); - return false; - } - - if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num+1].center.pose, difference2)) - { - ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!"); - return false; - } - - // make sure to avoid division by (almost) zero during force calculation (avoid numerical problems) - // -> if difference/distance is (close to) zero then the force in this direction should be zero as well - if(distance1 <= tiny_bubble_distance_) - distance1 = 1000000.0; - if(distance2 <= tiny_bubble_distance_) - distance2 = 1000000.0; - - // now calculate wrench - forces model an elastic band and are normed (distance) to render forces for small and large bubbles the same - wrench.force.x = internal_force_gain_*(difference1.linear.x/distance1 + difference2.linear.x/distance2); - wrench.force.y = internal_force_gain_*(difference1.linear.y/distance1 + difference2.linear.y/distance2); - wrench.force.z = internal_force_gain_*(difference1.linear.z/distance1 + difference2.linear.z/distance2); - wrench.torque.x = internal_force_gain_*(difference1.angular.x/distance1 + difference2.angular.x/distance2); - wrench.torque.y = internal_force_gain_*(difference1.angular.y/distance1 + difference2.angular.y/distance2); - wrench.torque.z = internal_force_gain_*(difference1.angular.z/distance1 + difference2.angular.z/distance2); - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Calculating internal forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z); - #endif - - // store wrench in vector - forces.wrench = wrench; - - return true; -} +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Bubbles overlapp, check for redundancies"); +#endif + // if there are bubbles between start and end of intervall remove them (they are redundant as start and end of intervall do overlap) + if((start_iter + 1) < end_iter) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Bubbles overlapp, removing Bubbles %d to %d.", (debug_dist_start + 1), (debug_dist_start + debug_dist_iters -1)); +#endif -bool EBandPlanner::calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // init tmp variables - double distance1, distance2; - geometry_msgs::Pose edge; - geometry_msgs::Pose2D edge_pose2D; - geometry_msgs::Wrench wrench; - - - // calculate delta-poses (on upper edge of bubble) for x-direction - edge = curr_bubble.center.pose; - edge.position.x = edge.position.x + curr_bubble.expansion; - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance1)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - // calculate delta-poses (on lower edge of bubble) for x-direction - edge.position.x = edge.position.x - 2.0*curr_bubble.expansion; - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance2)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - - // calculate difference-quotient (approx. of derivative) in x-direction - if(curr_bubble.expansion <= tiny_bubble_expansion_) - { - // avoid division by (almost) zero to avoid numerical problems - wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); - // actually we should never end up here - band should have been considered as broken - ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); - } - else - wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); - // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponding term - - - // calculate delta-poses (on upper edge of bubble) for y-direction - edge = curr_bubble.center.pose; - edge.position.y = edge.position.y + curr_bubble.expansion; - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance1)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - // calculate delta-poses (on lower edge of bubble) for x-direction - edge.position.y = edge.position.y - 2.0*curr_bubble.expansion; - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance2)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - - // calculate difference-quotient (approx. of derivative) in x-direction - if(curr_bubble.expansion <= tiny_bubble_expansion_) - { - // avoid division by (almost) zero to avoid numerical problems - wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); - // actually we should never end up here - band should have been considered as broken - ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); - } - else - wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); - // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term - - - // no force in z-direction - wrench.force.z = 0.0; - - - // no torque around x and y axis - wrench.torque.x = 0.0; - wrench.torque.y = 0.0; - - - // calculate delta-poses (on upper edge of bubble) for x-direction - PoseToPose2D(curr_bubble.center.pose, edge_pose2D); - edge_pose2D.theta = edge_pose2D.theta + (curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_)); - edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta); - PoseToPose2D(edge, edge_pose2D); - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance1)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - // calculate delta-poses (on lower edge of bubble) for x-direction - edge_pose2D.theta = edge_pose2D.theta - 2.0*(curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_)); - edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta); - PoseToPose2D(edge, edge_pose2D); - // get expansion on bubble at this point - if(!calcObstacleKinematicDistance(edge, distance2)) - { - ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); - // we cannot calculate external forces for this bubble - but still continue for the other bubbles - return true; - } - - // calculate difference-quotient (approx. of derivative) in x-direction - if(curr_bubble.expansion <= tiny_bubble_expansion_) - { - // avoid division by (almost) zero to avoid numerical problems - wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); - // actually we should never end up here - band should have been considered as broken - ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); - } - else - wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); - // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term - - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Calculating external forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z); - #endif - - // assign wrench to forces vector - forces.wrench = wrench; - - return true; -} + // erase bubbles between start and end (but not start and end themself) and get new iterator to end (old one is invalid) + tmp_iter = band.erase((start_iter+1), end_iter); + // write back changed iterator pointing to the end of the intervall + end_iter = tmp_iter; + } -bool EBandPlanner::suppressTangentialForces(int bubble_num, std::vector band, geometry_msgs::WrenchStamped& forces) -{ - //cycle over all bubbles except first and last (these are fixed) - if(band.size() <= 2) - { - // nothing to do here -> we can stop right away - no forces calculated - return true; - } - - double scalar_fd, scalar_dd; - geometry_msgs::Twist difference; - - // make sure this method was called for a valid element in the forces or bubbles vector - ROS_ASSERT( bubble_num > 0 ); - ROS_ASSERT( bubble_num < ((int) band.size() - 1) ); - - - // get pose-difference from following to preceding bubble -> "direction of the band in this bubble" - if(!calcBubbleDifference(band[bubble_num+1].center.pose, band[bubble_num-1].center.pose, difference)) - return false; - - // "project wrench" in middle bubble onto connecting vector - // scalar wrench*difference - scalar_fd = forces.wrench.force.x*difference.linear.x + forces.wrench.force.y*difference.linear.y + - forces.wrench.force.z*difference.linear.z + forces.wrench.torque.x*difference.angular.x + - forces.wrench.torque.y*difference.angular.y + forces.wrench.torque.z*difference.angular.z; - - // abs of difference-vector: scalar difference*difference - scalar_dd = difference.linear.x*difference.linear.x + difference.linear.y*difference.linear.y + difference.linear.z*difference.linear.z + - difference.angular.x*difference.angular.x + difference.angular.y*difference.angular.y + difference.angular.z*difference.angular.z; - - // avoid division by (almost) zero -> check if bubbles have (almost) same center-pose - if(scalar_dd <= tiny_bubble_distance_) - { - // there are redundant bubbles, this should normally not hapen -> probably error in band refinement - ROS_DEBUG("Calculating tangential forces for redundant bubbles. Bubble should have been removed. Local Planner probably ill configured"); - } - - // calculate orthogonal components - forces.wrench.force.x = forces.wrench.force.x - scalar_fd/scalar_dd * difference.linear.x; - forces.wrench.force.y = forces.wrench.force.y - scalar_fd/scalar_dd * difference.linear.y; - forces.wrench.force.z = forces.wrench.force.z - scalar_fd/scalar_dd * difference.linear.z; - forces.wrench.torque.x = forces.wrench.torque.x - scalar_fd/scalar_dd * difference.angular.x; - forces.wrench.torque.y = forces.wrench.torque.y - scalar_fd/scalar_dd * difference.angular.y; - forces.wrench.torque.z = forces.wrench.torque.z - scalar_fd/scalar_dd * difference.angular.z; - - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Supressing tangential forces: (x, y, theta) = (%f, %f, %f)", - forces.wrench.force.x, forces.wrench.force.y, forces.wrench.torque.z); - #endif - - return true; -} +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Bubbles overlapp - DONE"); +#endif + // we are done here (leaf of this branch is reached) + return true; + } -// problem (geometry) dependant functions - -bool EBandPlanner::interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // instantiate local variables - geometry_msgs::Pose2D start_pose2D, end_pose2D, interpolated_pose2D; - double delta_theta; - - // copy header - interpolated_center.header = start_center.header; - - // interpolate angles - // TODO make this in a better way - // - for instance use "slerp" to interpolate directly between quaternions - // - or work with pose2D right from the beginnning - // convert quaternions to euler angles - at this point this no longer works in 3D !! - PoseToPose2D(start_center.pose, start_pose2D); - PoseToPose2D(end_center.pose, end_pose2D); - // calc mean of theta angle - delta_theta = end_pose2D.theta - start_pose2D.theta; - delta_theta = angles::normalize_angle(delta_theta) / 2.0; - interpolated_pose2D.theta = start_pose2D.theta + delta_theta; - interpolated_pose2D.theta = angles::normalize_angle(interpolated_pose2D.theta); - // convert back to quaternion - interpolated_pose2D.x = 0.0; - interpolated_pose2D.y = 0.0; - Pose2DToPose(interpolated_center.pose, interpolated_pose2D); - - // interpolate positions - interpolated_center.pose.position.x = (end_center.pose.position.x + start_center.pose.position.x)/2.0; - interpolated_center.pose.position.y = (end_center.pose.position.y + start_center.pose.position.y)/2.0; - interpolated_center.pose.position.z = (end_center.pose.position.z + start_center.pose.position.z)/2.0; - - // TODO ideally this would take into account kinematics of the robot and for instance use splines - - return true; -} + // if bubbles do not overlap -> check whether there are still bubbles between start and end + if((start_iter + 1) < end_iter) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper"); +#endif + + // split remaining sequence of bubbles + // get distance between start and end iterator for this intervall + mid_int = std::distance(start_iter, end_iter); + mid_int = mid_int/2; // division by integer implies floor (round down) + + // now get iterator pointing to the middle (roughly) + tmp_iter = start_iter + mid_int; + // and realative position of end_iter to tmp_iter + diff_int = (int) std::distance(tmp_iter, end_iter); + + // after all this arithmetics - check that iterators are still valid + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( start_iter < end_iter ); + + + // and call removeAndFill recursively for the left intervall + if(!removeAndFill(band, start_iter, tmp_iter)) + { + // band is broken in this intervall and could not be fixed + return false; + } + + // carefull at this point!!! if we filled in or removed bubbles end_iter is not valid anymore + // but the relative position towards tmp_iter is still the same and tmp_iter was kept valid in the lower recursion steps + end_iter = tmp_iter + diff_int; + + // check that iterators are still valid - one more time + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); + + + // o.k. we are done with left hand intervall now do the same for the right hand intervall + // but first get relative position of start and tmp iter + diff_int = (int) std::distance(start_iter, tmp_iter); + if(!removeAndFill(band, tmp_iter, end_iter)) + { + // band is broken in this intervall and could not be fixed + return false; + } + + // if we filled in bubbles vector might have been reallocated -> start_iter might be invalid + start_iter = tmp_iter - diff_int; + + // check that iterators are still valid - almost done + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); + + + // we reached the leaf but we are not yet done + // -> we know that there are no redundant elements in the left intervall taken on its own + // -> and we know the same holds for the right intervall + // but the middle bubble itself might be redundant -> check it + if(checkOverlap(*(tmp_iter-1), *(tmp_iter+1))) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Removing middle bubble"); +#endif + + // again: get distance between (tmp_iter + 1) and end_iter, (+1 because we will erase tmp_iter itself) + diff_int = (int) std::distance((tmp_iter + 1), end_iter); + + // remove middle bubble and correct end_iter + tmp_iter = band.erase(tmp_iter); + end_iter = tmp_iter + diff_int; + } + + // check that iterators are still valid - almost almost + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( start_iter < end_iter ); + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper DONE"); +#endif + + //now we are done with this case + return true; + } -bool EBandPlanner::checkOverlap(Bubble bubble1, Bubble bubble2) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // calc (kinematic) Distance between bubbles - double distance = 0.0; - if(!calcBubbleDistance(bubble1.center.pose, bubble2.center.pose, distance)) - { - ROS_ERROR("failed to calculate Distance between two bubbles. Aborting check for overlap!"); - return false; - } - - // compare with size of the two bubbles - if(distance >= min_bubble_overlap_ * (bubble1.expansion + bubble2.expansion)) - return false; - - // TODO this does not account for kinematic properties -> improve - - // everything fine - bubbles overlap - return true; -} +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Gap detected, fill recursive"); +#endif + + // last possible case -> bubbles do not overlap AND there are nor bubbles in between -> try to fill gap recursively + if(!fillGap(band, start_iter, end_iter)) + { + // band is broken in this intervall and could not be fixed (this should only be called on a leaf, so we put a log_out here;) + ROS_DEBUG("Failed to fill gap between bubble %d and %d.", (int) distance(band.begin(), start_iter), (int) distance(band.begin(), end_iter) ); + return false; + } + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Refining Recursive - Gap detected, fill recursive DONE"); +#endif -bool EBandPlanner::calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } + // we could fill the gap (reached leaf of this branch) + return true; + } - geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D; - // TODO make this in a better way - // - or work with pose2D right from the beginnning - // convert quaternions to euler angles - at this point this no longer works in 3D !! - PoseToPose2D(start_center_pose, start_pose2D); - PoseToPose2D(end_center_pose, end_pose2D); + bool EBandPlanner::fillGap(std::vector& band, std::vector::iterator& start_iter,std::vector::iterator& end_iter) + { + // insert bubbles in the middle between not-overlapping bubbles (e.g. (Dist > Size Bub1) && (Dist > Size Bub2) ) + // repeat until gaps are closed - // get rotational difference - diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta; - diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta); - // get translational difference - diff_pose2D.x = end_pose2D.x - start_pose2D.x; - diff_pose2D.y = end_pose2D.y - start_pose2D.y; + // instantiate local variables + double distance = 0.0; + Bubble interpolated_bubble; + geometry_msgs::PoseStamped interpolated_center; + std::vector::iterator tmp_iter; + int diff_int, start_num, end_num; - // calc distance - double angle_to_pseudo_vel = diff_pose2D.theta * getCircumscribedRadius(*costmap_ros_); - //distance = sqrt( (diff_pose2D.x * diff_pose2D.x) + (diff_pose2D.y * diff_pose2D.y) + (angle_to_pseudo_vel * angle_to_pseudo_vel) ); - distance = sqrt( (diff_pose2D.x * diff_pose2D.x) + (diff_pose2D.y * diff_pose2D.y)); + // make sure this method was called for a valid element in the forces or bubbles vector + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( start_iter < end_iter ); - // TODO take into account kinematic properties of body - return true; -} +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - interpolate"); +#endif + // interpolate between bubbles [depends kinematic] + if(!interpolateBubbles(start_iter->center, end_iter->center, interpolated_center)) + { + // interpolation failed (for whatever reason), so return with false + start_num = std::distance(band.begin(), start_iter); + end_num = std::distance(band.begin(), end_iter); + ROS_DEBUG("Interpolation failed while trying to fill gap between bubble %d and %d.", start_num, end_num); + return false; + } -bool EBandPlanner::calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D; - - // TODO make this in a better way - // - or work with pose2D right from the beginnning - // convert quaternions to euler angles - at this point this no longer works in 3D !! - PoseToPose2D(start_center_pose, start_pose2D); - PoseToPose2D(end_center_pose, end_pose2D); - - // get rotational difference - diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta; - diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta); - // get translational difference - diff_pose2D.x = end_pose2D.x - start_pose2D.x; - diff_pose2D.y = end_pose2D.y - start_pose2D.y; - - difference.linear.x = diff_pose2D.x; - difference.linear.y = diff_pose2D.y; - difference.linear.z = 0.0; - // multiply by inscribed radius to math calculation of distance - difference.angular.x = 0.0; - difference.angular.y = 0.0; - difference.angular.z = diff_pose2D.theta*getCircumscribedRadius(*costmap_ros_); - - // TODO take into account kinematic properties of body - - return true; -} +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - calc expansion of interpolated bubble"); +#endif + + // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] + if(!calcObstacleKinematicDistance(interpolated_center.pose, distance)) + { + // pose probably outside map coordinates + start_num = std::distance(band.begin(), start_iter); + end_num = std::distance(band.begin(), end_iter); + ROS_DEBUG("Calculation of Distance failed for interpolated bubble - failed to fill gap between bubble %d and %d.", start_num, end_num); + return false; + } + + if(distance <= tiny_bubble_expansion_) + { + // band broken! frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid + start_num = std::distance(band.begin(), start_iter); + end_num = std::distance(band.begin(), end_iter); + ROS_DEBUG("Interpolated Bubble in Collision - failed to fill gap between bubble %d and %d.", start_num, end_num); + // TODO this means only that there is an obstacle on the direct interconnection between the bubbles - think about repair or rescue strategies - + return false; + } + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - inserting interpolated bubble at (%f, %f), with expansion %f", interpolated_center.pose.position.x, interpolated_center.pose.position.y, distance); +#endif + + // insert bubble and assign center and expansion + interpolated_bubble.center = interpolated_center; + interpolated_bubble.expansion = distance; + // insert bubble (vector.insert() inserts elements before given iterator) and get iterator pointing to it + tmp_iter = band.insert(end_iter, interpolated_bubble); + // insert is a little bit more tricky than erase, as it may require reallocation of the vector -> start and end iter could be invalid + start_iter = tmp_iter - 1; + end_iter = tmp_iter + 1; + + // check that iterators are still valid - just in case :) + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - check overlap interpolated bubble and first bubble"); +#endif + + // we have now two intervalls (left and right of inserted bubble) which need to be checked again and filled if neccessary + if(!checkOverlap(*start_iter, *tmp_iter)) + { + +#ifdef DEBUG_EBAND + ROS_DEBUG("Fill recursive - gap btw. interpolated and first bubble - fill recursive"); +#endif + + // gap in left intervall -> try to fill + if(!fillGap(band, start_iter, tmp_iter)) + { + // band is broken in this intervall and could not be fixed + return false; + } + // bubbles were inserted -> make sure to keep end_iter iterator valid + end_iter = tmp_iter + 1; + } + + // check that iterators are still valid - just in case :) + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - check overlap interpolated bubble and second bubble"); +#endif + + if(!checkOverlap(*tmp_iter, *end_iter)) + { + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - gap btw. interpolated and second bubble - fill recursive"); +#endif + + // get distance between start_iter and tmp_iter before filling right intervall (in case of reallocation of vector) + diff_int = (int) std::distance(start_iter, tmp_iter); -bool EBandPlanner::calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance) -{ - // calculate distance to nearest obstacle [depends kinematic, shape, environment] - - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - unsigned int cell_x, cell_y; - unsigned char disc_cost; - double weight = costmap_weight_; - - // read distance to nearest obstacle directly from costmap - // (does not take into account shape and kinematic properties) - // get cell for coordinates of bubble center - if(!costmap_->worldToMap(center_pose.position.x, center_pose.position.y, cell_x, cell_y)) { - // probably at the edge of the costmap - this value should be recovered soon - disc_cost = 1; - } else { - // get cost for this cell - disc_cost = costmap_->getCost(cell_x, cell_y); + // gap in left intervall -> try to fill + if(!fillGap(band, tmp_iter, end_iter)) + { + // band is broken in this intervall and could not be fixed + return false; + } + // bubbles were inserted -> make sure to keep start_iter iterator valid + start_iter = tmp_iter - diff_int; + } + + // check that iterators are still valid - just in case :) + ROS_ASSERT( start_iter >= band.begin() ); + ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector + ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) ); + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Fill recursive - gap closed"); +#endif + + // bubbles overlap, iterators are kept valid -> done + return true; } - // calculate distance to nearest obstacel from this cost (see costmap_2d in wiki for details) - - // For reference: here comes an excerpt of the cost calculation within the costmap function - /*if(distance == 0) - cost = LETHAL_OBSTACLE; - else if(distance <= cell_inscribed_radius_) - cost = INSCRIBED_INFLATED_OBSTACLE; - else { - //make sure cost falls off by Euclidean distance - double euclidean_distance = distance * resolution_; - double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); - cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); - }*/ - - if (disc_cost == costmap_2d::LETHAL_OBSTACLE) { - // pose is inside an obstacle - very bad - distance = 0.0; - } else if (disc_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { - // footprint is definitely inside an obstacle - still bad - distance = 0.0; - } else { - if (disc_cost == 0) { // freespace, no estimate of distance - disc_cost = 1; // lowest non freespace cost - } else if (disc_cost == 255) { // unknown space, we should never be here - disc_cost = 1; + + // optimization + + bool EBandPlanner::modifyBandArtificialForce(std::vector& band) + { + if(band.empty()) + { + ROS_ERROR("Trying to modify an empty band."); + return false; + } + + if(band.size() <= 2) + { + // nothing to do here -> we can stop right away + return true; } - double factor = ((double) disc_cost) / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1); - distance = -log(factor) / weight; + + std::vector internal_forces, external_forces, forces; + geometry_msgs::WrenchStamped wrench; + +#ifdef DEBUG_EBAND_ + //publish original band + if(visualization_) + eband_visual_->publishBand("bubbles", band); +#endif + + // init variables to calm down debug warnings + wrench.header.stamp = ros::Time::now(); + wrench.header.frame_id = band[0].center.header.frame_id; + wrench.wrench.force.x = 0.0; + wrench.wrench.force.y = 0.0; + wrench.wrench.force.z = 0.0; + wrench.wrench.torque.x = 0.0; + wrench.wrench.torque.y = 0.0; + wrench.wrench.torque.z = 0.0; + internal_forces.assign(band.size(), wrench); + external_forces = internal_forces; + forces = internal_forces; + + // TODO log timigs of planner + // instantiate variables for timing + //ros::Time time_stamp1, time_stamp2; + //ros::Duration duration; + //time_stamp1 = ros::Time::now(); + + // due to refinement band might change its size -> use while loop + int i = 1; + bool forward = true; // cycle 1xforwards and 1xbackwards through band + while( (i>0) && (i < ((int) band.size() - 1)) ) + { + ROS_DEBUG("Modifying bubble %d.", i); + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Calculating internal force for bubble %d.", i); +#endif + + if(!calcInternalForces(i, band, band.at(i), internal_forces.at(i))) + { + // calculation of internal forces failed - stopping optimization + ROS_DEBUG("Calculation of internal forces failed"); + return false; + } + +#ifdef DEBUG_EBAND_ + if(visualization_) + // publish internal forces + eband_visual_->publishForce("internal_forces", i, eband_visual_->blue, internal_forces[i], band[i]); + // Log out debug info about next step + ROS_DEBUG("Calculating external force for bubble %d.", i); +#endif + + + //if(!calcExternalForces(i, band, external_forces)) + if(!calcExternalForces(i, band.at(i), external_forces.at(i))) + { + // calculation of External Forces failed - stopping optimization + ROS_DEBUG("Calculation of external forces failed"); + return false; + } + +#ifdef DEBUG_EBAND_ + if(visualization_) + //publish external forces + eband_visual_->publishForce("external_forces", i, eband_visual_->red, external_forces[i], band[i]); + // Log out debug info about next step + ROS_DEBUG("Superposing internal and external forces"); +#endif + + + // sum up external and internal forces over all bubbles + forces.at(i).wrench.force.x = internal_forces.at(i).wrench.force.x + external_forces.at(i).wrench.force.x; + forces.at(i).wrench.force.y = internal_forces.at(i).wrench.force.y + external_forces.at(i).wrench.force.y; + forces.at(i).wrench.force.z = internal_forces.at(i).wrench.force.z + external_forces.at(i).wrench.force.z; + + forces.at(i).wrench.torque.x = internal_forces.at(i).wrench.torque.x + external_forces.at(i).wrench.torque.x; + forces.at(i).wrench.torque.y = internal_forces.at(i).wrench.torque.y + external_forces.at(i).wrench.torque.y; + forces.at(i).wrench.torque.z = internal_forces.at(i).wrench.torque.z + external_forces.at(i).wrench.torque.z; + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Superpose forces: (x, y, theta) = (%f, %f, %f)", forces.at(i).wrench.force.x, forces.at(i).wrench.force.y, forces.at(i).wrench.torque.z); + ROS_DEBUG("Supressing tangential forces"); +#endif + + if(!suppressTangentialForces(i, band, forces.at(i))) + { + // suppression of tangential forces failed + ROS_DEBUG("Supression of tangential forces failed"); + return false; + } + +#ifdef DEBUG_EBAND_ + if(visualization_) + //publish resulting forces + eband_visual_->publishForce("resulting_forces", i, eband_visual_->green, forces[i], band[i]); +#endif + + + ROS_DEBUG("Applying forces to modify band"); + if(!applyForces(i, band, forces)) + { + // band invalid + ROS_DEBUG("Band is invalid - Stopping Modification"); + return false; + } + +#ifdef DEBUG_EBAND_ + if(visualization_) + { + // publish band with changed bubble at resulting position + eband_visual_->publishBand("bubbles", band); + ros::Duration(0.01).sleep(); + } +#endif + + + //next bubble + if(forward) + { + i++; + if(i == ((int) band.size() - 1)) + { + // reached end of band - start backwards cycle until at start again - then stop + forward = false; + i--; + ROS_DEBUG("Optimization Elastic Band - Forward cycle done, starting backward cycle"); + } + } + else + { + i--; + } + } + + return true; } - return true; -} + + bool EBandPlanner::applyForces(int bubble_num, std::vector& band, std::vector forces) + { + //cycle over all bubbles except first and last (these are fixed) + if(band.size() <= 2) + { + // nothing to do here -> we can stop right away - no forces calculated + return true; + } + + geometry_msgs::Pose2D bubble_pose2D, new_bubble_pose2D; + geometry_msgs::Pose bubble_pose, new_bubble_pose; + geometry_msgs::Twist bubble_jump; + Bubble new_bubble = band.at(bubble_num); + double distance; + + + // move bubble + bubble_pose = band.at(bubble_num).center.pose; + PoseToPose2D(bubble_pose, bubble_pose2D); + + // move according to bubble_new = bubble_old + alpha*force -> we choose alpha to be the current expansion of the modified bubble + bubble_jump.linear.x = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.x; + bubble_jump.linear.y = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.y; + bubble_jump.linear.z = 0.0; + bubble_jump.angular.x = 0.0; + bubble_jump.angular.y = 0.0; + bubble_jump.angular.z = band.at(bubble_num).expansion/getCircumscribedRadius(*costmap_ros_) * forces.at(bubble_num).wrench.torque.z; + bubble_jump.angular.z = angles::normalize_angle(bubble_jump.angular.z); + + // apply changes to calc tmp bubble position + new_bubble_pose2D.x = bubble_pose2D.x + bubble_jump.linear.x; + new_bubble_pose2D.y = bubble_pose2D.y + bubble_jump.linear.y; + new_bubble_pose2D.theta = bubble_pose2D.theta + bubble_jump.angular.z; + new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta); + + // apply changes to local copy + Pose2DToPose(new_bubble_pose, new_bubble_pose2D); + new_bubble.center.pose = new_bubble_pose; + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f).", bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta, + bubble_jump.linear.x, bubble_jump.linear.y, bubble_jump.angular.z); +#endif + + + // check validity of moved bubble + + // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] + if(!calcObstacleKinematicDistance(new_bubble_pose, distance)) + { + ROS_DEBUG("Calculation of Distance failed. Frame %d of %d Probably outside map. Discarding Changes", bubble_num, ((int) band.size()) ); + +#ifdef DEBUG_EBAND_ + if(visualization_) + eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble); +#endif + + // this bubble must not be changed, but band is still valid -> continue with other bubbles + return true; + } + + if(distance <= tiny_bubble_expansion_) + { + // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid + ROS_DEBUG("Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Discarding Changes", bubble_num, ((int) band.size()) ); + +#ifdef DEBUG_EBAND_ + if(visualization_) + eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble); +#endif + + // this bubble must not be changed, but band is still valid -> continue with other bubbles + return true; + } + + // so far o.k. -> assign distance to new bubble + new_bubble.expansion = distance; + + + // check whether step was reasonable + + geometry_msgs::WrenchStamped new_bubble_force = forces.at(bubble_num); + + // check whether we get a valid force calculation here + if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force)) + { + // error during calculation of forces for the new position - discard changes + ROS_DEBUG("Cannot calculate forces on bubble %d at new position - discarding changes", bubble_num); + return true; + } + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Check for zero-crossings in force on bubble %d", bubble_num); +#endif + + // check for zero-crossings in the force-vector + double checksum_zero, abs_new_force, abs_old_force; + + // project force-vectors onto each other + checksum_zero = (new_bubble_force.wrench.force.x * forces.at(bubble_num).wrench.force.x) + + (new_bubble_force.wrench.force.y * forces.at(bubble_num).wrench.force.y) + + (new_bubble_force.wrench.torque.z * forces.at(bubble_num).wrench.torque.z); + + // if sign changes and ... + if(checksum_zero < 0.0) + { + ROS_DEBUG("Detected zero-crossings in force on bubble %d. Checking total change in force.", bubble_num); + // check the absolute values of the two vectors + abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) + + (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) + + (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) ); + abs_old_force = sqrt( (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) + + (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) + + (forces.at(bubble_num).wrench.torque.z * forces.at(bubble_num).wrench.torque.z) ); + + // force still has a significant high value (> ~75% of old force by default) + if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) ) + { + ROS_DEBUG("Detected significante change in force (%f to %f) on bubble %d. Entering Recursive Approximation.", abs_old_force, abs_new_force, bubble_num); + // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point + int curr_recursion_depth = 0; + geometry_msgs::Twist new_step_width; + Bubble curr_bubble = band.at(bubble_num); + geometry_msgs::WrenchStamped curr_bubble_force = forces.at(bubble_num); + + // half step size + new_step_width.linear.x = 0.5*bubble_jump.linear.x; + new_step_width.linear.y = 0.5*bubble_jump.linear.y; + new_step_width.linear.z = 0.5*bubble_jump.linear.z; + new_step_width.angular.x = 0.5*bubble_jump.angular.x; + new_step_width.angular.y = 0.5*bubble_jump.angular.y; + new_step_width.angular.z = 0.5*bubble_jump.angular.z; + + // one step deeper into the recursion + if(moveApproximateEquilibrium(bubble_num, band, curr_bubble, curr_bubble_force, new_step_width, curr_recursion_depth)) + { + // done with recursion - change bubble and hand it back + new_bubble = curr_bubble; + +#ifdef DEBUG_EBAND_ + geometry_msgs::Pose2D curr_bubble_pose2D; + PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D); + ROS_DEBUG("Instead - Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f) to (%f, %f, %f).", + bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta, + new_step_width.linear.x, new_step_width.linear.y, new_step_width.angular.z, + curr_bubble_pose2D.x, curr_bubble_pose2D.y, curr_bubble_pose2D.theta); +#endif + } + } + } + + + // check validity of resulting band (given the moved bubble) + + // TODO use this routine not only to check whether gap can be filled but also to fill gap (if possible) + // get local copy of band, set new position of moved bubble and init iterators + std::vector tmp_band = band; + std::vector::iterator start_iter, end_iter; + tmp_band.at(bubble_num) = new_bubble; + start_iter = tmp_band.begin(); + + // check left connection (bubble and bubble-1) + start_iter = start_iter + bubble_num - 1; + end_iter = start_iter + 1; + + // check Overlap - if bubbles do not overlap try to fill gap + if(!checkOverlap(*start_iter, *end_iter)) + { + if(!fillGap(tmp_band, start_iter, end_iter)) + { + ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes."); + // this bubble must not be changed, but band is still valid -> continue with other bubbles + return true; + } + } + + + // get fresh copy of band, set new position of bubble again and reinit iterators + tmp_band = band; + tmp_band.at(bubble_num) = new_bubble; + start_iter = tmp_band.begin(); + + // check right connection (bubble and bubble +1) + start_iter = start_iter + bubble_num; + end_iter = start_iter + 1; + + // check Overlap - if bubbles do not overlap try to fill gap + if(!checkOverlap(*start_iter, *end_iter)) + { + if(!fillGap(tmp_band, start_iter, end_iter)) + { + ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes."); + // this bubble must not be changed, but band is still valid -> continue with other bubbles + return true; + } + } + + + // check successful - bubble and band valid apply changes + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Frame %d of %d: Check successful - bubble and band valid. Applying Changes", bubble_num, ((int) band.size()) ); +#endif + + band.at(bubble_num) = new_bubble; + + return true; + } -// type conversions - -bool EBandPlanner::convertPlanToBand(std::vector plan, std::vector& band) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - // create local variables - double distance = 0.0; - std::vector tmp_band; - - ROS_DEBUG("Copying plan to band - Conversion started: %d frames to convert.", ((int) plan.size()) ); - - // get local copy of referenced variable - tmp_band = band; - - // adapt band to plan - tmp_band.resize(plan.size()); - for(int i = 0; i < ((int) plan.size()); i++) - { - #ifdef DEBUG_EBAND_ - ROS_DEBUG("Checking Frame %d of %d", i, ((int) plan.size()) ); - #endif - - // set poses in plan as centers of bubbles - tmp_band[i].center = plan[i]; - - // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] - if(!calcObstacleKinematicDistance(tmp_band[i].center.pose, distance)) - { - // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid - ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d outside map", i, ((int) plan.size()) ); - return false; - } - - if(distance <= 0.0) - { - // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid - ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d in collision. Plan invalid", i, ((int) plan.size()) ); - // TODO if frame in collision try to repair band instaed of aborting averything + bool EBandPlanner::moveApproximateEquilibrium(const int& bubble_num, const std::vector& band, Bubble& curr_bubble, + const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width, const int& curr_recursion_depth) + { + + double distance; + Bubble new_bubble = curr_bubble; + geometry_msgs::Pose2D new_bubble_pose2D, curr_bubble_pose2D; + geometry_msgs::WrenchStamped new_bubble_force = curr_bubble_force; + + // move bubble + PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D); + PoseToPose2D(new_bubble.center.pose, new_bubble_pose2D); + + // apply changes to calculate tmp bubble position + new_bubble_pose2D.x = curr_bubble_pose2D.x + curr_step_width.linear.x; + new_bubble_pose2D.y = curr_bubble_pose2D.y + curr_step_width.linear.y; + new_bubble_pose2D.theta = curr_bubble_pose2D.theta + curr_step_width.angular.z; + new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta); + + // apply changes to local copy + Pose2DToPose(new_bubble.center.pose, new_bubble_pose2D); + + + // check validity of moved bubble + + // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] + if(!calcObstacleKinematicDistance(new_bubble.center.pose, distance)) return false; - } + // we wont be able to calculate forces later on + if(distance == 0.0) + return false; - // assign to expansion of bubble - tmp_band[i].expansion = distance; - } - // write to referenced variable - band = tmp_band; + // so far o.k. -> assign distance to new bubble + new_bubble.expansion = distance; - ROS_DEBUG("Successfully converted plan to band"); - return true; -} + // check whether we get a valid force calculation here + if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force)) + return false; + + // great - lets store this - this is better then what we had so far + curr_bubble = new_bubble; + + // if everything is fine and we reached our maximum recursion depth + if(curr_recursion_depth >= max_recursion_depth_approx_equi_) + // break recursion at this point + return true; + + + // now - let's check for zero-crossing + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Check for zero-crossings in force on bubble %d - Recursion %d", bubble_num, curr_recursion_depth); +#endif + + double checksum_zero, abs_new_force, abs_old_force; + int new_recursion_depth; + geometry_msgs::Twist new_step_width; + + // check zero-crossing by projecting force-vectors onto each other + checksum_zero = (new_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + + (new_bubble_force.wrench.force.y * curr_bubble_force.wrench.force.y) + + (new_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z); + + if(checksum_zero < 0.0) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Detected zero-crossings in force on bubble %d - Recursion %d. Checking total change in force.", bubble_num, curr_recursion_depth); +#endif + + // check the absolute values of the two vectors + abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) + + (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) + + (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) ); + abs_old_force = sqrt( (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + + (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) + + (curr_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z) ); + + if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) ) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Detected significant change in force (%f to %f) on bubble %d - Recursion %d. Going one Recursion deeper.", abs_old_force, abs_new_force, bubble_num, curr_recursion_depth); +#endif + + // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point + new_recursion_depth = curr_recursion_depth + 1; + // half step size - backward direction + new_step_width.linear.x = -0.5*curr_step_width.linear.x; + new_step_width.linear.y = -0.5*curr_step_width.linear.y; + new_step_width.linear.z = -0.5*curr_step_width.linear.z; + new_step_width.angular.x = -0.5*curr_step_width.angular.x; + new_step_width.angular.y = -0.5*curr_step_width.angular.y; + new_step_width.angular.z = -0.5*curr_step_width.angular.z; + + // one step deeper into the recursion + if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth)) + // done with recursion - change bubble and hand it back + curr_bubble = new_bubble; + + // otherwise - could not get a better value - return without change (curr_bubble as asigned above) + } + + // otherwise - this is good enough for us - return with this value (curr_bubble as asigned above) + } + else + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("No zero-crossings in force on bubble %d - Recursion %d. Continue walk in same direction. Going one recursion deeper.", bubble_num, curr_recursion_depth); +#endif + + // continue walk in same direction + new_recursion_depth = curr_recursion_depth + 1; + // half step size - backward direction + new_step_width.linear.x = 0.5*curr_step_width.linear.x; + new_step_width.linear.y = 0.5*curr_step_width.linear.y; + new_step_width.linear.z = 0.5*curr_step_width.linear.z; + new_step_width.angular.x = 0.5*curr_step_width.angular.x; + new_step_width.angular.y = 0.5*curr_step_width.angular.y; + new_step_width.angular.z = 0.5*curr_step_width.angular.z; + + // one step deeper into the recursion + if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth)) + // done with recursion - change bubble and hand it back + curr_bubble = new_bubble; + + // otherwise - could not get a better value - return without change (curr_bubble as asigned above) + } + // done + return true; + } -bool EBandPlanner::convertBandToPlan(std::vector& plan, std::vector band) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - // create local variables - std::vector tmp_plan; + bool EBandPlanner::getForcesAt(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) + { + geometry_msgs::WrenchStamped internal_force, external_force; - // adapt plan to band - tmp_plan.resize(band.size()); - for(int i = 0; i < ((int) band.size()); i++) - { - // set centers of bubbles to StampedPose in plan - tmp_plan[i] = band[i].center; - } + if(!calcInternalForces(bubble_num, band, curr_bubble, internal_force)) + { + // calculation of internal forces failed - stopping optimization + ROS_DEBUG("Calculation of internal forces failed"); + return false; + } - //write to referenced variable and done - plan = tmp_plan; + if(!calcExternalForces(bubble_num, curr_bubble, external_force)) + { + // calculation of External Forces failed - stopping optimization + ROS_DEBUG("Calculation of external forces failed"); + return false; + } - return true; -} + // sum up external and internal forces over all bubbles + forces.wrench.force.x = internal_force.wrench.force.x + external_force.wrench.force.x; + forces.wrench.force.y = internal_force.wrench.force.y + external_force.wrench.force.y; + forces.wrench.force.z = internal_force.wrench.force.z + external_force.wrench.force.z; + + forces.wrench.torque.x = internal_force.wrench.torque.x + external_force.wrench.torque.x; + forces.wrench.torque.y = internal_force.wrench.torque.y + external_force.wrench.torque.y; + forces.wrench.torque.z = internal_force.wrench.torque.z + external_force.wrench.torque.z; + + if(!suppressTangentialForces(bubble_num, band, forces)) + { + // suppression of tangential forces failed + ROS_DEBUG("Supression of tangential forces failed"); + return false; + } + + return true; + } + + + bool EBandPlanner::calcInternalForces(int bubble_num, std::vector band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + //cycle over all bubbles except first and last (these are fixed) + if(band.size() <= 2) + { + // nothing to do here -> we can stop right away - no forces calculated + return true; + } + + // init tmp variables + double distance1, distance2; + geometry_msgs::Twist difference1, difference2; + geometry_msgs::Wrench wrench; + + // make sure this method was called for a valid element in the forces or bubbles vector + ROS_ASSERT( bubble_num > 0 ); + ROS_ASSERT( bubble_num < ((int) band.size() - 1) ); + + + // get distance between bubbles + if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num-1].center.pose, distance1)) + { + ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!"); + return false; + } + + if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num+1].center.pose, distance2)) + { + ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!"); + return false; + } + + // get (elementwise) difference bewtween bubbles + if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num-1].center.pose, difference1)) + { + ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!"); + return false; + } + + if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num+1].center.pose, difference2)) + { + ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!"); + return false; + } + + // make sure to avoid division by (almost) zero during force calculation (avoid numerical problems) + // -> if difference/distance is (close to) zero then the force in this direction should be zero as well + if(distance1 <= tiny_bubble_distance_) + distance1 = 1000000.0; + if(distance2 <= tiny_bubble_distance_) + distance2 = 1000000.0; + + // now calculate wrench - forces model an elastic band and are normed (distance) to render forces for small and large bubbles the same + wrench.force.x = internal_force_gain_*(difference1.linear.x/distance1 + difference2.linear.x/distance2); + wrench.force.y = internal_force_gain_*(difference1.linear.y/distance1 + difference2.linear.y/distance2); + wrench.force.z = internal_force_gain_*(difference1.linear.z/distance1 + difference2.linear.z/distance2); + wrench.torque.x = internal_force_gain_*(difference1.angular.x/distance1 + difference2.angular.x/distance2); + wrench.torque.y = internal_force_gain_*(difference1.angular.y/distance1 + difference2.angular.y/distance2); + wrench.torque.z = internal_force_gain_*(difference1.angular.z/distance1 + difference2.angular.z/distance2); + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Calculating internal forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z); +#endif + + // store wrench in vector + forces.wrench = wrench; + + return true; + } + + + bool EBandPlanner::calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // init tmp variables + double distance1, distance2; + geometry_msgs::Pose edge; + geometry_msgs::Pose2D edge_pose2D; + geometry_msgs::Wrench wrench; + + + // calculate delta-poses (on upper edge of bubble) for x-direction + edge = curr_bubble.center.pose; + edge.position.x = edge.position.x + curr_bubble.expansion; + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance1)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + // calculate delta-poses (on lower edge of bubble) for x-direction + edge.position.x = edge.position.x - 2.0*curr_bubble.expansion; + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance2)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + + // calculate difference-quotient (approx. of derivative) in x-direction + if(curr_bubble.expansion <= tiny_bubble_expansion_) + { + // avoid division by (almost) zero to avoid numerical problems + wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); + // actually we should never end up here - band should have been considered as broken + ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); + } + else + wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); + // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponding term + + + // calculate delta-poses (on upper edge of bubble) for y-direction + edge = curr_bubble.center.pose; + edge.position.y = edge.position.y + curr_bubble.expansion; + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance1)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + // calculate delta-poses (on lower edge of bubble) for x-direction + edge.position.y = edge.position.y - 2.0*curr_bubble.expansion; + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance2)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + + // calculate difference-quotient (approx. of derivative) in x-direction + if(curr_bubble.expansion <= tiny_bubble_expansion_) + { + // avoid division by (almost) zero to avoid numerical problems + wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); + // actually we should never end up here - band should have been considered as broken + ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); + } + else + wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); + // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term + + + // no force in z-direction + wrench.force.z = 0.0; + + + // no torque around x and y axis + wrench.torque.x = 0.0; + wrench.torque.y = 0.0; + + + // calculate delta-poses (on upper edge of bubble) for x-direction + PoseToPose2D(curr_bubble.center.pose, edge_pose2D); + edge_pose2D.theta = edge_pose2D.theta + (curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_)); + edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta); + PoseToPose2D(edge, edge_pose2D); + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance1)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + // calculate delta-poses (on lower edge of bubble) for x-direction + edge_pose2D.theta = edge_pose2D.theta - 2.0*(curr_bubble.expansion/getCircumscribedRadius(*costmap_ros_)); + edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta); + PoseToPose2D(edge, edge_pose2D); + // get expansion on bubble at this point + if(!calcObstacleKinematicDistance(edge, distance2)) + { + ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num); + // we cannot calculate external forces for this bubble - but still continue for the other bubbles + return true; + } + + // calculate difference-quotient (approx. of derivative) in x-direction + if(curr_bubble.expansion <= tiny_bubble_expansion_) + { + // avoid division by (almost) zero to avoid numerical problems + wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_); + // actually we should never end up here - band should have been considered as broken + ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured"); + } + else + wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion); + // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term + + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Calculating external forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z); +#endif + + // assign wrench to forces vector + forces.wrench = wrench; + + return true; + } + + + bool EBandPlanner::suppressTangentialForces(int bubble_num, std::vector band, geometry_msgs::WrenchStamped& forces) + { + //cycle over all bubbles except first and last (these are fixed) + if(band.size() <= 2) + { + // nothing to do here -> we can stop right away - no forces calculated + return true; + } + + double scalar_fd, scalar_dd; + geometry_msgs::Twist difference; + + // make sure this method was called for a valid element in the forces or bubbles vector + ROS_ASSERT( bubble_num > 0 ); + ROS_ASSERT( bubble_num < ((int) band.size() - 1) ); + + + // get pose-difference from following to preceding bubble -> "direction of the band in this bubble" + if(!calcBubbleDifference(band[bubble_num+1].center.pose, band[bubble_num-1].center.pose, difference)) + return false; + + // "project wrench" in middle bubble onto connecting vector + // scalar wrench*difference + scalar_fd = forces.wrench.force.x*difference.linear.x + forces.wrench.force.y*difference.linear.y + + forces.wrench.force.z*difference.linear.z + forces.wrench.torque.x*difference.angular.x + + forces.wrench.torque.y*difference.angular.y + forces.wrench.torque.z*difference.angular.z; + + // abs of difference-vector: scalar difference*difference + scalar_dd = difference.linear.x*difference.linear.x + difference.linear.y*difference.linear.y + difference.linear.z*difference.linear.z + + difference.angular.x*difference.angular.x + difference.angular.y*difference.angular.y + difference.angular.z*difference.angular.z; + + // avoid division by (almost) zero -> check if bubbles have (almost) same center-pose + if(scalar_dd <= tiny_bubble_distance_) + { + // there are redundant bubbles, this should normally not hapen -> probably error in band refinement + ROS_DEBUG("Calculating tangential forces for redundant bubbles. Bubble should have been removed. Local Planner probably ill configured"); + } + + // calculate orthogonal components + forces.wrench.force.x = forces.wrench.force.x - scalar_fd/scalar_dd * difference.linear.x; + forces.wrench.force.y = forces.wrench.force.y - scalar_fd/scalar_dd * difference.linear.y; + forces.wrench.force.z = forces.wrench.force.z - scalar_fd/scalar_dd * difference.linear.z; + forces.wrench.torque.x = forces.wrench.torque.x - scalar_fd/scalar_dd * difference.angular.x; + forces.wrench.torque.y = forces.wrench.torque.y - scalar_fd/scalar_dd * difference.angular.y; + forces.wrench.torque.z = forces.wrench.torque.z - scalar_fd/scalar_dd * difference.angular.z; + +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Supressing tangential forces: (x, y, theta) = (%f, %f, %f)", + forces.wrench.force.x, forces.wrench.force.y, forces.wrench.torque.z); +#endif + + return true; + } + + + // problem (geometry) dependant functions + + bool EBandPlanner::interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // instantiate local variables + geometry_msgs::Pose2D start_pose2D, end_pose2D, interpolated_pose2D; + double delta_theta; + + // copy header + interpolated_center.header = start_center.header; + + // interpolate angles + // TODO make this in a better way + // - for instance use "slerp" to interpolate directly between quaternions + // - or work with pose2D right from the beginnning + // convert quaternions to euler angles - at this point this no longer works in 3D !! + PoseToPose2D(start_center.pose, start_pose2D); + PoseToPose2D(end_center.pose, end_pose2D); + // calc mean of theta angle + delta_theta = end_pose2D.theta - start_pose2D.theta; + delta_theta = angles::normalize_angle(delta_theta) / 2.0; + interpolated_pose2D.theta = start_pose2D.theta + delta_theta; + interpolated_pose2D.theta = angles::normalize_angle(interpolated_pose2D.theta); + // convert back to quaternion + interpolated_pose2D.x = 0.0; + interpolated_pose2D.y = 0.0; + Pose2DToPose(interpolated_center.pose, interpolated_pose2D); + + // interpolate positions + interpolated_center.pose.position.x = (end_center.pose.position.x + start_center.pose.position.x)/2.0; + interpolated_center.pose.position.y = (end_center.pose.position.y + start_center.pose.position.y)/2.0; + interpolated_center.pose.position.z = (end_center.pose.position.z + start_center.pose.position.z)/2.0; + + // TODO ideally this would take into account kinematics of the robot and for instance use splines + + return true; + } + + + bool EBandPlanner::checkOverlap(Bubble bubble1, Bubble bubble2) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // calc (kinematic) Distance between bubbles + double distance = 0.0; + if(!calcBubbleDistance(bubble1.center.pose, bubble2.center.pose, distance)) + { + ROS_ERROR("failed to calculate Distance between two bubbles. Aborting check for overlap!"); + return false; + } + + // compare with size of the two bubbles + if(distance >= min_bubble_overlap_ * (bubble1.expansion + bubble2.expansion)) + return false; + + // TODO this does not account for kinematic properties -> improve + + // everything fine - bubbles overlap + return true; + } + + + bool EBandPlanner::calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D; + + // TODO make this in a better way + // - or work with pose2D right from the beginnning + // convert quaternions to euler angles - at this point this no longer works in 3D !! + PoseToPose2D(start_center_pose, start_pose2D); + PoseToPose2D(end_center_pose, end_pose2D); + + // get rotational difference + diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta; + diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta); + // get translational difference + diff_pose2D.x = end_pose2D.x - start_pose2D.x; + diff_pose2D.y = end_pose2D.y - start_pose2D.y; + + // calc distance + double angle_to_pseudo_vel = diff_pose2D.theta * getCircumscribedRadius(*costmap_ros_); + //distance = sqrt( (diff_pose2D.x * diff_pose2D.x) + (diff_pose2D.y * diff_pose2D.y) + (angle_to_pseudo_vel * angle_to_pseudo_vel) ); + distance = sqrt( (diff_pose2D.x * diff_pose2D.x) + (diff_pose2D.y * diff_pose2D.y)); + + // TODO take into account kinematic properties of body + + return true; + } + + + bool EBandPlanner::calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D; + + // TODO make this in a better way + // - or work with pose2D right from the beginnning + // convert quaternions to euler angles - at this point this no longer works in 3D !! + PoseToPose2D(start_center_pose, start_pose2D); + PoseToPose2D(end_center_pose, end_pose2D); + + // get rotational difference + diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta; + diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta); + // get translational difference + diff_pose2D.x = end_pose2D.x - start_pose2D.x; + diff_pose2D.y = end_pose2D.y - start_pose2D.y; + + difference.linear.x = diff_pose2D.x; + difference.linear.y = diff_pose2D.y; + difference.linear.z = 0.0; + // multiply by inscribed radius to math calculation of distance + difference.angular.x = 0.0; + difference.angular.y = 0.0; + difference.angular.z = diff_pose2D.theta*getCircumscribedRadius(*costmap_ros_); + + // TODO take into account kinematic properties of body + + return true; + } + + + bool EBandPlanner::calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance) + { + // calculate distance to nearest obstacle [depends kinematic, shape, environment] + + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + unsigned int cell_x, cell_y; + unsigned char disc_cost; + double weight = costmap_weight_; + + // read distance to nearest obstacle directly from costmap + // (does not take into account shape and kinematic properties) + // get cell for coordinates of bubble center + if(!costmap_->worldToMap(center_pose.position.x, center_pose.position.y, cell_x, cell_y)) { + // probably at the edge of the costmap - this value should be recovered soon + disc_cost = 1; + } else { + // get cost for this cell + disc_cost = costmap_->getCost(cell_x, cell_y); + } + + // calculate distance to nearest obstacel from this cost (see costmap_2d in wiki for details) + + // For reference: here comes an excerpt of the cost calculation within the costmap function + /*if(distance == 0) + cost = LETHAL_OBSTACLE; + else if(distance <= cell_inscribed_radius_) + cost = INSCRIBED_INFLATED_OBSTACLE; + else { + //make sure cost falls off by Euclidean distance + double euclidean_distance = distance * resolution_; + double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_)); + cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + }*/ + + if (disc_cost == costmap_2d::LETHAL_OBSTACLE) { + // pose is inside an obstacle - very bad + distance = 0.0; + } else if (disc_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + // footprint is definitely inside an obstacle - still bad + distance = 0.0; + } else { + if (disc_cost == 0) { // freespace, no estimate of distance + disc_cost = 1; // lowest non freespace cost + } else if (disc_cost == 255) { // unknown space, we should never be here + disc_cost = 1; + } + double factor = ((double) disc_cost) / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1); + distance = -log(factor) / weight; + } + + return true; + } + + + // type conversions + + bool EBandPlanner::convertPlanToBand(std::vector plan, std::vector& band) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // create local variables + double distance = 0.0; + std::vector tmp_band; + + ROS_DEBUG("Copying plan to band - Conversion started: %d frames to convert.", ((int) plan.size()) ); + + // get local copy of referenced variable + tmp_band = band; + + // adapt band to plan + tmp_band.resize(plan.size()); + for(int i = 0; i < ((int) plan.size()); i++) + { +#ifdef DEBUG_EBAND_ + ROS_DEBUG("Checking Frame %d of %d", i, ((int) plan.size()) ); +#endif + + // set poses in plan as centers of bubbles + tmp_band[i].center = plan[i]; + + // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment] + if(!calcObstacleKinematicDistance(tmp_band[i].center.pose, distance)) + { + // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid + ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d outside map", i, ((int) plan.size()) ); + return false; + } + + if(distance <= 0.0) + { + // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid + ROS_WARN("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d in collision. Plan invalid", i, ((int) plan.size()) ); + // TODO if frame in collision try to repair band instaed of aborting averything + return false; + } + + + // assign to expansion of bubble + tmp_band[i].expansion = distance; + } + + // write to referenced variable + band = tmp_band; + + ROS_DEBUG("Successfully converted plan to band"); + return true; + } + + + bool EBandPlanner::convertBandToPlan(std::vector& plan, std::vector band) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // create local variables + std::vector tmp_plan; + + // adapt plan to band + tmp_plan.resize(band.size()); + for(int i = 0; i < ((int) band.size()); i++) + { + // set centers of bubbles to StampedPose in plan + tmp_plan[i] = band[i].center; + } + + //write to referenced variable and done + plan = tmp_plan; + + return true; + } } diff --git a/src/eband_local_planner_ros.cpp b/src/eband_local_planner_ros.cpp index c69e39d..a4fd16e 100644 --- a/src/eband_local_planner_ros.cpp +++ b/src/eband_local_planner_ros.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #include @@ -49,375 +49,375 @@ PLUGINLIB_DECLARE_CLASS(eband_local_planner, EBandPlannerROS, eband_local_planner::EBandPlannerROS, nav_core::BaseLocalPlanner) -namespace eband_local_planner{ + namespace eband_local_planner{ -EBandPlannerROS::EBandPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {} + EBandPlannerROS::EBandPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {} -EBandPlannerROS::EBandPlannerROS(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) - : costmap_ros_(NULL), tf_(NULL), initialized_(false) -{ - // initialize planner - initialize(name, tf, costmap_ros); -} + EBandPlannerROS::EBandPlannerROS(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) + : costmap_ros_(NULL), tf_(NULL), initialized_(false) + { + // initialize planner + initialize(name, tf, costmap_ros); + } + + + EBandPlannerROS::~EBandPlannerROS() {} -EBandPlannerROS::~EBandPlannerROS() {} + void EBandPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) + { + // check if the plugin is already initialized + if(!initialized_) + { + // copy adress of costmap and Transform Listener (handed over from move_base) + costmap_ros_ = costmap_ros; + tf_ = tf; -void EBandPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) -{ - // check if the plugin is already initialized - if(!initialized_) - { - // copy adress of costmap and Transform Listener (handed over from move_base) - costmap_ros_ = costmap_ros; - tf_ = tf; + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle pn("~/" + name); + // read parameters from parameter server + // get tolerances for "Target reached" + pn.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05); + pn.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1); - // create Node Handle with name of plugin (as used in move_base for loading) - ros::NodeHandle pn("~/" + name); + // set lower bound for velocity -> if velocity in this region stop! (to avoid limit-cycles or lock) + pn.param("rot_stopped_vel", rot_stopped_vel_, 1e-2); + pn.param("trans_stopped_vel", trans_stopped_vel_, 1e-2); - // read parameters from parameter server - // get tolerances for "Target reached" - pn.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05); - pn.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1); + // advertise topics (adapted global plan and predicted local trajectory) + g_plan_pub_ = pn.advertise("global_plan", 1); + l_plan_pub_ = pn.advertise("local_plan", 1); - // set lower bound for velocity -> if velocity in this region stop! (to avoid limit-cycles or lock) - pn.param("rot_stopped_vel", rot_stopped_vel_, 1e-2); - pn.param("trans_stopped_vel", trans_stopped_vel_, 1e-2); - // advertise topics (adapted global plan and predicted local trajectory) - g_plan_pub_ = pn.advertise("global_plan", 1); - l_plan_pub_ = pn.advertise("local_plan", 1); + // subscribe to topics (to get odometry information, we need to get a handle to the topic in the global namespace) + ros::NodeHandle gn; + odom_sub_ = gn.subscribe("odom", 1, boost::bind(&EBandPlannerROS::odomCallback, this, _1)); - // subscribe to topics (to get odometry information, we need to get a handle to the topic in the global namespace) - ros::NodeHandle gn; - odom_sub_ = gn.subscribe("odom", 1, boost::bind(&EBandPlannerROS::odomCallback, this, _1)); + // create the actual planner that we'll use. Pass Name of plugin and pointer to global costmap to it. + // (configuration is done via parameter server) + eband_ = boost::shared_ptr(new EBandPlanner(name, costmap_ros_)); + // create the according controller + eband_trj_ctrl_ = boost::shared_ptr(new EBandTrajectoryCtrl(name, costmap_ros_)); - // create the actual planner that we'll use. Pass Name of plugin and pointer to global costmap to it. - // (configuration is done via parameter server) - eband_ = boost::shared_ptr(new EBandPlanner(name, costmap_ros_)); + // create object for visualization + eband_visual_ = boost::shared_ptr(new EBandVisualization); - // create the according controller - eband_trj_ctrl_ = boost::shared_ptr(new EBandTrajectoryCtrl(name, costmap_ros_)); + // pass visualization object to elastic band + eband_->setVisualization(eband_visual_); - // create object for visualization - eband_visual_ = boost::shared_ptr(new EBandVisualization); + // pass visualization object to controller + eband_trj_ctrl_->setVisualization(eband_visual_); - // pass visualization object to elastic band - eband_->setVisualization(eband_visual_); + // initialize visualization - set node handle and pointer to costmap + eband_visual_->initialize(pn, costmap_ros); - // pass visualization object to controller - eband_trj_ctrl_->setVisualization(eband_visual_); - // initialize visualization - set node handle and pointer to costmap - eband_visual_->initialize(pn, costmap_ros); + // set initialized flag + initialized_ = true; + // HACK for band snapping + band_snapped_hack_ = false; + band_snapped_hack_count_ = 0; + + // this is only here to make this process visible in the rxlogger right from the start + ROS_DEBUG("Elastic Band plugin initialized."); + } + else + { + ROS_WARN("This planner has already been initialized, doing nothing."); + } + } - // set initialized flag - initialized_ = true; - // HACK for band snapping - band_snapped_hack_ = false; - band_snapped_hack_count_ = 0; + // set global plan to wrapper and pass it to eband + bool EBandPlannerROS::setPlan(const std::vector& orig_global_plan) + { - // this is only here to make this process visible in the rxlogger right from the start - ROS_DEBUG("Elastic Band plugin initialized."); - } - else - { - ROS_WARN("This planner has already been initialized, doing nothing."); - } -} + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + //reset the global plan + global_plan_.clear(); + global_plan_ = orig_global_plan; + + // transform global plan to the map frame we are working in + // this also cuts the plan off (reduces it to local window) + std::vector start_end_counts (2, (int) global_plan_.size()); // counts from the end() of the plan + if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, start_end_counts)) + { + // if plan could not be tranformed abort control and local planning + ROS_WARN("Could not transform the global plan to the frame of the controller"); + return false; + } -// set global plan to wrapper and pass it to eband -bool EBandPlannerROS::setPlan(const std::vector& orig_global_plan) -{ - - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } + // also check if there really is a plan + if(transformed_plan_.empty()) + { + // if global plan passed in is empty... we won't do anything + ROS_WARN("Transformed plan is empty. Aborting local planner!"); + return false; + } - //reset the global plan - global_plan_.clear(); - global_plan_ = orig_global_plan; - - // transform global plan to the map frame we are working in - // this also cuts the plan off (reduces it to local window) - std::vector start_end_counts (2, (int) global_plan_.size()); // counts from the end() of the plan - if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, start_end_counts)) - { - // if plan could not be tranformed abort control and local planning - ROS_WARN("Could not transform the global plan to the frame of the controller"); - return false; - } - - // also check if there really is a plan - if(transformed_plan_.empty()) - { - // if global plan passed in is empty... we won't do anything - ROS_WARN("Transformed plan is empty. Aborting local planner!"); - return false; - } - - // set plan - as this is fresh from the global planner robot pose should be identical to start frame - if(!eband_->setPlan(transformed_plan_)) - { - if (band_snapped_hack_) { - // OK - this is BAD. global planner's costmap might not have caught up - band_snapped_hack_count_++; - if (band_snapped_hack_count_ > 10) { - // need to give up - ROS_ERROR("Global replanning failed despite numerous attempts!!! Giving up."); - // reset vars + // set plan - as this is fresh from the global planner robot pose should be identical to start frame + if(!eband_->setPlan(transformed_plan_)) + { + if (band_snapped_hack_) { + // OK - this is BAD. global planner's costmap might not have caught up + band_snapped_hack_count_++; + if (band_snapped_hack_count_ > 10) { + // need to give up + ROS_ERROR("Global replanning failed despite numerous attempts!!! Giving up."); + // reset vars + band_snapped_hack_ = false; + band_snapped_hack_count_ = 0; + return false; + } + ROS_INFO("Band Hack count #%i", band_snapped_hack_count_); + return true; + + } else { + ROS_ERROR("Setting plan to Elastic Band method failed!"); + return false; + } + } else { band_snapped_hack_ = false; band_snapped_hack_count_ = 0; - return false; } - ROS_INFO("Band Hack count #%i", band_snapped_hack_count_); + ROS_INFO("Global plan set to elastic band for optimization"); + + // plan transformed and set to elastic band successfully - set counters to global variable + plan_start_end_counter_ = start_end_counts; + + // let eband refine the plan before starting continuous operation (to smooth sampling based plans) + eband_->optimizeBand(); + + + // display result + std::vector current_band; + if(eband_->getBand(current_band)) + eband_visual_->publishBand("bubbles", current_band); + + // set goal as not reached + goal_reached_ = false; + return true; + } + + + bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + if (band_snapped_hack_count_ > 0) { // waiting for global costmap to get a good plan + return false; + } + + // instantiate local variables + //std::vector local_plan; + tf::Stamped global_pose; + geometry_msgs::PoseStamped global_pose_msg; + std::vector tmp_plan; + + // get curent robot position + ROS_DEBUG("Reading current robot Position from costmap and appending it to elastic band."); + if(!costmap_ros_->getRobotPose(global_pose)) + { + ROS_WARN("Could not retrieve up to date robot pose from costmap for local planning."); + return false; + } + + // convert robot pose to frame in plan and set position in band at which to append + tf::poseStampedTFToMsg(global_pose, global_pose_msg); + tmp_plan.assign(1, global_pose_msg); + eband_local_planner::AddAtPosition add_frames_at = add_front; + + // set it to elastic band and let eband connect it + if(!eband_->addFrames(tmp_plan, add_frames_at)) + { + ROS_WARN("Could not connect robot pose to existing elastic band."); + return false; + } - } else { - ROS_ERROR("Setting plan to Elastic Band method failed!"); - return false; + // get additional path-frames which are now in moving window + ROS_DEBUG("Checking for new path frames in moving window"); + std::vector plan_start_end_counter = plan_start_end_counter_; + std::vector append_transformed_plan; + // transform global plan to the map frame we are working in - careful this also cuts the plan off (reduces it to local window) + if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, plan_start_end_counter)) + { + // if plan could not be tranformed abort control and local planning + ROS_WARN("Could not transform the global plan to the frame of the controller"); + return false; + } + + // also check if there really is a plan + if(transformed_plan_.empty()) + { + // if global plan passed in is empty... we won't do anything + ROS_WARN("Transformed plan is empty. Aborting local planner!"); + return false; + } + + ROS_DEBUG("Retrieved start-end-counts are: (%d, %d)", plan_start_end_counter.at(0), plan_start_end_counter.at(1)); + ROS_DEBUG("Current start-end-counts are: (%d, %d)", plan_start_end_counter_.at(0), plan_start_end_counter_.at(1)); + + // identify new frames - if there are any + append_transformed_plan.clear(); + // did last transformed plan end futher away from end of complete plan than this transformed plan? + if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(1)) // counting from the back (as start might be pruned) + { + // new frames in moving window + if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(0)) // counting from the back (as start might be pruned) + { + // append everything + append_transformed_plan = transformed_plan_; + } + else + { + // append only the new portion of the plan + int discarded_frames = plan_start_end_counter.at(0) - plan_start_end_counter_.at(1); + ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 >= transformed_plan_.begin()); + ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 < transformed_plan_.end()); + append_transformed_plan.assign(transformed_plan_.begin() + discarded_frames + 1, transformed_plan_.end()); + } + + // set it to elastic band and let eband connect it + ROS_DEBUG("Adding %d new frames to current band", (int) append_transformed_plan.size()); + add_frames_at = add_back; + if(eband_->addFrames(append_transformed_plan, add_back)) + { + // appended frames succesfully to global plan - set new start-end counts + ROS_DEBUG("Sucessfully added frames to band"); + plan_start_end_counter_ = plan_start_end_counter; + } + else { + ROS_WARN("Failed to add frames to existing band"); + return false; + } + } + else + ROS_DEBUG("Nothing to add"); + + // update Elastic Band (react on obstacle from costmap, ...) + ROS_DEBUG("Calling optimization method for elastic band"); + std::vector current_band; + if(!eband_->optimizeBand()) + { + ROS_WARN("Optimization failed - Band invalid - No controls availlable"); + band_snapped_hack_ = true; + // display current band + if(eband_->getBand(current_band)) + eband_visual_->publishBand("bubbles", current_band); + return false; + } else { + band_snapped_hack_ = false; + } + + // get current Elastic Band and + eband_->getBand(current_band); + // set it to the controller + if(!eband_trj_ctrl_->setBand(current_band)) + { + ROS_DEBUG("Failed to to set current band to Trajectory Controller"); + return false; + } + + // set Odometry to controller + if(!eband_trj_ctrl_->setOdometry(base_odom_)) + { + ROS_DEBUG("Failed to to set current odometry to Trajectory Controller"); + return false; + } + + // get resulting commands from the controller + geometry_msgs::Twist cmd_twist; + if(!eband_trj_ctrl_->getTwist(cmd_twist, goal_reached_)) + { + ROS_DEBUG("Failed to calculate Twist from band in Trajectory Controller"); + return false; + } + + + // set retrieved commands to reference variable + ROS_DEBUG("Retrieving velocity command: (%f, %f, %f)", cmd_twist.linear.x, cmd_twist.linear.y, cmd_twist.angular.z); + cmd_vel = cmd_twist; + + + // publish plan + std::vector refined_plan; + if(eband_->getPlan(refined_plan)) + // TODO publish local and current gloabl plan + base_local_planner::publishPlan(refined_plan, g_plan_pub_); + //base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0); + + // display current band + if(eband_->getBand(current_band)) + eband_visual_->publishBand("bubbles", current_band); + + return true; } - } else { - band_snapped_hack_ = false; - band_snapped_hack_count_ = 0; - } - ROS_INFO("Global plan set to elastic band for optimization"); - // plan transformed and set to elastic band successfully - set counters to global variable - plan_start_end_counter_ = start_end_counts; - // let eband refine the plan before starting continuous operation (to smooth sampling based plans) - eband_->optimizeBand(); + bool EBandPlannerROS::isGoalReached() + { + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + return false; + } + return goal_reached_; - // display result - std::vector current_band; - if(eband_->getBand(current_band)) - eband_visual_->publishBand("bubbles", current_band); + // // copy odometry information to local variable + // nav_msgs::Odometry base_odom; + // { + // // make sure we do not read new date from topic right at the moment + // boost::mutex::scoped_lock lock(odom_mutex_); + // base_odom = base_odom_; + // } - // set goal as not reached - goal_reached_ = false; + // tf::Stamped global_pose; + // costmap_ros_->getRobotPose(global_pose); - return true; -} + // // analogous to dwa_planner the actual check uses the routine implemented in trajectory_planner (trajectory rollout) + // bool is_goal_reached = base_local_planner::isGoalReached( + // *tf_, global_plan_, *(costmap_ros_->getCostmap()), + // costmap_ros_->getGlobalFrameID(), global_pose, base_odom, + // rot_stopped_vel_, trans_stopped_vel_, xy_goal_tolerance_, + // yaw_goal_tolerance_ + // ); + // return is_goal_reached; + + } -bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - if (band_snapped_hack_count_ > 0) { // waiting for global costmap to get a good plan - return false; - } - // instantiate local variables - //std::vector local_plan; - tf::Stamped global_pose; - geometry_msgs::PoseStamped global_pose_msg; - std::vector tmp_plan; - - // get curent robot position - ROS_DEBUG("Reading current robot Position from costmap and appending it to elastic band."); - if(!costmap_ros_->getRobotPose(global_pose)) - { - ROS_WARN("Could not retrieve up to date robot pose from costmap for local planning."); - return false; - } - - // convert robot pose to frame in plan and set position in band at which to append - tf::poseStampedTFToMsg(global_pose, global_pose_msg); - tmp_plan.assign(1, global_pose_msg); - eband_local_planner::AddAtPosition add_frames_at = add_front; - - // set it to elastic band and let eband connect it - if(!eband_->addFrames(tmp_plan, add_frames_at)) - { - ROS_WARN("Could not connect robot pose to existing elastic band."); - return false; - } - - // get additional path-frames which are now in moving window - ROS_DEBUG("Checking for new path frames in moving window"); - std::vector plan_start_end_counter = plan_start_end_counter_; - std::vector append_transformed_plan; - // transform global plan to the map frame we are working in - careful this also cuts the plan off (reduces it to local window) - if(!eband_local_planner::transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, costmap_ros_->getGlobalFrameID(), transformed_plan_, plan_start_end_counter)) - { - // if plan could not be tranformed abort control and local planning - ROS_WARN("Could not transform the global plan to the frame of the controller"); - return false; - } - - // also check if there really is a plan - if(transformed_plan_.empty()) - { - // if global plan passed in is empty... we won't do anything - ROS_WARN("Transformed plan is empty. Aborting local planner!"); - return false; - } - - ROS_DEBUG("Retrieved start-end-counts are: (%d, %d)", plan_start_end_counter.at(0), plan_start_end_counter.at(1)); - ROS_DEBUG("Current start-end-counts are: (%d, %d)", plan_start_end_counter_.at(0), plan_start_end_counter_.at(1)); - - // identify new frames - if there are any - append_transformed_plan.clear(); - // did last transformed plan end futher away from end of complete plan than this transformed plan? - if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(1)) // counting from the back (as start might be pruned) - { - // new frames in moving window - if(plan_start_end_counter_.at(1) > plan_start_end_counter.at(0)) // counting from the back (as start might be pruned) - { - // append everything - append_transformed_plan = transformed_plan_; - } - else - { - // append only the new portion of the plan - int discarded_frames = plan_start_end_counter.at(0) - plan_start_end_counter_.at(1); - ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 >= transformed_plan_.begin()); - ROS_ASSERT(transformed_plan_.begin() + discarded_frames + 1 < transformed_plan_.end()); - append_transformed_plan.assign(transformed_plan_.begin() + discarded_frames + 1, transformed_plan_.end()); - } - - // set it to elastic band and let eband connect it - ROS_DEBUG("Adding %d new frames to current band", (int) append_transformed_plan.size()); - add_frames_at = add_back; - if(eband_->addFrames(append_transformed_plan, add_back)) - { - // appended frames succesfully to global plan - set new start-end counts - ROS_DEBUG("Sucessfully added frames to band"); - plan_start_end_counter_ = plan_start_end_counter; - } - else { - ROS_WARN("Failed to add frames to existing band"); - return false; + void EBandPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) + { + // lock Callback while reading data from topic + boost::mutex::scoped_lock lock(odom_mutex_); + + // get odometry and write it to member variable (we assume that the odometry is published in the frame of the base) + base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x; + base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y; + base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z; } - } - else - ROS_DEBUG("Nothing to add"); - - // update Elastic Band (react on obstacle from costmap, ...) - ROS_DEBUG("Calling optimization method for elastic band"); - std::vector current_band; - if(!eband_->optimizeBand()) - { - ROS_WARN("Optimization failed - Band invalid - No controls availlable"); - band_snapped_hack_ = true; - // display current band - if(eband_->getBand(current_band)) - eband_visual_->publishBand("bubbles", current_band); - return false; - } else { - band_snapped_hack_ = false; - } - // get current Elastic Band and - eband_->getBand(current_band); - // set it to the controller - if(!eband_trj_ctrl_->setBand(current_band)) - { - ROS_DEBUG("Failed to to set current band to Trajectory Controller"); - return false; - } - - // set Odometry to controller - if(!eband_trj_ctrl_->setOdometry(base_odom_)) - { - ROS_DEBUG("Failed to to set current odometry to Trajectory Controller"); - return false; - } - - // get resulting commands from the controller - geometry_msgs::Twist cmd_twist; - if(!eband_trj_ctrl_->getTwist(cmd_twist, goal_reached_)) - { - ROS_DEBUG("Failed to calculate Twist from band in Trajectory Controller"); - return false; - } - - - // set retrieved commands to reference variable - ROS_DEBUG("Retrieving velocity command: (%f, %f, %f)", cmd_twist.linear.x, cmd_twist.linear.y, cmd_twist.angular.z); - cmd_vel = cmd_twist; - - - // publish plan - std::vector refined_plan; - if(eband_->getPlan(refined_plan)) - // TODO publish local and current gloabl plan - base_local_planner::publishPlan(refined_plan, g_plan_pub_); - //base_local_planner::publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0); - - // display current band - if(eband_->getBand(current_band)) - eband_visual_->publishBand("bubbles", current_band); - - return true; -} - - -bool EBandPlannerROS::isGoalReached() -{ - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); - return false; - } - - return goal_reached_; - - // // copy odometry information to local variable - // nav_msgs::Odometry base_odom; - // { - // // make sure we do not read new date from topic right at the moment - // boost::mutex::scoped_lock lock(odom_mutex_); - // base_odom = base_odom_; - // } - - // tf::Stamped global_pose; - // costmap_ros_->getRobotPose(global_pose); - - // // analogous to dwa_planner the actual check uses the routine implemented in trajectory_planner (trajectory rollout) - // bool is_goal_reached = base_local_planner::isGoalReached( - // *tf_, global_plan_, *(costmap_ros_->getCostmap()), - // costmap_ros_->getGlobalFrameID(), global_pose, base_odom, - // rot_stopped_vel_, trans_stopped_vel_, xy_goal_tolerance_, - // yaw_goal_tolerance_ - // ); - - // return is_goal_reached; - -} - - -void EBandPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) -{ - // lock Callback while reading data from topic - boost::mutex::scoped_lock lock(odom_mutex_); - - // get odometry and write it to member variable (we assume that the odometry is published in the frame of the base) - base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x; - base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y; - base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z; -} - - -} + + } diff --git a/src/eband_trajectory_controller.cpp b/src/eband_trajectory_controller.cpp index bf1f958..a5c873b 100644 --- a/src/eband_trajectory_controller.cpp +++ b/src/eband_trajectory_controller.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #include #include @@ -41,869 +41,869 @@ namespace eband_local_planner{ -using std::min; -using std::max; + using std::min; + using std::max; -EBandTrajectoryCtrl::EBandTrajectoryCtrl() : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false) {} + EBandTrajectoryCtrl::EBandTrajectoryCtrl() : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false) {} -EBandTrajectoryCtrl::EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros) - : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false) -{ - // initialize planner - initialize(name, costmap_ros); - - // Initialize pid object (note we'll be further clamping its output) - pid_.initPid(1, 0, 0, 10, -10); -} - - -EBandTrajectoryCtrl::~EBandTrajectoryCtrl() {} + EBandTrajectoryCtrl::EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros) + : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false) + { + // initialize planner + initialize(name, costmap_ros); + // Initialize pid object (note we'll be further clamping its output) + pid_.initPid(1, 0, 0, 10, -10); + } -void EBandTrajectoryCtrl::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) -{ - // check if trajectory controller is already initialized - if(!initialized_) - { - // create Node Handle with name of plugin (as used in move_base for loading) - ros::NodeHandle node_private("~/" + name); - - // read parameters from parameter server - node_private.param("max_vel_lin", max_vel_lin_, 0.75); - node_private.param("max_vel_th", max_vel_th_, 1.0); + EBandTrajectoryCtrl::~EBandTrajectoryCtrl() {} - node_private.param("min_vel_lin", min_vel_lin_, 0.1); - node_private.param("min_vel_th", min_vel_th_, 0.0); - node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0); - node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0); + void EBandTrajectoryCtrl::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) + { - node_private.param("xy_goal_tolerance", tolerance_trans_, 0.02); - node_private.param("yaw_goal_tolerance", tolerance_rot_, 0.04); - node_private.param("tolerance_timeout", tolerance_timeout_, 0.5); + // check if trajectory controller is already initialized + if(!initialized_) + { + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle node_private("~/" + name); - node_private.param("k_prop", k_p_, 4.0); - node_private.param("k_damp", k_nu_, 3.5); + // read parameters from parameter server + node_private.param("max_vel_lin", max_vel_lin_, 0.75); + node_private.param("max_vel_th", max_vel_th_, 1.0); - node_private.param("Ctrl_Rate", ctrl_freq_, 10.0); // TODO retrieve this from move base parameters + node_private.param("min_vel_lin", min_vel_lin_, 0.1); + node_private.param("min_vel_th", min_vel_th_, 0.0); - node_private.param("max_acceleration", acc_max_, 0.5); - node_private.param("virtual_mass", virt_mass_, 0.75); + node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0); + node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0); - node_private.param("max_translational_acceleration", acc_max_trans_, 0.5); - node_private.param("max_rotational_acceleration", acc_max_rot_, 1.5); + node_private.param("xy_goal_tolerance", tolerance_trans_, 0.02); + node_private.param("yaw_goal_tolerance", tolerance_rot_, 0.04); + node_private.param("tolerance_timeout", tolerance_timeout_, 0.5); - node_private.param("rotation_correction_threshold", rotation_correction_threshold_, 0.5); + node_private.param("k_prop", k_p_, 4.0); + node_private.param("k_damp", k_nu_, 3.5); - // diffferential drive parameters - node_private.param("differential_drive", differential_drive_hack_, true); - node_private.param("k_int", k_int_, 0.005); - node_private.param("k_diff", k_diff_, -0.005); - node_private.param("bubble_velocity_multiplier", bubble_velocity_multiplier_, 2.0); - node_private.param("rotation_threshold_multiplier", rotation_threshold_multiplier_, 0.75); - // Ctrl_rate, k_prop, max_vel_lin, max_vel_th, tolerance_trans, tolerance_rot, min_in_place_vel_th + node_private.param("Ctrl_Rate", ctrl_freq_, 10.0); // TODO retrieve this from move base parameters - // copy adress of costmap and Transform Listener (handed over from move_base) - costmap_ros_ = costmap_ros; + node_private.param("max_acceleration", acc_max_, 0.5); + node_private.param("virtual_mass", virt_mass_, 0.75); - // init velocity for interpolation - last_vel_.linear.x = 0.0; - last_vel_.linear.y = 0.0; - last_vel_.linear.z = 0.0; - last_vel_.angular.x = 0.0; - last_vel_.angular.y = 0.0; - last_vel_.angular.z = 0.0; + node_private.param("max_translational_acceleration", acc_max_trans_, 0.5); + node_private.param("max_rotational_acceleration", acc_max_rot_, 1.5); - // set the general refernce frame to that in which the band is given - geometry_msgs::Pose2D tmp_pose2D; - tmp_pose2D.x = 0.0; - tmp_pose2D.y = 0.0; - tmp_pose2D.theta = 0.0; - Pose2DToPose(ref_frame_band_, tmp_pose2D); + node_private.param("rotation_correction_threshold", rotation_correction_threshold_, 0.5); - // set initialized flag - initialized_ = true; - } - else - { - ROS_WARN("This planner has already been initialized, doing nothing."); - } -} + // diffferential drive parameters + node_private.param("differential_drive", differential_drive_hack_, true); + node_private.param("k_int", k_int_, 0.005); + node_private.param("k_diff", k_diff_, -0.005); + node_private.param("bubble_velocity_multiplier", bubble_velocity_multiplier_, 2.0); + node_private.param("rotation_threshold_multiplier", rotation_threshold_multiplier_, 0.75); + // Ctrl_rate, k_prop, max_vel_lin, max_vel_th, tolerance_trans, tolerance_rot, min_in_place_vel_th + // copy adress of costmap and Transform Listener (handed over from move_base) + costmap_ros_ = costmap_ros; -void EBandTrajectoryCtrl::setVisualization(boost::shared_ptr target_visual) -{ - target_visual_ = target_visual; + // init velocity for interpolation + last_vel_.linear.x = 0.0; + last_vel_.linear.y = 0.0; + last_vel_.linear.z = 0.0; + last_vel_.angular.x = 0.0; + last_vel_.angular.y = 0.0; + last_vel_.angular.z = 0.0; - visualization_ = true; -} + // set the general refernce frame to that in which the band is given + geometry_msgs::Pose2D tmp_pose2D; + tmp_pose2D.x = 0.0; + tmp_pose2D.y = 0.0; + tmp_pose2D.theta = 0.0; + Pose2DToPose(ref_frame_band_, tmp_pose2D); -bool EBandTrajectoryCtrl::setBand(const std::vector& elastic_band) -{ - elastic_band_ = elastic_band; - band_set_ = true; - return true; -} + // set initialized flag + initialized_ = true; + } + else + { + ROS_WARN("This planner has already been initialized, doing nothing."); + } + } -bool EBandTrajectoryCtrl::setOdometry(const nav_msgs::Odometry& odometry) -{ - odom_vel_.linear.x = odometry.twist.twist.linear.x; - odom_vel_.linear.y = odometry.twist.twist.linear.y; - odom_vel_.linear.z = 0.0; - odom_vel_.angular.x = 0.0; - odom_vel_.angular.y = 0.0; - odom_vel_.angular.z = odometry.twist.twist.angular.z; + void EBandTrajectoryCtrl::setVisualization(boost::shared_ptr target_visual) + { + target_visual_ = target_visual; - return true; -} + visualization_ = true; + } -// Return the angular difference between the direction we're pointing -// and the direction we want to move in -double angularDiff (const geometry_msgs::Twist& heading, - const geometry_msgs::Pose& pose) -{ - const double pi = 3.14159265; - const double t1 = atan2(heading.linear.y, heading.linear.x); - const double t2 = tf::getYaw(pose.orientation); - const double d = t1-t2; - - if (fabs(d)& elastic_band) + { + elastic_band_ = elastic_band; + band_set_ = true; + return true; + } -bool EBandTrajectoryCtrl::getTwistDifferentialDrive(geometry_msgs::Twist& twist_cmd, bool& goal_reached) { - goal_reached = false; - geometry_msgs::Twist robot_cmd, bubble_diff; - robot_cmd.linear.x = 0.0; - robot_cmd.angular.z = 0.0; + bool EBandTrajectoryCtrl::setOdometry(const nav_msgs::Odometry& odometry) + { + odom_vel_.linear.x = odometry.twist.twist.linear.x; + odom_vel_.linear.y = odometry.twist.twist.linear.y; + odom_vel_.linear.z = 0.0; + odom_vel_.angular.x = 0.0; + odom_vel_.angular.y = 0.0; + odom_vel_.angular.z = odometry.twist.twist.angular.z; - bool command_provided = false; + return true; + } - // check if plugin initialized - if(!initialized_) { - ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner"); - return false; - } + // Return the angular difference between the direction we're pointing + // and the direction we want to move in + double angularDiff (const geometry_msgs::Twist& heading, + const geometry_msgs::Pose& pose) + { + const double pi = 3.14159265; + const double t1 = atan2(heading.linear.y, heading.linear.x); + const double t2 = tf::getYaw(pose.orientation); + const double d = t1-t2; + + if (fabs(d) tolerance_trans_ || - fabs(bubble_diff.linear.y) > tolerance_trans_) { - in_final_goal_turn_ = false; + // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan) + if( (!band_set_) || (elastic_band_.size() < 2) ) { + ROS_WARN("Requesting feedforward command from empty band."); + return false; } // Get the differences between the first 2 bubbles in the robot's frame - curr_target_bubble = 1; bubble_diff = getFrame1ToFrame2InRefFrameNew( elastic_band_.at(0).center.pose, elastic_band_.at(1).center.pose, elastic_band_.at(0).center.pose); - // using 0.75 here as you might go out of tolerance during rotation step - // once you enter rotation, unless you exceed tolerances, continue with rotation - while(fabs(bubble_diff.linear.x) <= 0.75 * tolerance_trans_ && - fabs(bubble_diff.linear.y) <= 0.75 * tolerance_trans_ || - in_final_goal_turn_) { - if(curr_target_bubble < ((int) elastic_band_.size()) - 1) { + // Check 1 + // We need to check if we are within the threshold of the final destination + if (!command_provided) { + int curr_target_bubble = 1; + + while(curr_target_bubble < ((int) elastic_band_.size()) - 1) { curr_target_bubble++; bubble_diff = + getFrame1ToFrame2InRefFrameNew( + elastic_band_.at(0).center.pose, + elastic_band_.at(curr_target_bubble).center.pose, + elastic_band_.at(0).center.pose); + } + + // if you go past tolerance, then try to get closer again + if(fabs(bubble_diff.linear.x) > tolerance_trans_ || + fabs(bubble_diff.linear.y) > tolerance_trans_) { + in_final_goal_turn_ = false; + } + + // Get the differences between the first 2 bubbles in the robot's frame + curr_target_bubble = 1; + bubble_diff = getFrame1ToFrame2InRefFrameNew( + elastic_band_.at(0).center.pose, + elastic_band_.at(1).center.pose, + elastic_band_.at(0).center.pose); + + // using 0.75 here as you might go out of tolerance during rotation step + // once you enter rotation, unless you exceed tolerances, continue with rotation + while(fabs(bubble_diff.linear.x) <= 0.75 * tolerance_trans_ && + fabs(bubble_diff.linear.y) <= 0.75 * tolerance_trans_ || + in_final_goal_turn_) { + if(curr_target_bubble < ((int) elastic_band_.size()) - 1) { + curr_target_bubble++; + bubble_diff = getFrame1ToFrame2InRefFrameNew( elastic_band_.at(0).center.pose, elastic_band_.at(curr_target_bubble).center.pose, elastic_band_.at(0).center.pose); - } else { - // Calculate orientation difference to goal orientation (not captured in bubble_diff) - double robot_yaw = tf::getYaw(elastic_band_.at(0).center.pose.orientation); - double goal_yaw = tf::getYaw(elastic_band_.at((int)elastic_band_.size() - 1).center.pose.orientation); - float orientation_diff = angles::normalize_angle(goal_yaw - robot_yaw); - if (fabs(orientation_diff) > tolerance_rot_) { - in_final_goal_turn_ = true; - ROS_DEBUG("Performing in place rotation for goal (diff): %f", orientation_diff); - double rotation_sign = -2 * (orientation_diff < 0) + 1; - robot_cmd.angular.z = - rotation_sign * min_in_place_vel_th_ + k_p_ * orientation_diff; - if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation - robot_cmd.angular.z = rotation_sign * max_vel_th_; - } } else { - ROS_INFO ("TrajectoryController: Goal reached with distance %.2f, %.2f (od = %.2f)" - "; sending zero velocity", - bubble_diff.linear.x, bubble_diff.linear.y, orientation_diff); - // goal position reached - robot_cmd.linear.x = 0.0; - robot_cmd.angular.z = 0.0; - goal_reached = true; + // Calculate orientation difference to goal orientation (not captured in bubble_diff) + double robot_yaw = tf::getYaw(elastic_band_.at(0).center.pose.orientation); + double goal_yaw = tf::getYaw(elastic_band_.at((int)elastic_band_.size() - 1).center.pose.orientation); + float orientation_diff = angles::normalize_angle(goal_yaw - robot_yaw); + if (fabs(orientation_diff) > tolerance_rot_) { + in_final_goal_turn_ = true; + ROS_DEBUG("Performing in place rotation for goal (diff): %f", orientation_diff); + double rotation_sign = -2 * (orientation_diff < 0) + 1; + robot_cmd.angular.z = + rotation_sign * min_in_place_vel_th_ + k_p_ * orientation_diff; + if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation + robot_cmd.angular.z = rotation_sign * max_vel_th_; + } + } else { + ROS_INFO ("TrajectoryController: Goal reached with distance %.2f, %.2f (od = %.2f)" + "; sending zero velocity", + bubble_diff.linear.x, bubble_diff.linear.y, orientation_diff); + // goal position reached + robot_cmd.linear.x = 0.0; + robot_cmd.angular.z = 0.0; + goal_reached = true; + } + command_provided = true; + break; } - command_provided = true; - break; } } - } - // Get the differences between the first 2 bubbles in the robot's frame - bubble_diff = getFrame1ToFrame2InRefFrameNew( - elastic_band_.at(0).center.pose, - elastic_band_.at(1).center.pose, - elastic_band_.at(0).center.pose); - - // Check 1 - check if the robot's current pose is too misaligned with the next bubble - if (!command_provided) { - // calculate an estimate of the in-place rotation threshold - double distance_to_next_bubble = sqrt( - bubble_diff.linear.x * bubble_diff.linear.x + - bubble_diff.linear.y * bubble_diff.linear.y); - double radius_of_next_bubble = 0.7 * elastic_band_.at(1).expansion; - double in_place_rotation_threshold = + // Get the differences between the first 2 bubbles in the robot's frame + bubble_diff = getFrame1ToFrame2InRefFrameNew( + elastic_band_.at(0).center.pose, + elastic_band_.at(1).center.pose, + elastic_band_.at(0).center.pose); + + // Check 1 - check if the robot's current pose is too misaligned with the next bubble + if (!command_provided) { + // calculate an estimate of the in-place rotation threshold + double distance_to_next_bubble = sqrt( + bubble_diff.linear.x * bubble_diff.linear.x + + bubble_diff.linear.y * bubble_diff.linear.y); + double radius_of_next_bubble = 0.7 * elastic_band_.at(1).expansion; + double in_place_rotation_threshold = rotation_threshold_multiplier_ * fabs(atan2(radius_of_next_bubble,distance_to_next_bubble)); - ROS_DEBUG("In-place rotation threshold: %f(%f,%f)", - in_place_rotation_threshold, radius_of_next_bubble, distance_to_next_bubble); - - // check if we are above this threshold, if so then perform in-place rotation - if (fabs(bubble_diff.angular.z) > in_place_rotation_threshold) { - robot_cmd.angular.z = k_p_ * bubble_diff.angular.z; - double rotation_sign = (bubble_diff.angular.z < 0) ? -1.0 : +1.0; - if (fabs(robot_cmd.angular.z) < min_in_place_vel_th_) { - robot_cmd.angular.z = rotation_sign * min_in_place_vel_th_; + ROS_DEBUG("In-place rotation threshold: %f(%f,%f)", + in_place_rotation_threshold, radius_of_next_bubble, distance_to_next_bubble); + + // check if we are above this threshold, if so then perform in-place rotation + if (fabs(bubble_diff.angular.z) > in_place_rotation_threshold) { + robot_cmd.angular.z = k_p_ * bubble_diff.angular.z; + double rotation_sign = (bubble_diff.angular.z < 0) ? -1.0 : +1.0; + if (fabs(robot_cmd.angular.z) < min_in_place_vel_th_) { + robot_cmd.angular.z = rotation_sign * min_in_place_vel_th_; + } + if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation + robot_cmd.angular.z = rotation_sign * max_vel_th_; + } + ROS_DEBUG("Performing in place rotation for start (diff): %f", bubble_diff.angular.z, robot_cmd.angular.z); + command_provided = true; } - if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation - robot_cmd.angular.z = rotation_sign * max_vel_th_; + } + + + // Check 3 - If we reach here, it means we need to use our PID controller to + // move towards the next bubble + if (!command_provided) { + + // Select a linear velocity (based on the current bubble radius) + double forward_sign = -2 * (bubble_diff.linear.x < 0) + 1; + double bubble_radius = 0.7 * elastic_band_.at(0).expansion; + double velocity_multiplier = bubble_velocity_multiplier_ * bubble_radius; + double linear_velocity = velocity_multiplier * max_vel_lin_; + linear_velocity *= cos(bubble_diff.angular.z); //decrease while turning + if (fabs(linear_velocity) > max_vel_lin_) { + linear_velocity = forward_sign * max_vel_lin_; + } else if (fabs(linear_velocity) < min_vel_lin_) { + linear_velocity = forward_sign * min_vel_lin_; } - ROS_DEBUG("Performing in place rotation for start (diff): %f", bubble_diff.angular.z, robot_cmd.angular.z); + + // Select an angular velocity (based on PID controller) + double error = bubble_diff.angular.z; + double rotation_sign = -2 * (bubble_diff.angular.z < 0) + 1; + double angular_velocity = k_p_ * error; + if (fabs(angular_velocity) > max_vel_th_) { + angular_velocity = rotation_sign * max_vel_th_; + } else if (fabs(angular_velocity) < min_vel_th_) { + angular_velocity = rotation_sign * min_vel_th_; + } + + ROS_DEBUG("Selected velocity: lin: %f, ang: %f", + linear_velocity, angular_velocity); + + robot_cmd.linear.x = linear_velocity; + robot_cmd.angular.z = angular_velocity; command_provided = true; } + + twist_cmd = robot_cmd; + ROS_DEBUG("Final command: %f, %f", twist_cmd.linear.x, twist_cmd.angular.z); + return true; } - // Check 3 - If we reach here, it means we need to use our PID controller to - // move towards the next bubble - if (!command_provided) { - - // Select a linear velocity (based on the current bubble radius) - double forward_sign = -2 * (bubble_diff.linear.x < 0) + 1; - double bubble_radius = 0.7 * elastic_band_.at(0).expansion; - double velocity_multiplier = bubble_velocity_multiplier_ * bubble_radius; - double linear_velocity = velocity_multiplier * max_vel_lin_; - linear_velocity *= cos(bubble_diff.angular.z); //decrease while turning - if (fabs(linear_velocity) > max_vel_lin_) { - linear_velocity = forward_sign * max_vel_lin_; - } else if (fabs(linear_velocity) < min_vel_lin_) { - linear_velocity = forward_sign * min_vel_lin_; + bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached) + { + goal_reached = false; + if (differential_drive_hack_) { + return getTwistDifferentialDrive(twist_cmd, goal_reached); } - // Select an angular velocity (based on PID controller) - double error = bubble_diff.angular.z; - double rotation_sign = -2 * (bubble_diff.angular.z < 0) + 1; - double angular_velocity = k_p_ * error; - if (fabs(angular_velocity) > max_vel_th_) { - angular_velocity = rotation_sign * max_vel_th_; - } else if (fabs(angular_velocity) < min_vel_th_) { - angular_velocity = rotation_sign * min_vel_th_; + // init twist cmd to be handed back to caller + geometry_msgs::Twist robot_cmd, bubble_diff, control_deviation; + robot_cmd.linear.x = 0.0; + robot_cmd.linear.y = 0.0; + robot_cmd.linear.z = 0.0; + robot_cmd.angular.x = 0.0; + robot_cmd.angular.y = 0.0; + robot_cmd.angular.z = 0.0; + + // make sure command vector is clean + twist_cmd = robot_cmd; + control_deviation = robot_cmd; + + // check if plugin initialized + if(!initialized_) + { + ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner"); + return false; } - ROS_DEBUG("Selected velocity: lin: %f, ang: %f", - linear_velocity, angular_velocity); + // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan) + if( (!band_set_) || (elastic_band_.size() < 2) ) + { + ROS_WARN("Requesting feedforward command from empty band."); + return false; + } - robot_cmd.linear.x = linear_velocity; - robot_cmd.angular.z = angular_velocity; - command_provided = true; - } + // calc intersection of bubble-radius with sequence of vector connecting the bubbles - twist_cmd = robot_cmd; - ROS_DEBUG("Final command: %f, %f", twist_cmd.linear.x, twist_cmd.angular.z); - return true; -} + // get distance to target from bubble-expansion + double scaled_radius = 0.7 * elastic_band_.at(0).expansion; + // get difference and distance between bubbles in odometry frame + double bubble_distance, ang_pseudo_dist; + bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, + elastic_band_.at(1).center.pose, + ref_frame_band_); + ang_pseudo_dist = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); + bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) + + (ang_pseudo_dist * ang_pseudo_dist) ); + + if(visualization_) + { + target_visual_->publishBubble("ctrl_target", 1, target_visual_->blue, elastic_band_.at(0)); + target_visual_->publishBubble("ctrl_target", 2, target_visual_->blue, elastic_band_.at(1)); + } -bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached) -{ - goal_reached = false; - if (differential_drive_hack_) { - return getTwistDifferentialDrive(twist_cmd, goal_reached); - } + // by default our control deviation is the difference between the bubble centers + double abs_ctrl_dev; + control_deviation = bubble_diff; + + + ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_); + abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) + + (control_deviation.linear.y * control_deviation.linear.y) + + (ang_pseudo_dist * ang_pseudo_dist) ); + + // yet depending on the expansion of our bubble we might want to adapt this point + if(scaled_radius < bubble_distance) + { + // triviale case - simply scale bubble_diff + double scale_difference = scaled_radius / bubble_distance; + bubble_diff.linear.x *= scale_difference; + bubble_diff.linear.y *= scale_difference; + bubble_diff.angular.z *= scale_difference; + // set controls + control_deviation = bubble_diff; + } - // init twist cmd to be handed back to caller - geometry_msgs::Twist robot_cmd, bubble_diff, control_deviation; - robot_cmd.linear.x = 0.0; - robot_cmd.linear.y = 0.0; - robot_cmd.linear.z = 0.0; - robot_cmd.angular.x = 0.0; - robot_cmd.angular.y = 0.0; - robot_cmd.angular.z = 0.0; - - // make sure command vector is clean - twist_cmd = robot_cmd; - control_deviation = robot_cmd; - - // check if plugin initialized - if(!initialized_) - { - ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner"); - return false; - } - - // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan) - if( (!band_set_) || (elastic_band_.size() < 2) ) - { - ROS_WARN("Requesting feedforward command from empty band."); - return false; - } - - // calc intersection of bubble-radius with sequence of vector connecting the bubbles - - // get distance to target from bubble-expansion - double scaled_radius = 0.7 * elastic_band_.at(0).expansion; - - // get difference and distance between bubbles in odometry frame - double bubble_distance, ang_pseudo_dist; - bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, - elastic_band_.at(1).center.pose, - ref_frame_band_); - ang_pseudo_dist = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); - bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) + - (ang_pseudo_dist * ang_pseudo_dist) ); - - if(visualization_) - { - target_visual_->publishBubble("ctrl_target", 1, target_visual_->blue, elastic_band_.at(0)); - target_visual_->publishBubble("ctrl_target", 2, target_visual_->blue, elastic_band_.at(1)); - } - - // by default our control deviation is the difference between the bubble centers - double abs_ctrl_dev; - control_deviation = bubble_diff; - - - ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_); - abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) + - (control_deviation.linear.y * control_deviation.linear.y) + - (ang_pseudo_dist * ang_pseudo_dist) ); - - // yet depending on the expansion of our bubble we might want to adapt this point - if(scaled_radius < bubble_distance) - { - // triviale case - simply scale bubble_diff - double scale_difference = scaled_radius / bubble_distance; - bubble_diff.linear.x *= scale_difference; - bubble_diff.linear.y *= scale_difference; - bubble_diff.angular.z *= scale_difference; - // set controls - control_deviation = bubble_diff; - } - - // if scaled_radius = bubble_distance -- we have nothing to do at all - - if(scaled_radius > bubble_distance) - { - // o.k. now we have to do a little bit more -> check next but one bubble - if(elastic_band_.size() > 2) - { - // get difference between next and next but one bubble - double next_bubble_distance; - geometry_msgs::Twist next_bubble_diff; - next_bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(1).center.pose, - elastic_band_.at(2).center.pose, - ref_frame_band_); - ang_pseudo_dist = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); - next_bubble_distance = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) + - (next_bubble_diff.linear.y * next_bubble_diff.linear.y) + - (ang_pseudo_dist * ang_pseudo_dist) ); - - if(scaled_radius > (bubble_distance + next_bubble_distance) ) - { - // we should normally not end up here - but just to be sure - control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x; - control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y; - control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z; - // done - if(visualization_) - target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2)); - } - else - { - if(visualization_) - target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2)); - - // we want to calculate intersection point of bubble ... - // ... and vector connecting the following bubbles - double b_distance, cosine_at_bub; - double vec_prod, norm_vec1, norm_vec2; - double ang_pseudo_dist1, ang_pseudo_dist2; - - // get distance between next bubble center and intersection point - ang_pseudo_dist1 = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); - ang_pseudo_dist2 = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); - // careful! - we need this sign because of the direction of the vectors and the definition of the vector-product - vec_prod = - ( (bubble_diff.linear.x * next_bubble_diff.linear.x) + - (bubble_diff.linear.y * next_bubble_diff.linear.y) + - (ang_pseudo_dist1 * ang_pseudo_dist2) ); - - norm_vec1 = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + - (bubble_diff.linear.y * bubble_diff.linear.y) + - (ang_pseudo_dist1 * ang_pseudo_dist1) ); - - norm_vec2 = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) + - (next_bubble_diff.linear.y * next_bubble_diff.linear.y) + - (ang_pseudo_dist2 * ang_pseudo_dist2) ); - - // reform the cosine-rule - cosine_at_bub = vec_prod / norm_vec1 / norm_vec2; - b_distance = bubble_distance * cosine_at_bub + sqrt( scaled_radius*scaled_radius - - bubble_distance*bubble_distance * (1.0 - cosine_at_bub*cosine_at_bub) ); - - // get difference vector from next_bubble to intersection point - double scale_next_difference = b_distance / next_bubble_distance; - next_bubble_diff.linear.x *= scale_next_difference; - next_bubble_diff.linear.y *= scale_next_difference; - next_bubble_diff.angular.z *= scale_next_difference; - - // and finally get the control deviation - control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x; - control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y; - control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z; - // done - } - } - } - - // plot control deviation - ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_); - abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) + - (control_deviation.linear.y * control_deviation.linear.y) + - (ang_pseudo_dist * ang_pseudo_dist) ); - - - if(visualization_) - { - // compose bubble from ctrl-target - geometry_msgs::Pose2D tmp_bubble_2d, curr_bubble_2d; - geometry_msgs::Pose tmp_pose; - // init bubble for visualization - Bubble new_bubble = elastic_band_.at(0); - PoseToPose2D(elastic_band_.at(0).center.pose, curr_bubble_2d); - tmp_bubble_2d.x = curr_bubble_2d.x + control_deviation.linear.x; - tmp_bubble_2d.y = curr_bubble_2d.y + control_deviation.linear.y; - tmp_bubble_2d.theta = curr_bubble_2d.theta + control_deviation.angular.z; - Pose2DToPose(tmp_pose, tmp_bubble_2d); - new_bubble.center.pose = tmp_pose; - new_bubble.expansion = 0.1; // just draw a small bubble - target_visual_->publishBubble("ctrl_target", 0, target_visual_->red, new_bubble); - } - - - const geometry_msgs::Point& goal = (--elastic_band_.end())->center.pose.position; - const double dx = elastic_band_.at(0).center.pose.position.x - goal.x; - const double dy = elastic_band_.at(0).center.pose.position.y - goal.y; - const double dist_to_goal = sqrt(dx*dx + dy*dy); - - // Assuming we're far enough from the final goal, make sure to rotate so - // we're facing the right way - if (dist_to_goal > rotation_correction_threshold_) + // if scaled_radius = bubble_distance -- we have nothing to do at all + + if(scaled_radius > bubble_distance) + { + // o.k. now we have to do a little bit more -> check next but one bubble + if(elastic_band_.size() > 2) + { + // get difference between next and next but one bubble + double next_bubble_distance; + geometry_msgs::Twist next_bubble_diff; + next_bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(1).center.pose, + elastic_band_.at(2).center.pose, + ref_frame_band_); + ang_pseudo_dist = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); + next_bubble_distance = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) + + (next_bubble_diff.linear.y * next_bubble_diff.linear.y) + + (ang_pseudo_dist * ang_pseudo_dist) ); + + if(scaled_radius > (bubble_distance + next_bubble_distance) ) { - - const double angular_diff = angularDiff(control_deviation, elastic_band_.at(0).center.pose); - const double vel = pid_.computeCommand(angular_diff, ros::Duration(1/ctrl_freq_)); - const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_/fabs(vel) : 1.0; - control_deviation.angular.z = vel*mult; - const double abs_vel = fabs(control_deviation.angular.z); - - ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", - "Angular diff is %.2f and desired angular " - "vel is %.2f. Initial translation velocity " - "is %.2f, %.2f", angular_diff, - control_deviation.angular.z, - control_deviation.linear.x, - control_deviation.linear.y); - const double trans_mult = max(0.01, 1.0 - abs_vel/max_vel_th_); // There are some weird tf errors if I let it be 0 - control_deviation.linear.x *= trans_mult; - control_deviation.linear.y *= trans_mult; - ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", - "Translation multiplier is %.2f and scaled " - "translational velocity is %.2f, %.2f", - trans_mult, control_deviation.linear.x, - control_deviation.linear.y); + // we should normally not end up here - but just to be sure + control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x; + control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y; + control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z; + // done + if(visualization_) + target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2)); } else - ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", - "Not applying angle correction because " - "distance to goal is %.2f", dist_to_goal); - - - - - // now the actual control procedure start (using attractive Potentials) - geometry_msgs::Twist desired_velocity, currbub_maxvel_dir; - double desvel_abs, desvel_abs_trans, currbub_maxvel_abs; - double scale_des_vel; - desired_velocity = robot_cmd; - currbub_maxvel_dir = robot_cmd; - - // calculate "equilibrium velocity" (Khatib86 - Realtime Obstacle Avoidance) - desired_velocity.linear.x = k_p_/k_nu_ * control_deviation.linear.x; - desired_velocity.linear.y = k_p_/k_nu_ * control_deviation.linear.y; - desired_velocity.angular.z = k_p_/k_nu_ * control_deviation.angular.z; - - //robot_cmd = desired_velocity; - - // get max vel for current bubble - int curr_bub_num = 0; - currbub_maxvel_abs = getBubbleTargetVel(curr_bub_num, elastic_band_, currbub_maxvel_dir); - - // if neccessarry scale desired vel to stay lower than currbub_maxvel_abs - ang_pseudo_dist = desired_velocity.angular.z * getCircumscribedRadius(*costmap_ros_); - desvel_abs = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) + - (desired_velocity.linear.y * desired_velocity.linear.y) + - (ang_pseudo_dist * ang_pseudo_dist) ); - if(desvel_abs > currbub_maxvel_abs) - { - scale_des_vel = currbub_maxvel_abs / desvel_abs; - desired_velocity.linear.x *= scale_des_vel; - desired_velocity.linear.y *= scale_des_vel; - desired_velocity.angular.z *= scale_des_vel; - } - - // make sure to stay within velocity bounds for the robot - desvel_abs_trans = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) + (desired_velocity.linear.y * desired_velocity.linear.y) ); - // for translation - if(desvel_abs_trans > max_vel_lin_) - { - scale_des_vel = max_vel_lin_ / desvel_abs_trans; - desired_velocity.linear.x *= scale_des_vel; - desired_velocity.linear.y *= scale_des_vel; - // to make sure we are staying inside the bubble also scale rotation - desired_velocity.angular.z *= scale_des_vel; - } - - // for rotation - if(fabs(desired_velocity.angular.z) > max_vel_th_) - { - scale_des_vel = max_vel_th_ / fabs(desired_velocity.angular.z); - desired_velocity.angular.z *= scale_des_vel; - // to make sure we are staying inside the bubble also scale translation - desired_velocity.linear.x *= scale_des_vel; - desired_velocity.linear.y *= scale_des_vel; - } - - // calculate resulting force (accel. resp.) (Khatib86 - Realtime Obstacle Avoidance) - geometry_msgs::Twist acc_desired; - acc_desired = robot_cmd; - acc_desired.linear.x = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.x - last_vel_.linear.x); - acc_desired.linear.y = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.y - last_vel_.linear.y); - acc_desired.angular.z = (1.0/virt_mass_) * k_nu_ * (desired_velocity.angular.z - last_vel_.angular.z); - - // constrain acceleration - double scale_acc; - double abs_acc_trans = sqrt( (acc_desired.linear.x*acc_desired.linear.x) + (acc_desired.linear.y*acc_desired.linear.y) ); - if(abs_acc_trans > acc_max_trans_) - { - scale_acc = acc_max_trans_ / abs_acc_trans; - acc_desired.linear.x *= scale_acc; - acc_desired.linear.y *= scale_acc; - // again - keep relations - stay in bubble - acc_desired.angular.z *= scale_acc; - } - - if(fabs(acc_desired.angular.z) > acc_max_rot_) - { - scale_acc = fabs(acc_desired.angular.z) / acc_max_rot_; - acc_desired.angular.z *= scale_acc; - // again - keep relations - stay in bubble - acc_desired.linear.x *= scale_acc; - acc_desired.linear.y *= scale_acc; - } - - // and get velocity-cmds by integrating them over one time-step - last_vel_.linear.x = last_vel_.linear.x + acc_desired.linear.x / ctrl_freq_; - last_vel_.linear.y = last_vel_.linear.y + acc_desired.linear.y / ctrl_freq_; - last_vel_.angular.z = last_vel_.angular.z + acc_desired.angular.z / ctrl_freq_; - - - // we are almost done now take into accoun stick-slip and similar nasty things - - // last checks - limit current twist cmd (upper and lower bounds) - last_vel_ = limitTwist(last_vel_); - - // finally set robot_cmd (to non-zero value) - robot_cmd = last_vel_; - - // now convert into robot-body frame - robot_cmd = transformTwistFromFrame1ToFrame2(robot_cmd, ref_frame_band_, elastic_band_.at(0).center.pose); - - // check whether we reached the end of the band - int curr_target_bubble = 1; - while(fabs(bubble_diff.linear.x) <= tolerance_trans_ && - fabs(bubble_diff.linear.y) <= tolerance_trans_ && - fabs(bubble_diff.angular.z) <= tolerance_rot_) - { - if(curr_target_bubble < ((int) elastic_band_.size()) - 1) - { - curr_target_bubble++; - // transform next target bubble into robot-body frame - // and get difference to robot bubble - bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, elastic_band_.at(curr_target_bubble).center.pose, - ref_frame_band_); - } - else - { - ROS_DEBUG_THROTTLE_NAMED (1.0, "controller_state", - "Goal reached with distance %.2f, %.2f, %.2f" - "; sending zero velocity", - bubble_diff.linear.x, bubble_diff.linear.y, - bubble_diff.angular.z); - // goal position reached - robot_cmd.linear.x = 0.0; - robot_cmd.linear.y = 0.0; - robot_cmd.angular.z = 0.0; - // reset velocity - last_vel_.linear.x = 0.0; - last_vel_.linear.y = 0.0; - last_vel_.angular.z = 0.0; - goal_reached = true; - break; - } - } - - twist_cmd = robot_cmd; - - return true; -} + { + if(visualization_) + target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2)); + + // we want to calculate intersection point of bubble ... + // ... and vector connecting the following bubbles + double b_distance, cosine_at_bub; + double vec_prod, norm_vec1, norm_vec2; + double ang_pseudo_dist1, ang_pseudo_dist2; + + // get distance between next bubble center and intersection point + ang_pseudo_dist1 = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); + ang_pseudo_dist2 = next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); + // careful! - we need this sign because of the direction of the vectors and the definition of the vector-product + vec_prod = - ( (bubble_diff.linear.x * next_bubble_diff.linear.x) + + (bubble_diff.linear.y * next_bubble_diff.linear.y) + + (ang_pseudo_dist1 * ang_pseudo_dist2) ); + + norm_vec1 = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + + (bubble_diff.linear.y * bubble_diff.linear.y) + + (ang_pseudo_dist1 * ang_pseudo_dist1) ); + + norm_vec2 = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) + + (next_bubble_diff.linear.y * next_bubble_diff.linear.y) + + (ang_pseudo_dist2 * ang_pseudo_dist2) ); + + // reform the cosine-rule + cosine_at_bub = vec_prod / norm_vec1 / norm_vec2; + b_distance = bubble_distance * cosine_at_bub + sqrt( scaled_radius*scaled_radius - + bubble_distance*bubble_distance * (1.0 - cosine_at_bub*cosine_at_bub) ); + + // get difference vector from next_bubble to intersection point + double scale_next_difference = b_distance / next_bubble_distance; + next_bubble_diff.linear.x *= scale_next_difference; + next_bubble_diff.linear.y *= scale_next_difference; + next_bubble_diff.angular.z *= scale_next_difference; + + // and finally get the control deviation + control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x; + control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y; + control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z; + // done + } + } + } + // plot control deviation + ang_pseudo_dist = control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_); + abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) + + (control_deviation.linear.y * control_deviation.linear.y) + + (ang_pseudo_dist * ang_pseudo_dist) ); + + + if(visualization_) + { + // compose bubble from ctrl-target + geometry_msgs::Pose2D tmp_bubble_2d, curr_bubble_2d; + geometry_msgs::Pose tmp_pose; + // init bubble for visualization + Bubble new_bubble = elastic_band_.at(0); + PoseToPose2D(elastic_band_.at(0).center.pose, curr_bubble_2d); + tmp_bubble_2d.x = curr_bubble_2d.x + control_deviation.linear.x; + tmp_bubble_2d.y = curr_bubble_2d.y + control_deviation.linear.y; + tmp_bubble_2d.theta = curr_bubble_2d.theta + control_deviation.angular.z; + Pose2DToPose(tmp_pose, tmp_bubble_2d); + new_bubble.center.pose = tmp_pose; + new_bubble.expansion = 0.1; // just draw a small bubble + target_visual_->publishBubble("ctrl_target", 0, target_visual_->red, new_bubble); + } -double EBandTrajectoryCtrl::getBubbleTargetVel(const int& target_bub_num, const std::vector& band, geometry_msgs::Twist& VelDir) -{ - // init reference for direction vector - VelDir.linear.x = 0.0; - VelDir.linear.y = 0.0; - VelDir.linear.z = 0.0; - VelDir.angular.x = 0.0; - VelDir.angular.y = 0.0; - VelDir.angular.z = 0.0; - // if we are looking at the last bubble - target vel is always zero - if(target_bub_num >= ((int) band.size() - 1)) - return 0.0; + const geometry_msgs::Point& goal = (--elastic_band_.end())->center.pose.position; + const double dx = elastic_band_.at(0).center.pose.position.x - goal.x; + const double dy = elastic_band_.at(0).center.pose.position.y - goal.y; + const double dist_to_goal = sqrt(dx*dx + dy*dy); + + // Assuming we're far enough from the final goal, make sure to rotate so + // we're facing the right way + if (dist_to_goal > rotation_correction_threshold_) + { + + const double angular_diff = angularDiff(control_deviation, elastic_band_.at(0).center.pose); + const double vel = pid_.computeCommand(angular_diff, ros::Duration(1/ctrl_freq_)); + const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_/fabs(vel) : 1.0; + control_deviation.angular.z = vel*mult; + const double abs_vel = fabs(control_deviation.angular.z); + + ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", + "Angular diff is %.2f and desired angular " + "vel is %.2f. Initial translation velocity " + "is %.2f, %.2f", angular_diff, + control_deviation.angular.z, + control_deviation.linear.x, + control_deviation.linear.y); + const double trans_mult = max(0.01, 1.0 - abs_vel/max_vel_th_); // There are some weird tf errors if I let it be 0 + control_deviation.linear.x *= trans_mult; + control_deviation.linear.y *= trans_mult; + ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", + "Translation multiplier is %.2f and scaled " + "translational velocity is %.2f, %.2f", + trans_mult, control_deviation.linear.x, + control_deviation.linear.y); + } + else + ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction", + "Not applying angle correction because " + "distance to goal is %.2f", dist_to_goal); + + + + + // now the actual control procedure start (using attractive Potentials) + geometry_msgs::Twist desired_velocity, currbub_maxvel_dir; + double desvel_abs, desvel_abs_trans, currbub_maxvel_abs; + double scale_des_vel; + desired_velocity = robot_cmd; + currbub_maxvel_dir = robot_cmd; + + // calculate "equilibrium velocity" (Khatib86 - Realtime Obstacle Avoidance) + desired_velocity.linear.x = k_p_/k_nu_ * control_deviation.linear.x; + desired_velocity.linear.y = k_p_/k_nu_ * control_deviation.linear.y; + desired_velocity.angular.z = k_p_/k_nu_ * control_deviation.angular.z; + + //robot_cmd = desired_velocity; + + // get max vel for current bubble + int curr_bub_num = 0; + currbub_maxvel_abs = getBubbleTargetVel(curr_bub_num, elastic_band_, currbub_maxvel_dir); + + // if neccessarry scale desired vel to stay lower than currbub_maxvel_abs + ang_pseudo_dist = desired_velocity.angular.z * getCircumscribedRadius(*costmap_ros_); + desvel_abs = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) + + (desired_velocity.linear.y * desired_velocity.linear.y) + + (ang_pseudo_dist * ang_pseudo_dist) ); + if(desvel_abs > currbub_maxvel_abs) + { + scale_des_vel = currbub_maxvel_abs / desvel_abs; + desired_velocity.linear.x *= scale_des_vel; + desired_velocity.linear.y *= scale_des_vel; + desired_velocity.angular.z *= scale_des_vel; + } + // make sure to stay within velocity bounds for the robot + desvel_abs_trans = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) + (desired_velocity.linear.y * desired_velocity.linear.y) ); + // for translation + if(desvel_abs_trans > max_vel_lin_) + { + scale_des_vel = max_vel_lin_ / desvel_abs_trans; + desired_velocity.linear.x *= scale_des_vel; + desired_velocity.linear.y *= scale_des_vel; + // to make sure we are staying inside the bubble also scale rotation + desired_velocity.angular.z *= scale_des_vel; + } - // otherwise check for max_vel calculated from current bubble size - double v_max_curr_bub, v_max_next_bub; - double bubble_distance, angle_to_pseudo_vel, delta_vel_max; - geometry_msgs::Twist bubble_diff; + // for rotation + if(fabs(desired_velocity.angular.z) > max_vel_th_) + { + scale_des_vel = max_vel_th_ / fabs(desired_velocity.angular.z); + desired_velocity.angular.z *= scale_des_vel; + // to make sure we are staying inside the bubble also scale translation + desired_velocity.linear.x *= scale_des_vel; + desired_velocity.linear.y *= scale_des_vel; + } - // distance for braking s = 0.5*v*v/a - v_max_curr_bub = sqrt(2 * elastic_band_.at(target_bub_num).expansion * acc_max_); + // calculate resulting force (accel. resp.) (Khatib86 - Realtime Obstacle Avoidance) + geometry_msgs::Twist acc_desired; + acc_desired = robot_cmd; + acc_desired.linear.x = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.x - last_vel_.linear.x); + acc_desired.linear.y = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.y - last_vel_.linear.y); + acc_desired.angular.z = (1.0/virt_mass_) * k_nu_ * (desired_velocity.angular.z - last_vel_.angular.z); + + // constrain acceleration + double scale_acc; + double abs_acc_trans = sqrt( (acc_desired.linear.x*acc_desired.linear.x) + (acc_desired.linear.y*acc_desired.linear.y) ); + if(abs_acc_trans > acc_max_trans_) + { + scale_acc = acc_max_trans_ / abs_acc_trans; + acc_desired.linear.x *= scale_acc; + acc_desired.linear.y *= scale_acc; + // again - keep relations - stay in bubble + acc_desired.angular.z *= scale_acc; + } - // get distance to next bubble center - ROS_ASSERT( (target_bub_num >= 0) && ((target_bub_num +1) < (int) band.size()) ); - bubble_diff = getFrame1ToFrame2InRefFrame(band.at(target_bub_num).center.pose, band.at(target_bub_num + 1).center.pose, - ref_frame_band_); - angle_to_pseudo_vel = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); + if(fabs(acc_desired.angular.z) > acc_max_rot_) + { + scale_acc = fabs(acc_desired.angular.z) / acc_max_rot_; + acc_desired.angular.z *= scale_acc; + // again - keep relations - stay in bubble + acc_desired.linear.x *= scale_acc; + acc_desired.linear.y *= scale_acc; + } - bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) + - (angle_to_pseudo_vel * angle_to_pseudo_vel) ); + // and get velocity-cmds by integrating them over one time-step + last_vel_.linear.x = last_vel_.linear.x + acc_desired.linear.x / ctrl_freq_; + last_vel_.linear.y = last_vel_.linear.y + acc_desired.linear.y / ctrl_freq_; + last_vel_.angular.z = last_vel_.angular.z + acc_desired.angular.z / ctrl_freq_; - // calculate direction vector - norm of diference - VelDir.linear.x =bubble_diff.linear.x/bubble_distance; - VelDir.linear.y =bubble_diff.linear.y/bubble_distance; - VelDir.angular.z =bubble_diff.angular.z/bubble_distance; - // if next bubble outside this one we will always be able to break fast enough - if(bubble_distance > band.at(target_bub_num).expansion ) - return v_max_curr_bub; + // we are almost done now take into accoun stick-slip and similar nasty things + // last checks - limit current twist cmd (upper and lower bounds) + last_vel_ = limitTwist(last_vel_); - // next bubble center inside this bubble - take into account restrictions on next bubble - int next_bub_num = target_bub_num + 1; - geometry_msgs::Twist dummy_twist; - v_max_next_bub = getBubbleTargetVel(next_bub_num, band, dummy_twist); // recursive call + // finally set robot_cmd (to non-zero value) + robot_cmd = last_vel_; - // if velocity at next bubble bigger (or equal) than our velocity - we are on the safe side - if(v_max_next_bub >= v_max_curr_bub) - return v_max_curr_bub; + // now convert into robot-body frame + robot_cmd = transformTwistFromFrame1ToFrame2(robot_cmd, ref_frame_band_, elastic_band_.at(0).center.pose); + // check whether we reached the end of the band + int curr_target_bubble = 1; + while(fabs(bubble_diff.linear.x) <= tolerance_trans_ && + fabs(bubble_diff.linear.y) <= tolerance_trans_ && + fabs(bubble_diff.angular.z) <= tolerance_rot_) + { + if(curr_target_bubble < ((int) elastic_band_.size()) - 1) + { + curr_target_bubble++; + // transform next target bubble into robot-body frame + // and get difference to robot bubble + bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, elastic_band_.at(curr_target_bubble).center.pose, + ref_frame_band_); + } + else + { + ROS_DEBUG_THROTTLE_NAMED (1.0, "controller_state", + "Goal reached with distance %.2f, %.2f, %.2f" + "; sending zero velocity", + bubble_diff.linear.x, bubble_diff.linear.y, + bubble_diff.angular.z); + // goal position reached + robot_cmd.linear.x = 0.0; + robot_cmd.linear.y = 0.0; + robot_cmd.angular.z = 0.0; + // reset velocity + last_vel_.linear.x = 0.0; + last_vel_.linear.y = 0.0; + last_vel_.angular.z = 0.0; + goal_reached = true; + break; + } + } - // otherwise max. allowed vel is next vel + plus possible reduction on the way between the bubble-centers - delta_vel_max = sqrt(2 * bubble_distance * acc_max_); - v_max_curr_bub = v_max_next_bub + delta_vel_max; + twist_cmd = robot_cmd; - return v_max_curr_bub; -} + return true; + } -geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame) -{ - - geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D, ref_frame_pose2D; - geometry_msgs::Pose2D frame1_pose2D_rf, frame2_pose2D_rf; - geometry_msgs::Twist frame_diff; - - // transform all frames to Pose2d - PoseToPose2D(frame1, frame1_pose2D); - PoseToPose2D(frame2, frame2_pose2D); - PoseToPose2D(ref_frame, ref_frame_pose2D); - - // transform frame1 into ref frame - frame1_pose2D_rf.x = (frame1_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) + - (frame1_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta); - frame1_pose2D_rf.y = -(frame1_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) + - (frame1_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta); - frame1_pose2D_rf.theta = frame1_pose2D.theta - ref_frame_pose2D.theta; - frame1_pose2D_rf.theta = angles::normalize_angle(frame1_pose2D_rf.theta); - // transform frame2 into ref frame - frame2_pose2D_rf.x = (frame2_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) + - (frame2_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta); - frame2_pose2D_rf.y = -(frame2_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) + - (frame2_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta); - frame2_pose2D_rf.theta = frame2_pose2D.theta - ref_frame_pose2D.theta; - frame2_pose2D_rf.theta = angles::normalize_angle(frame2_pose2D_rf.theta); - - // get differences - frame_diff.linear.x = frame2_pose2D_rf.x - frame1_pose2D_rf.x; - frame_diff.linear.y = frame2_pose2D_rf.y - frame1_pose2D_rf.y; - frame_diff.linear.z = 0.0; - frame_diff.angular.x = 0.0; - frame_diff.angular.y = 0.0; - frame_diff.angular.z = frame2_pose2D_rf.theta - frame1_pose2D_rf.theta; - // normalize angle - frame_diff.angular.z = angles::normalize_angle(frame_diff.angular.z); - - return frame_diff; -} + double EBandTrajectoryCtrl::getBubbleTargetVel(const int& target_bub_num, const std::vector& band, geometry_msgs::Twist& VelDir) + { + // init reference for direction vector + VelDir.linear.x = 0.0; + VelDir.linear.y = 0.0; + VelDir.linear.z = 0.0; + VelDir.angular.x = 0.0; + VelDir.angular.y = 0.0; + VelDir.angular.z = 0.0; -geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame) -{ + // if we are looking at the last bubble - target vel is always zero + if(target_bub_num >= ((int) band.size() - 1)) + return 0.0; - double x1 = frame1.position.x - ref_frame.position.x; - double y1 = frame1.position.y - ref_frame.position.y; - double x2 = frame2.position.x - ref_frame.position.x; - double y2 = frame2.position.y - ref_frame.position.y; - double yaw_ref = tf::getYaw(ref_frame.orientation); - double x_diff = x2 - x1; - double y_diff = y2 - y1; - double theta_diff = atan2(y_diff, x_diff); + // otherwise check for max_vel calculated from current bubble size + double v_max_curr_bub, v_max_next_bub; + double bubble_distance, angle_to_pseudo_vel, delta_vel_max; + geometry_msgs::Twist bubble_diff; - // Now project this vector on to the reference frame - double rotation = angles::normalize_angle(yaw_ref); - double x_final = x_diff * cos(rotation) + y_diff * sin(rotation); - double y_final = - x_diff * sin(rotation) + y_diff * cos(rotation); + // distance for braking s = 0.5*v*v/a + v_max_curr_bub = sqrt(2 * elastic_band_.at(target_bub_num).expansion * acc_max_); - geometry_msgs::Twist twist_msg; - twist_msg.linear.x = x_final; - twist_msg.linear.y = y_final; - twist_msg.angular.z = angles::normalize_angle(theta_diff - yaw_ref); + // get distance to next bubble center + ROS_ASSERT( (target_bub_num >= 0) && ((target_bub_num +1) < (int) band.size()) ); + bubble_diff = getFrame1ToFrame2InRefFrame(band.at(target_bub_num).center.pose, band.at(target_bub_num + 1).center.pose, + ref_frame_band_); + angle_to_pseudo_vel = bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_); - return twist_msg; -} + bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) + + (angle_to_pseudo_vel * angle_to_pseudo_vel) ); + // calculate direction vector - norm of diference + VelDir.linear.x =bubble_diff.linear.x/bubble_distance; + VelDir.linear.y =bubble_diff.linear.y/bubble_distance; + VelDir.angular.z =bubble_diff.angular.z/bubble_distance; -geometry_msgs::Twist EBandTrajectoryCtrl::transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist, - const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2) -{ - geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D; - geometry_msgs::Twist tmp_transformed; - double delta_ang; + // if next bubble outside this one we will always be able to break fast enough + if(bubble_distance > band.at(target_bub_num).expansion ) + return v_max_curr_bub; - tmp_transformed = curr_twist; - // transform all frames to Pose2d - PoseToPose2D(frame1, frame1_pose2D); - PoseToPose2D(frame2, frame2_pose2D); + // next bubble center inside this bubble - take into account restrictions on next bubble + int next_bub_num = target_bub_num + 1; + geometry_msgs::Twist dummy_twist; + v_max_next_bub = getBubbleTargetVel(next_bub_num, band, dummy_twist); // recursive call - // get orientation diff of frames - delta_ang = frame2_pose2D.theta - frame1_pose2D.theta; - delta_ang = angles::normalize_angle(delta_ang); + // if velocity at next bubble bigger (or equal) than our velocity - we are on the safe side + if(v_max_next_bub >= v_max_curr_bub) + return v_max_curr_bub; - // tranform twist - tmp_transformed.linear.x = curr_twist.linear.x * cos(delta_ang) + curr_twist.linear.y * sin(delta_ang); - tmp_transformed.linear.y = -curr_twist.linear.x * sin(delta_ang) + curr_twist.linear.y * cos(delta_ang); - return tmp_transformed; -} + // otherwise max. allowed vel is next vel + plus possible reduction on the way between the bubble-centers + delta_vel_max = sqrt(2 * bubble_distance * acc_max_); + v_max_curr_bub = v_max_next_bub + delta_vel_max; + return v_max_curr_bub; + } -geometry_msgs::Twist EBandTrajectoryCtrl::limitTwist(const geometry_msgs::Twist& twist) -{ - geometry_msgs::Twist res = twist; - - //make sure to bound things by our velocity limits - double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_; - double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y); - if (lin_overshoot > 1.0) - { - res.linear.x /= lin_overshoot; - res.linear.y /= lin_overshoot; - // keep relations - res.angular.z /= lin_overshoot; - } - - //we only want to enforce a minimum velocity if we're not rotating in place - if(lin_undershoot > 1.0) - { - res.linear.x *= lin_undershoot; - res.linear.y *= lin_undershoot; - // we cannot keep relations here for stability reasons - } - - if (fabs(res.angular.z) > max_vel_th_) - { - double scale = max_vel_th_/fabs(res.angular.z); - //res.angular.z = max_vel_th_ * sign(res.angular.z); - res.angular.z *= scale; - // keep relations - res.linear.x *= scale; - res.linear.y *= scale; - } - - if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z); - // we cannot keep relations here for stability reasons - - //we want to check for whether or not we're desired to rotate in place - if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_) - { - if (fabs(res.angular.z) < min_in_place_vel_th_) - res.angular.z = min_in_place_vel_th_ * sign(res.angular.z); - - res.linear.x = 0.0; - res.linear.y = 0.0; - } - - ROS_DEBUG("Angular command %f", res.angular.z); - return res; -} + + geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame) + { + + geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D, ref_frame_pose2D; + geometry_msgs::Pose2D frame1_pose2D_rf, frame2_pose2D_rf; + geometry_msgs::Twist frame_diff; + + // transform all frames to Pose2d + PoseToPose2D(frame1, frame1_pose2D); + PoseToPose2D(frame2, frame2_pose2D); + PoseToPose2D(ref_frame, ref_frame_pose2D); + + // transform frame1 into ref frame + frame1_pose2D_rf.x = (frame1_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) + + (frame1_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta); + frame1_pose2D_rf.y = -(frame1_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) + + (frame1_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta); + frame1_pose2D_rf.theta = frame1_pose2D.theta - ref_frame_pose2D.theta; + frame1_pose2D_rf.theta = angles::normalize_angle(frame1_pose2D_rf.theta); + // transform frame2 into ref frame + frame2_pose2D_rf.x = (frame2_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) + + (frame2_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta); + frame2_pose2D_rf.y = -(frame2_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) + + (frame2_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta); + frame2_pose2D_rf.theta = frame2_pose2D.theta - ref_frame_pose2D.theta; + frame2_pose2D_rf.theta = angles::normalize_angle(frame2_pose2D_rf.theta); + + // get differences + frame_diff.linear.x = frame2_pose2D_rf.x - frame1_pose2D_rf.x; + frame_diff.linear.y = frame2_pose2D_rf.y - frame1_pose2D_rf.y; + frame_diff.linear.z = 0.0; + frame_diff.angular.x = 0.0; + frame_diff.angular.y = 0.0; + frame_diff.angular.z = frame2_pose2D_rf.theta - frame1_pose2D_rf.theta; + // normalize angle + frame_diff.angular.z = angles::normalize_angle(frame_diff.angular.z); + + return frame_diff; + } + + geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame) + { + + double x1 = frame1.position.x - ref_frame.position.x; + double y1 = frame1.position.y - ref_frame.position.y; + double x2 = frame2.position.x - ref_frame.position.x; + double y2 = frame2.position.y - ref_frame.position.y; + double yaw_ref = tf::getYaw(ref_frame.orientation); + + double x_diff = x2 - x1; + double y_diff = y2 - y1; + double theta_diff = atan2(y_diff, x_diff); + + // Now project this vector on to the reference frame + double rotation = angles::normalize_angle(yaw_ref); + double x_final = x_diff * cos(rotation) + y_diff * sin(rotation); + double y_final = - x_diff * sin(rotation) + y_diff * cos(rotation); + + geometry_msgs::Twist twist_msg; + twist_msg.linear.x = x_final; + twist_msg.linear.y = y_final; + twist_msg.angular.z = angles::normalize_angle(theta_diff - yaw_ref); + + return twist_msg; + } + + + geometry_msgs::Twist EBandTrajectoryCtrl::transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist, + const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2) + { + geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D; + geometry_msgs::Twist tmp_transformed; + double delta_ang; + + tmp_transformed = curr_twist; + + // transform all frames to Pose2d + PoseToPose2D(frame1, frame1_pose2D); + PoseToPose2D(frame2, frame2_pose2D); + + // get orientation diff of frames + delta_ang = frame2_pose2D.theta - frame1_pose2D.theta; + delta_ang = angles::normalize_angle(delta_ang); + + // tranform twist + tmp_transformed.linear.x = curr_twist.linear.x * cos(delta_ang) + curr_twist.linear.y * sin(delta_ang); + tmp_transformed.linear.y = -curr_twist.linear.x * sin(delta_ang) + curr_twist.linear.y * cos(delta_ang); + + return tmp_transformed; + } + + + geometry_msgs::Twist EBandTrajectoryCtrl::limitTwist(const geometry_msgs::Twist& twist) + { + geometry_msgs::Twist res = twist; + + //make sure to bound things by our velocity limits + double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_; + double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y); + if (lin_overshoot > 1.0) + { + res.linear.x /= lin_overshoot; + res.linear.y /= lin_overshoot; + // keep relations + res.angular.z /= lin_overshoot; + } + + //we only want to enforce a minimum velocity if we're not rotating in place + if(lin_undershoot > 1.0) + { + res.linear.x *= lin_undershoot; + res.linear.y *= lin_undershoot; + // we cannot keep relations here for stability reasons + } + + if (fabs(res.angular.z) > max_vel_th_) + { + double scale = max_vel_th_/fabs(res.angular.z); + //res.angular.z = max_vel_th_ * sign(res.angular.z); + res.angular.z *= scale; + // keep relations + res.linear.x *= scale; + res.linear.y *= scale; + } + + if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z); + // we cannot keep relations here for stability reasons + + //we want to check for whether or not we're desired to rotate in place + if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_) + { + if (fabs(res.angular.z) < min_in_place_vel_th_) + res.angular.z = min_in_place_vel_th_ * sign(res.angular.z); + + res.linear.x = 0.0; + res.linear.y = 0.0; + } + + ROS_DEBUG("Angular command %f", res.angular.z); + return res; + } } diff --git a/src/eband_visualization.cpp b/src/eband_visualization.cpp index a68a5a6..971180b 100644 --- a/src/eband_visualization.cpp +++ b/src/eband_visualization.cpp @@ -1,39 +1,39 @@ /********************************************************************* -* -* Software License Agreement (BSD License) -* -* Copyright (c) 2010, Willow Garage, Inc. -* 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 Willow Garage, Inc. 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. -* -* Author: Christian Connette -*********************************************************************/ + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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. + * + * Author: Christian Connette + *********************************************************************/ #include @@ -42,347 +42,347 @@ namespace eband_local_planner{ -EBandVisualization::EBandVisualization() : initialized_(false) {} + EBandVisualization::EBandVisualization() : initialized_(false) {} -EBandVisualization::EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros) -{ - initialize(pn, costmap_ros); -} + EBandVisualization::EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros) + { + initialize(pn, costmap_ros); + } -EBandVisualization::~EBandVisualization() {} + EBandVisualization::~EBandVisualization() {} -void EBandVisualization::initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros) -{ - // check if visualization already initialized - if(!initialized_) - { - // read parameters from parameter server - pn.param("marker_lifetime", marker_lifetime_, 0.5); + void EBandVisualization::initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros) + { + // check if visualization already initialized + if(!initialized_) + { + // read parameters from parameter server + pn.param("marker_lifetime", marker_lifetime_, 0.5); - // advertise topics - one_bubble_pub_ = pn.advertise("eband_visualization", 1); - // although we want to publish MarkerArrays we have to advertise Marker topic first -> rviz searchs relative to this - bubble_pub_ = pn.advertise("eband_visualization_array", 1); + // advertise topics + one_bubble_pub_ = pn.advertise("eband_visualization", 1); + // although we want to publish MarkerArrays we have to advertise Marker topic first -> rviz searchs relative to this + bubble_pub_ = pn.advertise("eband_visualization_array", 1); - // copy adress of costmap and Transform Listener (handed over from move_base) -> to get robot shape - costmap_ros_ = costmap_ros; + // copy adress of costmap and Transform Listener (handed over from move_base) -> to get robot shape + costmap_ros_ = costmap_ros; - initialized_ = true; - } - else - { - ROS_WARN("Trying to initialize already initialized visualization, doing nothing."); - } -} + initialized_ = true; + } + else + { + ROS_WARN("Trying to initialize already initialized visualization, doing nothing."); + } + } -void EBandVisualization::publishBand(std::string marker_name_space, std::vector band) -{ - // check if visualization was initialized - if(!initialized_) - { - ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); - return; - } + void EBandVisualization::publishBand(std::string marker_name_space, std::vector band) + { + // check if visualization was initialized + if(!initialized_) + { + ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); + return; + } - visualization_msgs::MarkerArray eband_msg; - eband_msg.markers.resize(band.size()); + visualization_msgs::MarkerArray eband_msg; + eband_msg.markers.resize(band.size()); - visualization_msgs::MarkerArray eband_heading_msg; - eband_heading_msg.markers.resize(band.size()); - std::string marker_heading_name_space = marker_name_space; - marker_heading_name_space.append("_heading"); + visualization_msgs::MarkerArray eband_heading_msg; + eband_heading_msg.markers.resize(band.size()); + std::string marker_heading_name_space = marker_name_space; + marker_heading_name_space.append("_heading"); - // convert elastic band to msg - for(int i = 0; i < ((int) band.size()); i++) - { - // convert bubbles in eband to marker msg - bubbleToMarker(band[i], eband_msg.markers[i], marker_name_space, i, green); + // convert elastic band to msg + for(int i = 0; i < ((int) band.size()); i++) + { + // convert bubbles in eband to marker msg + bubbleToMarker(band[i], eband_msg.markers[i], marker_name_space, i, green); - // convert bubbles in eband to marker msg - // bubbleHeadingToMarker(band[i], eband_heading_msg.markers[i], marker_heading_name_space, i, green); - } + // convert bubbles in eband to marker msg + // bubbleHeadingToMarker(band[i], eband_heading_msg.markers[i], marker_heading_name_space, i, green); + } - // publish - bubble_pub_.publish(eband_msg); - //bubble_pub_.publish(eband_heading_msg); -} + // publish + bubble_pub_.publish(eband_msg); + //bubble_pub_.publish(eband_heading_msg); + } -void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Bubble bubble) -{ - // check if visualization was initialized - if(!initialized_) - { - ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); - return; - } + void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Bubble bubble) + { + // check if visualization was initialized + if(!initialized_) + { + ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); + return; + } - visualization_msgs::Marker bubble_msg; + visualization_msgs::Marker bubble_msg; - // convert bubble to marker msg - bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, green); + // convert bubble to marker msg + bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, green); - // publish - one_bubble_pub_.publish(bubble_msg); -} - - -void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble) -{ - // check if visualization was initialized - if(!initialized_) - { - ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); - return; - } - - visualization_msgs::Marker bubble_msg; - - // convert bubble to marker msg - bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, marker_color); - - // publish - one_bubble_pub_.publish(bubble_msg); -} - - -void EBandVisualization::publishForceList(std::string marker_name_space, std::vector forces, std::vector band) -{ - // check if visualization was initialized - if(!initialized_) - { - ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); - return; - } - - visualization_msgs::MarkerArray forces_msg; - forces_msg.markers.resize(forces.size()); - - //before converting to msg - check whether internal, external or resulting forces - switch color - Color marker_color = green; - if(marker_name_space.compare("internal_forces") == 0) - marker_color = blue; - - if(marker_name_space.compare("external_forces") == 0) - marker_color = red; - - if(marker_name_space.compare("resulting_forces") == 0) - marker_color = green; - - for(int i = 0; i < ((int) forces.size()); i++) - { - // convert wrenches in force list into marker msg - forceToMarker(forces[i], band[i].center.pose, forces_msg.markers[i], marker_name_space, i, marker_color); - } - - // publish - bubble_pub_.publish(forces_msg); -} - -void EBandVisualization::publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble) -{ - // check if visualization was initialized - if(!initialized_) - { - ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); - return; - } - - visualization_msgs::Marker force_msg; - - // convert wrenches in force list into marker msg - forceToMarker(force, bubble.center.pose, force_msg, marker_name_space, id, marker_color); - - // publish - one_bubble_pub_.publish(force_msg); -} - - -void EBandVisualization::bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) -{ - geometry_msgs::Pose2D tmp_pose2d; - - // header - marker.header.stamp = ros::Time::now(); - marker.header.frame_id = bubble.center.header.frame_id; - - // identifier and cmds - marker.ns = marker_name_space; - marker.id = marker_id; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - - // body - marker.pose = bubble.center.pose; - // get theta-angle to display as elevation - PoseToPose2D(bubble.center.pose, tmp_pose2d); - marker.pose.position.z = 0;//tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); - // scale ~ diameter --> is 2x expansion ~ radius - marker.scale.x = 2.0*bubble.expansion; - marker.scale.y = 2.0*bubble.expansion; - marker.scale.z = 2.0*bubble.expansion; - - // color (rgb) - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 0.0f; - switch(marker_color) - { - case red: { marker.color.r = 1.0f; break; } - case green: { marker.color.g = 1.0f; break; } - case blue: { marker.color.b = 1.0f; break; } - } - // transparency (alpha value < 1 : displays marker transparent) - marker.color.a = 0.75; - - // lifetime of this marker - marker.lifetime = ros::Duration(marker_lifetime_); -} - - -void EBandVisualization::bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) -{ - geometry_msgs::Pose2D tmp_pose2d; - - // header - marker.header.stamp = ros::Time::now(); - marker.header.frame_id = bubble.center.header.frame_id; - - // identifier and cmds - marker.ns = marker_name_space; - marker.id = marker_id; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - - // body - marker.pose = bubble.center.pose; - // get theta-angle to display as elevation - PoseToPose2D(bubble.center.pose, tmp_pose2d); - marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); - // scale ~ diameter --> is 2x expansion ~ radius - marker.scale.x = 0.9; - marker.scale.y = 0.45; - marker.scale.z = 0.45; - - // color (rgb) - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 0.0f; - switch(marker_color) - { - case red: { marker.color.r = 1.0f; break; } - case green: { marker.color.g = 1.0f; break; } - case blue: { marker.color.b = 1.0f; break; } - } - // transparency (alpha value < 1 : displays marker transparent) - marker.color.a = 1.0; - - // lifetime of this marker - marker.lifetime = ros::Duration(marker_lifetime_); -} - - -void EBandVisualization::forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) -{ - geometry_msgs::Pose2D tmp_pose2d; - - // header - marker.header.stamp = ros::Time::now(); - marker.header.frame_id = wrench.header.frame_id; - - // identifier and cmds - marker.ns = marker_name_space; - marker.id = marker_id; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - - // body - origin - marker.pose.position = wrench_origin.position; - // get theta-angle to display as elevation - PoseToPose2D(wrench_origin, tmp_pose2d); - marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); - - // body - orientation of vector (calc. quaternion that transform xAxis into force-vector) - // check if force different from zero - if( (wrench.wrench.force.x != 0) || (wrench.wrench.force.y != 0) || (wrench.wrench.torque.z != 0) ) - { - // find AxisAngle Representation then convert to Quaternion - // (Axis = normalize(vec1) cross normalize(vec2) // cos(Angle) = normalize(vec1) dot normalize(vec2)) - Eigen::Vector3d x_axis(1.0, 0.0, 0.0); - Eigen::Vector3d target_vec( wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_) ); - Eigen::Vector3d rotation_axis(1.0, 0.0, 0.0); - double rotation_angle = 0.0; - // get Axis orthogonal to both vectors () - x_axis.normalize(); // unneccessary but just in case - target_vec.normalize(); - if(!(x_axis == target_vec)) - { - // vector not identical - cross-product defined - rotation_axis = x_axis.cross(target_vec); - rotation_angle = x_axis.dot(target_vec); - rotation_angle = acos(rotation_angle); - } - // create AngleAxis representation - rotation_axis.normalize(); // normalize vector -> otherwise AngleAxis will be invalid! - const double s = sin(rotation_angle/2); - const double c = cos(rotation_angle/2); - Eigen::Quaterniond rotate_quat(c, s*rotation_axis.x(), s*rotation_axis.y(), s*rotation_axis.z()); - - // transform quaternion back from Eigen to ROS - tf::Quaternion orientation_tf; - geometry_msgs::Quaternion orientation_msg; - tf::quaternionEigenToTF(rotate_quat, orientation_tf); - tf::quaternionTFToMsg(orientation_tf, orientation_msg); - - // finally set orientation of marker - marker.pose.orientation = orientation_msg; - - // scale ~ diameter --> is 2x expansion ~ radius - double scale = sqrt( (wrench.wrench.force.x * wrench.wrench.force.x) + (wrench.wrench.force.y * wrench.wrench.force.y) + - ( (wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_))*(wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_)) ) ); - marker.scale.x = scale; //1.0; - marker.scale.y = scale; //1.0; - marker.scale.z = scale; //1.0; - - // color (rgb) - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 0.0f; - switch(marker_color) - { - case red: { marker.color.r = 1.0f; break; } - case green: { marker.color.g = 1.0f; break; } - case blue: { marker.color.b = 1.0f; break; } - } - // transparency (alpha value < 1 : displays marker transparent) - marker.color.a = 1.25; - } - else - { - // force on this bubble is zero -> make marker invisible - marker.pose.orientation = wrench_origin.orientation; // just take orientation of bubble as dummy-value - - // scale - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 0.0; - - // color (rgb) - marker.color.r = 0.0f; - marker.color.g = 0.0f; - marker.color.b = 0.0f; - - // transparency (alpha value < 1 : displays marker transparent) - marker.color.a = 0.0; - } - - // lifetime of this marker - marker.lifetime = ros::Duration(marker_lifetime_); -} + // publish + one_bubble_pub_.publish(bubble_msg); + } + + + void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble) + { + // check if visualization was initialized + if(!initialized_) + { + ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); + return; + } + + visualization_msgs::Marker bubble_msg; + + // convert bubble to marker msg + bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, marker_color); + + // publish + one_bubble_pub_.publish(bubble_msg); + } + + + void EBandVisualization::publishForceList(std::string marker_name_space, std::vector forces, std::vector band) + { + // check if visualization was initialized + if(!initialized_) + { + ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); + return; + } + + visualization_msgs::MarkerArray forces_msg; + forces_msg.markers.resize(forces.size()); + + //before converting to msg - check whether internal, external or resulting forces - switch color + Color marker_color = green; + if(marker_name_space.compare("internal_forces") == 0) + marker_color = blue; + + if(marker_name_space.compare("external_forces") == 0) + marker_color = red; + + if(marker_name_space.compare("resulting_forces") == 0) + marker_color = green; + + for(int i = 0; i < ((int) forces.size()); i++) + { + // convert wrenches in force list into marker msg + forceToMarker(forces[i], band[i].center.pose, forces_msg.markers[i], marker_name_space, i, marker_color); + } + + // publish + bubble_pub_.publish(forces_msg); + } + + void EBandVisualization::publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble) + { + // check if visualization was initialized + if(!initialized_) + { + ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization"); + return; + } + + visualization_msgs::Marker force_msg; + + // convert wrenches in force list into marker msg + forceToMarker(force, bubble.center.pose, force_msg, marker_name_space, id, marker_color); + + // publish + one_bubble_pub_.publish(force_msg); + } + + + void EBandVisualization::bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) + { + geometry_msgs::Pose2D tmp_pose2d; + + // header + marker.header.stamp = ros::Time::now(); + marker.header.frame_id = bubble.center.header.frame_id; + + // identifier and cmds + marker.ns = marker_name_space; + marker.id = marker_id; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + + // body + marker.pose = bubble.center.pose; + // get theta-angle to display as elevation + PoseToPose2D(bubble.center.pose, tmp_pose2d); + marker.pose.position.z = 0;//tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); + // scale ~ diameter --> is 2x expansion ~ radius + marker.scale.x = 2.0*bubble.expansion; + marker.scale.y = 2.0*bubble.expansion; + marker.scale.z = 2.0*bubble.expansion; + + // color (rgb) + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + switch(marker_color) + { + case red: { marker.color.r = 1.0f; break; } + case green: { marker.color.g = 1.0f; break; } + case blue: { marker.color.b = 1.0f; break; } + } + // transparency (alpha value < 1 : displays marker transparent) + marker.color.a = 0.75; + + // lifetime of this marker + marker.lifetime = ros::Duration(marker_lifetime_); + } + + + void EBandVisualization::bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) + { + geometry_msgs::Pose2D tmp_pose2d; + + // header + marker.header.stamp = ros::Time::now(); + marker.header.frame_id = bubble.center.header.frame_id; + + // identifier and cmds + marker.ns = marker_name_space; + marker.id = marker_id; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + // body + marker.pose = bubble.center.pose; + // get theta-angle to display as elevation + PoseToPose2D(bubble.center.pose, tmp_pose2d); + marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); + // scale ~ diameter --> is 2x expansion ~ radius + marker.scale.x = 0.9; + marker.scale.y = 0.45; + marker.scale.z = 0.45; + + // color (rgb) + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + switch(marker_color) + { + case red: { marker.color.r = 1.0f; break; } + case green: { marker.color.g = 1.0f; break; } + case blue: { marker.color.b = 1.0f; break; } + } + // transparency (alpha value < 1 : displays marker transparent) + marker.color.a = 1.0; + + // lifetime of this marker + marker.lifetime = ros::Duration(marker_lifetime_); + } + + + void EBandVisualization::forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color) + { + geometry_msgs::Pose2D tmp_pose2d; + + // header + marker.header.stamp = ros::Time::now(); + marker.header.frame_id = wrench.header.frame_id; + + // identifier and cmds + marker.ns = marker_name_space; + marker.id = marker_id; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + + // body - origin + marker.pose.position = wrench_origin.position; + // get theta-angle to display as elevation + PoseToPose2D(wrench_origin, tmp_pose2d); + marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_); + + // body - orientation of vector (calc. quaternion that transform xAxis into force-vector) + // check if force different from zero + if( (wrench.wrench.force.x != 0) || (wrench.wrench.force.y != 0) || (wrench.wrench.torque.z != 0) ) + { + // find AxisAngle Representation then convert to Quaternion + // (Axis = normalize(vec1) cross normalize(vec2) // cos(Angle) = normalize(vec1) dot normalize(vec2)) + Eigen::Vector3d x_axis(1.0, 0.0, 0.0); + Eigen::Vector3d target_vec( wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_) ); + Eigen::Vector3d rotation_axis(1.0, 0.0, 0.0); + double rotation_angle = 0.0; + // get Axis orthogonal to both vectors () + x_axis.normalize(); // unneccessary but just in case + target_vec.normalize(); + if(!(x_axis == target_vec)) + { + // vector not identical - cross-product defined + rotation_axis = x_axis.cross(target_vec); + rotation_angle = x_axis.dot(target_vec); + rotation_angle = acos(rotation_angle); + } + // create AngleAxis representation + rotation_axis.normalize(); // normalize vector -> otherwise AngleAxis will be invalid! + const double s = sin(rotation_angle/2); + const double c = cos(rotation_angle/2); + Eigen::Quaterniond rotate_quat(c, s*rotation_axis.x(), s*rotation_axis.y(), s*rotation_axis.z()); + + // transform quaternion back from Eigen to ROS + tf::Quaternion orientation_tf; + geometry_msgs::Quaternion orientation_msg; + tf::quaternionEigenToTF(rotate_quat, orientation_tf); + tf::quaternionTFToMsg(orientation_tf, orientation_msg); + + // finally set orientation of marker + marker.pose.orientation = orientation_msg; + + // scale ~ diameter --> is 2x expansion ~ radius + double scale = sqrt( (wrench.wrench.force.x * wrench.wrench.force.x) + (wrench.wrench.force.y * wrench.wrench.force.y) + + ( (wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_))*(wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_)) ) ); + marker.scale.x = scale; //1.0; + marker.scale.y = scale; //1.0; + marker.scale.z = scale; //1.0; + + // color (rgb) + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + switch(marker_color) + { + case red: { marker.color.r = 1.0f; break; } + case green: { marker.color.g = 1.0f; break; } + case blue: { marker.color.b = 1.0f; break; } + } + // transparency (alpha value < 1 : displays marker transparent) + marker.color.a = 1.25; + } + else + { + // force on this bubble is zero -> make marker invisible + marker.pose.orientation = wrench_origin.orientation; // just take orientation of bubble as dummy-value + + // scale + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 0.0; + + // color (rgb) + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + + // transparency (alpha value < 1 : displays marker transparent) + marker.color.a = 0.0; + } + + // lifetime of this marker + marker.lifetime = ros::Duration(marker_lifetime_); + } };