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 FOOTSTEP_PLANNER_NODE_H__ 00030 #define FOOTSTEP_PLANNER_NODE_H__ 00031 00032 #include <ros/ros.h> 00033 00034 #include <boost/thread/mutex.hpp> 00035 #include <boost/bind.hpp> 00036 00037 #include <geometry_msgs/PoseStamped.h> 00038 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00039 00040 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h> 00041 #include <vigir_footstep_planning_msgs/visualization.h> 00042 00043 #include <vigir_footstep_planner/footstep_planner.h> 00044 00045 00046 00047 namespace vigir_footstep_planning 00048 { 00053 class FootstepPlannerNode 00054 { 00055 public: 00056 FootstepPlannerNode(ros::NodeHandle& nh); 00057 virtual ~FootstepPlannerNode(); 00058 00059 virtual void initPlugins(ros::NodeHandle& nh); 00060 virtual void init(ros::NodeHandle& nh); 00061 00062 protected: 00063 // callbacks 00064 void planningResultCallback(const msgs::StepPlanRequestService::Response& resp); 00065 void planningResultActionCallback(const msgs::StepPlanRequestService::Response& resp, SimpleActionServer<msgs::StepPlanRequestAction>::Ptr& as); 00066 00067 void planningFeedbackCallback(const msgs::PlanningFeedback& feedback); 00068 void planningFeedbackActionCallback(const msgs::PlanningFeedback& feedback, SimpleActionServer<msgs::StepPlanRequestAction>::Ptr& as); 00069 00070 void planningPreemptionActionCallback(SimpleActionServer<msgs::StepPlanRequestAction>::Ptr& as); 00071 00072 // subscriber 00073 void setParams(const std_msgs::StringConstPtr& params_name); 00074 void stepPlanRequest(const msgs::StepPlanRequestConstPtr& plan_request); 00075 void goalPoseCallback(const geometry_msgs::PoseStampedConstPtr& goal_pose); 00076 00077 // service calls 00078 bool stepPlanRequestService(msgs::StepPlanRequestService::Request& req, msgs::StepPlanRequestService::Response& resp); 00079 bool updateFootService(msgs::UpdateFootService::Request& req, msgs::UpdateFootService::Response& resp); 00080 bool updateFeetService(msgs::UpdateFeetService::Request& req, msgs::UpdateFeetService::Response& resp); 00081 bool updateStepPlanService(msgs::UpdateStepPlanService::Request& req, msgs::UpdateStepPlanService::Response& resp); 00082 00083 // action server calls 00084 void stepPlanRequestAction(SimpleActionServer<msgs::StepPlanRequestAction>::Ptr& as); 00085 void stepPlanRequestPreempt(SimpleActionServer<msgs::StepPlanRequestAction>::Ptr& as); 00086 void updateFootAction(SimpleActionServer<msgs::UpdateFootAction>::Ptr& as); 00087 void updateFeetAction(SimpleActionServer<msgs::UpdateFeetAction>::Ptr& as); 00088 void updateStepPlanAction(SimpleActionServer<msgs::UpdateStepPlanAction>::Ptr& as); 00089 00090 // subscribers 00091 ros::Subscriber set_active_parameter_set_sub; 00092 ros::Subscriber step_plan_request_sub; 00093 ros::Subscriber goal_pose_sub; 00094 00095 // publisher 00096 ros::Publisher step_plan_pub; 00097 ros::Publisher step_plan_request_vis_pub; 00098 ros::Publisher step_plan_vis_pub; 00099 ros::Publisher error_status_pub; 00100 ros::Publisher temp_step_plan_pub; 00101 ros::Publisher feedback_pub; 00102 00103 // service clients 00104 ros::ServiceClient generate_feet_pose_client; 00105 00106 // service servers 00107 ros::ServiceServer step_plan_request_srv; 00108 ros::ServiceServer update_foot_srv; 00109 ros::ServiceServer update_feet_srv; 00110 ros::ServiceServer update_step_plan_srv; 00111 00112 // action servers 00113 SimpleActionServer<msgs::StepPlanRequestAction>::Ptr step_plan_request_as; 00114 SimpleActionServer<msgs::UpdateFootAction>::Ptr update_foot_as; 00115 SimpleActionServer<msgs::UpdateFeetAction>::Ptr update_feet_as; 00116 SimpleActionServer<msgs::UpdateStepPlanAction>::Ptr update_step_plan_as; 00117 00118 mutable boost::recursive_mutex step_plan_request_as_mutex; 00119 00120 FootstepPlanner::Ptr footstep_planner; 00121 geometry_msgs::Vector3 foot_size; 00122 }; 00123 } 00124 #endif