00001 #include <ros/ros.h> 00002 #include <actionlib/server/simple_action_server.h> 00003 #include <move_base_msgs/MoveBaseAction.h> 00004 #include <geometry_msgs/Twist.h> 00005 #include <geometry_msgs/PoseStamped.h> 00006 #include <tf/transform_listener.h> 00007 #include <tf/message_filter.h> 00008 #include <message_filters/subscriber.h> 00009 #include <laser_geometry/laser_geometry.h> 00010 #include <sensor_msgs/LaserScan.h> 00011 #include <sensor_msgs/PointCloud.h> 00012 #include <geometry_msgs/Point32.h> 00013 #include <stdio.h> 00014 #include <stdlib.h> 00015 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer; 00016 00017 class MoveOmniBase { 00018 public: 00019 ros::NodeHandle nh; 00020 MoveOmniBase(std::string name, tf::TransformListener& tf); 00021 void BaseLaserCB(const sensor_msgs::LaserScan::ConstPtr& scan); 00022 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal); 00023 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal); 00024 geometry_msgs::PoseStamped goalToLocalFrame(const geometry_msgs::PoseStamped& goal_pose_msg); 00025 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); 00026 00027 //virtual ~MoveOmniBase(); 00028 00029 tf::TransformListener& tf_; 00030 MoveBaseActionServer* as_; 00031 00032 //ros::ServiceServer align_save_srv_; 00033 //message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub_; 00034 //tf::MessageFilter<sensor_msgs::LaserScan> laser_notifier_; 00035 00036 00037 00038 ros::Publisher vel_pub_; 00039 ros::Publisher action_goal_pub_; 00040 00041 ros::Subscriber goal_sub_; 00042 ros::Subscriber base_scan_sub_; 00043 bool enableLaser; 00044 bool front_collision; 00045 bool left_collision; 00046 bool right_collision; 00047 };