helper.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef VIGIR_FOOTSTEP_PLANNING_LIB_HELPER_H__
00030 #define VIGIR_FOOTSTEP_PLANNING_LIB_HELPER_H__
00031 
00032 #include <ros/ros.h>
00033 #include <tf/tf.h>
00034 
00035 #include <boost/function.hpp>
00036 #include <boost/bind.hpp>
00037 
00038 #include <actionlib/client/simple_action_client.h>
00039 #include <actionlib/server/simple_action_server.h>
00040 
00041 #include <nav_msgs/OccupancyGrid.h>
00042 
00043 #include <vigir_generic_params/generic_params_msgs.h>
00044 
00045 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00046 
00047 
00048 
00049 namespace vigir_footstep_planning
00050 {
00051 template <typename ActionSpec>
00052 class SimpleActionClient
00053     : public actionlib::SimpleActionClient<ActionSpec>
00054 {
00055 public:
00056   // typedefs
00057   typedef boost::shared_ptr<SimpleActionClient> Ptr;
00058   typedef boost::shared_ptr<const SimpleActionClient> ConstPtr;
00059 
00060   SimpleActionClient(ros::NodeHandle nh, std::string name, bool spin_thread = true)
00061     : actionlib::SimpleActionClient<ActionSpec>(nh, name, spin_thread)
00062   {
00063   }
00064 
00065   static Ptr create(ros::NodeHandle nh, std::string name, bool spin_thread = true)
00066   {
00067     return Ptr(new SimpleActionClient<ActionSpec>(nh, name, spin_thread));
00068   }
00069 };
00070 
00071 template <typename ActionSpec>
00072 class SimpleActionServer
00073   : public actionlib::SimpleActionServer<ActionSpec>
00074 {
00075 public:
00076   // typedefs
00077   typedef boost::shared_ptr<SimpleActionServer<ActionSpec> > Ptr;
00078   typedef boost::shared_ptr<const SimpleActionServer<ActionSpec> > ConstPtr;
00079   typedef boost::function<void ()> ExecuteCallback;
00080   typedef boost::function<void ()> PreemptCallback;
00081 
00082   SimpleActionServer(ros::NodeHandle nh, std::string name, bool auto_start)
00083     : actionlib::SimpleActionServer<ActionSpec>(nh, name, auto_start)
00084   {
00085   }
00086 
00087   template <typename S> void finish(const S& result)
00088   {
00089     if (hasError(result.status))
00090       this->setAborted(result, toString(result.status));
00091     else
00092       this->setSucceeded(result, toString(result.status));
00093   }
00094 
00095   void preempt()
00096   {
00097     actionlib::SimpleActionServer<ActionSpec>::setPreempted();
00098   }
00099 
00100   static Ptr create(ros::NodeHandle nh, std::string name, bool auto_start, ExecuteCallback execute_cb, PreemptCallback preempt_cb = PreemptCallback())
00101   {
00102     Ptr as(new SimpleActionServer<ActionSpec>(nh, name, false));
00103 
00104     as->registerGoalCallback(execute_cb);
00105 
00106     if (!preempt_cb.empty())
00107       preempt_cb = boost::bind(&SimpleActionServer::preempt, as.get());
00108     as->registerPreemptCallback(preempt_cb);
00109 
00110     if (auto_start)
00111       as->start();
00112 
00113     return as;
00114   }
00115 };
00116 
00117 msgs::ErrorStatus determineStartFeetPose(msgs::Feet& start_feet, ros::ServiceClient& generate_feet_pose_client, const std_msgs::Header& header);
00118 msgs::ErrorStatus determineGoalFeetPose(msgs::Feet& goal_feet, ros::ServiceClient& generate_feet_pose_client, const geometry_msgs::PoseStamped& goal_pose);
00119 
00120 msgs::ErrorStatus transform(msgs::Foot& foot, ros::ServiceClient& transform_foot_pose_client, const std::string& target_frame);
00121 msgs::ErrorStatus transform(msgs::Feet& feet, ros::ServiceClient& transform_feet_poses_client, const std::string& target_frame);
00122 msgs::ErrorStatus transform(msgs::StepPlan& step_plan, ros::ServiceClient& transform_step_plan_client, const std::string& target_frame);
00123 
00124 template <typename T>
00125 msgs::ErrorStatus transformToPlannerFrame(T& p, ros::ServiceClient& foot_pose_transformer_client)
00126 {
00127   return transform(p, foot_pose_transformer_client, "planner");
00128 }
00129 
00130 template <typename T>
00131 msgs::ErrorStatus transformToRobotFrame(T& p, ros::ServiceClient& foot_pose_transformer_client)
00132 {
00133   return transform(p, foot_pose_transformer_client, "robot");
00134 }
00135 
00136 
00137 
00138 // loading of common parameters
00139 bool getXYZ(ros::NodeHandle& nh, const std::string name, geometry_msgs::Vector3& val);
00140 bool getRPY(ros::NodeHandle& nh, const std::string name, geometry_msgs::Vector3& val);
00141 
00142 bool getFootSize(ros::NodeHandle& nh, geometry_msgs::Vector3& foot_size);
00143 bool getUpperBodySize(ros::NodeHandle& nh, geometry_msgs::Vector3& upper_body_size);
00144 bool getUpperBodyOriginShift(ros::NodeHandle& nh, geometry_msgs::Vector3& upper_body_origin_shift);
00145 
00146 
00147 
00148 bool getGridMapCoords(const nav_msgs::OccupancyGrid& map, double x, double y, int& map_x, int& map_y);
00149 bool getGridMapIndex(const nav_msgs::OccupancyGrid& map, double x, double y, int& idx);
00150 
00151 
00152 
00153 inline std::string& strip(std::string& s, const char c) { return vigir_generic_params::strip(s, c); }
00154 inline std::string strip_const(const std::string& s, const char c) { return vigir_generic_params::strip_const(s, c); }
00155 }
00156 
00157 #endif


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56