[jsk_footstep_planner] add comments to FootstepPlanner::planCB() in footstep_planner.cpp

jsk_footstep_planner/euslisp/hoge.l

@@ -0,0 +1,27 @@
+#!/usr/bin/env roseus
+
+(ros::load-ros-manifest "jsk_footstep_planner")
+(load "package://jsk_footstep_controller/euslisp/util.l")
+
+(warn "
+## launch simple node (not using pointcloud)
+roslaunch jsk_footstep_planner optimistic_footstep_planner.launch USE_CONTROLLER:=false USE_MARKER:=false USE_PERCEPTION:=false ROBOT:=JAXON_RED
+
+")
+(initialize-eus-footstep-planning-client)
+(while t
+    (setq result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0))))
+    (let ((footstep-coords (footstep-array->coords result)))
+      (print-readable-coords footstep-coords)
+      ;; (send *ri* :set-foot-steps footstep-coords) ;; send real-robot if needed
+      )
+    (ros::spin-once)
+    )
+
+(warn "
+(setq result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0))))
+(let ((footstep-coords (footstep-array->coords result)))
+  (print-readable-coords footstep-coords)
+  ;; (send *ri* :set-foot-steps footstep-coords) ;; send real-robot if needed
+  )
+")

jsk_footstep_planner/include/jsk_footstep_planner/footstep_astar_solver.h

@@ -42,12 +42,23 @@
 
 namespace jsk_footstep_planner
 {
-  // Only available for FootstepState and FootstepGraph
-  // because close list behavior is specialized for the purpose
+  //! The FootsteAStarSolver class
+  /*!
+    A start solver class for FootstepState and FootstepGraph
+
+    Caution:
+      This class only works with FootstepState and FootstepGraph
+      because close list behavior is specialized for the purpose
+  */
   template <class GraphT>
   class FootstepAStarSolver: public AStarSolver<GraphT>
   {
   public:
+
+    //! typedefs
+    /*!
+     Type definitions used in this class.
+     */
     typedef boost::shared_ptr<FootstepAStarSolver> Ptr;
     typedef typename GraphT::StateT State;
     typedef typename GraphT::StateT::Ptr StatePtr;
@@ -57,6 +68,10 @@ namespace jsk_footstep_planner
     typedef typename std::priority_queue<SolverNodePtr,
                                          std::vector<SolverNodePtr>,
                                          std::greater<SolverNodePtr> > OpenList;    
+
+    //! Constructor
+    /*!
+     */
     FootstepAStarSolver(
       GraphPtr graph, size_t x_num, size_t y_num, size_t theta_num,
       unsigned int profile_period = 1024,

jsk_footstep_planner/include/jsk_footstep_planner/footstep_planner.h

@@ -152,8 +152,8 @@ namespace jsk_footstep_planner
     std::vector<Eigen::Affine3f> successors_;
     Eigen::Vector3f collision_bbox_size_;
     Eigen::Affine3f collision_bbox_offset_;
-    Eigen::Vector3f inv_lleg_footstep_offset_;
-    Eigen::Vector3f inv_rleg_footstep_offset_;
+    Eigen::Vector3f inv_lleg_footstep_offset_; // default left footstep offset from center position
+    Eigen::Vector3f inv_rleg_footstep_offset_; // default right footstep offset from center position
     std_msgs::Header latest_header_;
     // Common Parameters
     FootstepParameters parameters_;
@@ -169,8 +169,8 @@ namespace jsk_footstep_planner
     double resolution_x_;
     double resolution_y_;
     double resolution_theta_;
-    double footstep_size_x_;
-    double footstep_size_y_;
+    double footstep_size_x_; // default footstep size x
+    double footstep_size_y_; // default footstep size y
     int close_list_x_num_;
     int close_list_y_num_;
     int close_list_theta_num_;

jsk_footstep_planner/include/jsk_footstep_planner/footstep_state.h

@@ -87,7 +87,7 @@ namespace jsk_footstep_planner
                   const Eigen::Vector3f& resolution):
       leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution)
     {
-      debug_print_ = false;
+      debug_print_ = true;
       float x = pose_.translation()[0];
       float y = pose_.translation()[1];
       float roll, pitch, yaw;
@@ -107,7 +107,7 @@ namespace jsk_footstep_planner
       leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution),
       index_x_(index_x), index_y_(index_y), index_yaw_(index_yaw)
     {
-      debug_print_ = false;
+      debug_print_ = true;
     }
 
     static
@@ -220,6 +220,26 @@ namespace jsk_footstep_planner
     inline virtual int indexT() { return index_yaw_; }
 
 
+    /**
+     * Check if footstep can be supported with pointcloud by 2 steps
+     *
+     *  - 
+     *
+     * @param [in] pose:
+     *      pose of footstep ?
+     * @param [in] cloud:
+     *      pointcloud of env
+     * @param [in] tree:
+     *      kdtree object used when finding nearest points from given point cloud
+     * @param [in] foot_x_samppling_num:
+     *      number of divided section in x direction when dividing footstep in order to find supporting pointcloud in each section.
+     * @param [in] foot_y_samppling_num:
+     *      number of divided section in y direction when dividing footstep in order to find supporting pointcloud in each section.
+     * @param [in] vertex_threshold:
+     *      平面からどれくらいz方向にずれていても許容するかのスレッショルド
+     *      およびに、キーポイントの近くに点があるかを探す探索範囲
+     *      
+     */
     virtual FootstepSupportState
     isSupportedByPointCloud(const Eigen::Affine3f& pose,
                             pcl::PointCloud<pcl::PointNormal>::Ptr cloud,

jsk_footstep_planner/src/footstep_planner.cpp

@@ -407,13 +407,33 @@ namespace jsk_footstep_planner
     pub.publish(msg);
   }
 
+  /*
+   * Callback function of actionlib server to plan footsteps to a given goal
+   */
   void FootstepPlanner::planCB(
     const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
   {
     boost::mutex::scoped_lock lock(mutex_);
+
+    // Store header of latest action goal footstep
     latest_header_ = goal->goal_footstep.header;
     ROS_INFO("planCB");
-    // check message sanity
+
+
+    /*
+     * Verifying whether a requested message meets requirements below or not
+     *   - FootstepArray in the "initial_footstep" record of a goal is set.
+     *   - The size of FootstepArray in the "initial_footstep" record of a goal is 2.
+     *   - The frame_id of the "initial_footstep" record of a goal is equal to
+     *         the frame_id of pointcloud
+     *   - The frame_id of the "initial_footstep" record of a goal is equal to
+     *         the frame_id of obstacle pointcloudif obstacle_mode is used.
+     *   - The size of FootstepArray in the "goal_footstep" record of a goal is 2.
+     *
+     * And verifying if settings below is correct.
+     *   - pointcloud options are correctly set.
+     */
+    // check messgae sanity
     if (goal->initial_footstep.footsteps.size() == 0) {
       ROS_ERROR("no initial footstep is specified");
       as_.setPreempted();
@@ -449,15 +469,23 @@ namespace jsk_footstep_planner
         return;
       }
     }
-    Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
-    Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
-    // check goal is whether collision free
-    // conevrt goal footstep into FootstepState::Ptr instance.
     if (goal->goal_footstep.footsteps.size() != 2) {
       ROS_ERROR("goal footstep should be a pair of footsteps");
       as_.setPreempted();
       return;
     }
+
+
+    /*
+     * Initialize footstep_size and search_resolution
+     */
+    Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
+    Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
+
+
+    /*
+     * Convert goal footstep (Footstep message instance) into FootstepState::Ptr instance.
+     */
     std::vector<jsk_footstep_msgs::Footstep > goal_ros;
     goal_ros.push_back(goal->goal_footstep.footsteps[0]);
     goal_ros.push_back(goal->goal_footstep.footsteps[1]);
@@ -483,18 +511,26 @@ namespace jsk_footstep_planner
         goal_ros[i].dimensions.z = 0.000001;
       }
     }
-
     FootstepState::Ptr first_goal = FootstepState::fromROSMsg(goal_ros[0],
                                                               footstep_size,
                                                               search_resolution);
     FootstepState::Ptr second_goal = FootstepState::fromROSMsg(goal_ros[1],
                                                                footstep_size,
                                                                search_resolution);
+
+    /*
+     *  Check whether a given goal is reachable or not
+     */
+    // check goal is whether collision free
     if (!graph_->isSuccessable(second_goal, first_goal)) {
       ROS_ERROR("goal is non-realistic");
       as_.setPreempted();
       return;
     }
+
+    /*
+     * Set Timeout
+     */
     ros::WallDuration timeout;
     if(goal->timeout.toSec() == 0.0) {
       timeout = ros::WallDuration(planning_timeout_);
@@ -503,6 +539,9 @@ namespace jsk_footstep_planner
     }
 
 
+    /*
+     * Initialize search tree with start footstep state and goal footstep state
+     */
     ////////////////////////////////////////////////////////////////////
     // set start state
     // 0 is always start
@@ -536,7 +575,6 @@ namespace jsk_footstep_planner
         return;
       }
     }
-
     ////////////////////////////////////////////////////////////////////
     // set goal state
     jsk_footstep_msgs::Footstep left_goal, right_goal;
@@ -571,6 +609,7 @@ namespace jsk_footstep_planner
         return;
       }
     }
+    ////////////////////////////////////////////////////////////////////
     // set parameters
     if (parameters_.use_transition_limit) {
       graph_->setTransitionLimit(
@@ -594,17 +633,19 @@ namespace jsk_footstep_planner
         TransitionLimitRP::Ptr(new TransitionLimitRP(
                                      parameters_.global_transition_limit_roll,
                                      parameters_.global_transition_limit_pitch)));
-
     }
     else {
       graph_->setGlobalTransitionLimit(TransitionLimitRP::Ptr());
     }
     graph_->setParameters(parameters_);
-
     graph_->setSuccessorFunction(boost::bind(&FootstepGraph::successors_original, graph_, _1, _2));
     graph_->setPathCostFunction(boost::bind(&FootstepGraph::path_cost_original, graph_, _1, _2, _3));
-
     //ROS_INFO_STREAM(graph_->infoString());
+
+
+    /*
+     * Setting up a solver for footstep graph
+     */
     // Solver setup
     FootstepAStarSolver<FootstepGraph> solver(graph_,
                                               close_list_x_num_,
@@ -635,13 +676,24 @@ namespace jsk_footstep_planner
     }
     graph_->clearPerceptionDuration();
     solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
+
+
+    /*
+     * Plan footsteps using the solver
+     */
     ROS_INFO("start_solver timeout: %f", timeout.toSec());
     ros::WallTime start_time = ros::WallTime::now();
     std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
     ros::WallTime end_time = ros::WallTime::now();
     double planning_duration = (end_time - start_time).toSec();
     //ROS_INFO_STREAM("took " << planning_duration << " sec");
     //ROS_INFO_STREAM("path: " << path.size());
+
+
+    /*
+     * Finalize footstep path
+     */
+    // when solver failed
     if (path.size() == 0) {
       pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
       solver.openListToPointCloud(open_list_cloud);
@@ -666,7 +718,8 @@ namespace jsk_footstep_planner
       as_.setPreempted();
       return;
     }
-    // finalize in graph
+    // when solver succeeded
+    // finalize in graph (add exact final footsteps to result footstep
     std::vector <FootstepState::Ptr> finalizeSteps;
     if (! (graph_->finalizeSteps((path.size() >1 ? path[path.size()-2]->getState() : FootstepState::Ptr()),
                                  path[path.size()-1]->getState(),
@@ -697,9 +750,14 @@ namespace jsk_footstep_planner
         ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
       }
     }
+    // return result of action server
     result_.result = ros_path;
     as_.setSucceeded(result_);
 
+
+    /*
+     *  Publish the result of planning
+     */
     pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
     solver.openListToPointCloud(open_list_cloud);
     solver.closeListToPointCloud(close_list_cloud);