Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_
00025 #define FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_
00026
00027 #include <actionlib/client/simple_action_client.h>
00028 #include <footstep_planner/FootstepPlanner.h>
00029 #include <footstep_planner/State.h>
00030 #include <geometry_msgs/Pose.h>
00031 #include <geometry_msgs/PoseStamped.h>
00032 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00033 #include <humanoid_nav_msgs/ClipFootstep.h>
00034 #include <humanoid_nav_msgs/ExecFootstepsAction.h>
00035 #include <humanoid_nav_msgs/ExecFootstepsFeedback.h>
00036 #include <humanoid_nav_msgs/PlanFootsteps.h>
00037 #include <humanoid_nav_msgs/StepTargetService.h>
00038 #include <nav_msgs/Path.h>
00039 #include <nav_msgs/OccupancyGrid.h>
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <tf/transform_listener.h>
00043
00044 #include <assert.h>
00045
00046
00047 namespace footstep_planner
00048 {
00053 class FootstepNavigation
00054 {
00055 public:
00056 FootstepNavigation();
00057 virtual ~FootstepNavigation();
00058
00060 bool setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose);
00061
00063 bool setGoal(float x, float y, float theta);
00064
00070 void goalPoseCallback(
00071 const geometry_msgs::PoseStampedConstPtr& goal_pose);
00072
00078 void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);
00079
00080 protected:
00087 bool plan();
00088
00097 bool replan();
00098
00100 void startExecution();
00101
00107 bool getFootTransform(const std::string& foot_id,
00108 const std::string& world_frame_id,
00109 const ros::Time& time,
00110 const ros::Duration& waiting_time,
00111 tf::Transform* foot);
00112
00119 bool getFootstep(const tf::Pose& from, const State& from_planned,
00120 const State& to, humanoid_nav_msgs::StepTarget* footstep);
00121
00135 bool getFootstepsFromPath(
00136 const State& current_support_leg, int starting_step_num,
00137 std::vector<humanoid_nav_msgs::StepTarget>& footsteps);
00138
00140 bool updateStart();
00141
00143 void executeFootsteps();
00144
00149 void executeFootstepsFast();
00150
00155 void activeCallback();
00156
00161 void doneCallback(
00162 const actionlib::SimpleClientGoalState& state,
00163 const humanoid_nav_msgs::ExecFootstepsResultConstPtr& result);
00164
00169 void feedbackCallback(
00170 const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr& fb);
00171
00172 bool performable(const humanoid_nav_msgs::StepTarget& footstep);
00173 bool performable(float step_x, float step_y);
00174
00183 bool performanceValid(const humanoid_nav_msgs::ClipFootstep& footstep);
00184
00186 bool performanceValid(const State& planned, const State& executed);
00187
00189 bool performanceValid(float a_x, float a_y, float a_theta,
00190 float b_x, float b_y, float b_theta);
00191
00192 FootstepPlanner ivPlanner;
00193
00194 ros::Subscriber ivGridMapSub;
00195 ros::Subscriber ivRobotPoseSub;
00196 ros::Subscriber ivGoalPoseSub;
00197
00198 ros::ServiceClient ivFootstepSrv;
00199 ros::ServiceClient ivClipFootstepSrv;
00200
00201 tf::TransformListener ivTransformListener;
00202
00203 boost::mutex ivExecutionLock;
00204
00205 boost::shared_ptr<boost::thread> ivFootstepExecutionPtr;
00206
00207 std::string ivIdFootRight;
00208 std::string ivIdFootLeft;
00209 std::string ivIdMapFrame;
00210
00211 double ivAccuracyX;
00212 double ivAccuracyY;
00213 double ivAccuracyTheta;
00214 double ivCellSize;
00215 int ivNumAngleBins;
00216
00218 bool ivForwardSearch;
00219
00221 bool ivExecutingFootsteps;
00222
00224 double ivFeedbackFrequency;
00225
00227 actionlib::SimpleActionClient<
00228 humanoid_nav_msgs::ExecFootstepsAction> ivFootstepsExecution;
00229
00231 const int ivExecutionShift;
00232
00237 int ivControlStepIdx;
00238
00243 int ivResetStepIdx;
00244
00246 bool ivSafeExecution;
00247
00248 double ivMaxStepX;
00249 double ivMaxStepY;
00250 double ivMaxStepTheta;
00251 double ivMaxInvStepX;
00252 double ivMaxInvStepY;
00253 double ivMaxInvStepTheta;
00254
00255 std::vector<std::pair<double, double> > ivStepRange;
00256 };
00257 }
00258 #endif // FOOTSTEP_PLANNER_FOOTSTEPNAVIGATION_H_