bipedRobin_planner_ros.h

Go to the documentation of this file.

#ifndef BIPEDROBIN_LOCAL_PLANNER_BIPEDROBIN_PLANNER_ROS_H_
#define BIPEDROBIN_LOCAL_PLANNER_BIPEDROBIN_PLANNER_ROS_H_
#include <boost/shared_ptr.hpp>
#include <nav_core/base_local_planner.h>
#include <std_msgs/UInt8.h>
#include <bipedRobin_msgs/StepTarget3DService.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <footstep_planner/FootstepPlanner.h>
#include <footstep_planner/State.h>

namespace bipedRobin_local_planner {
  class BipedRobinPlannerROS : public nav_core::BaseLocalPlanner {
                public:
                BipedRobinPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}

                void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);

                bool isGoalReached();

                bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

                bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
                
                void stepsLeftCallback(const std_msgs::UInt8 stepsLeftInBuffer);

        private:
                inline double sign(double x){
                return x < 0.0 ? -1.0 : 1.0;
                }
                
                
                void planningThread(void);
                void updateMapToPlanner();
                bool initializeNavigator();
                void  getFootstepFromState(const footstep_planner::State& from,  const footstep_planner::State& to, bipedRobin_msgs::StepTarget3D* footstep);

                //void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

                void goalPoseCallback(const geometry_msgs::PoseStampedConstPtr& goal_pose);

                void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);

                costmap_2d::Costmap2DROS* costmap_ros_;
                tf::TransformListener* tf_;
                double max_vel_th_, min_vel_th_, min_rot_vel_;
                double rot_stopped_vel_, trans_stopped_vel_;
                double yaw_goal_tolerance_, xy_goal_tolerance_;
                bool prune_plan_;
                bool initialized_;
                int ivSendStep;
                bool ivPlanSteps;
                
                ros::Publisher g_plan_pub_, l_plan_pub_; //for local and global plan to publish
                ros::Publisher ivMapPub; //for local occupancy grid
                ros::ServiceClient ivFootstepIncSrv;
                ros::Subscriber stepsLeftInBufferSub;
                
                footstep_planner::FootstepPlanner ivPlanner;

                ros::ServiceServer ivFootstepPlanService;
                boost::thread* ivFootstepPlanningThread;
                boost::mutex ivFootstepPlanningMutex;
                boost::mutex planner_mutex_;
                boost::mutex localPlan_mutex_;

                std::string ivIdFootRight;
                std::string ivIdFootLeft;
                std::string ivIdMapFrame;
                double ivPlanningDistance;

                std::list <footstep_planner::State> ivStepsToStart;
                footstep_planner::State ivLeftLegStart;
                footstep_planner::State ivRightLegStart;
                footstep_planner::State ivLeftLegCurrent;
                footstep_planner::State ivRightLegCurrent;
                double ivLocalGoalX;
                double ivLocalGoalY;
                double ivLocalGoalTheta;

                std::vector<geometry_msgs::PoseStamped> global_plan_;
        };
};
#endif