00001 00009 /***************************************************************************** 00010 ** Ifdefs 00011 *****************************************************************************/ 00012 00013 #ifndef AUTO_DOCKING_ROS_HPP_ 00014 #define AUTO_DOCKING_ROS_HPP_ 00015 00016 /***************************************************************************** 00017 ** Includes 00018 *****************************************************************************/ 00019 00020 #include <ros/ros.h> 00021 #include <actionlib/server/simple_action_server.h> 00022 #include <kobuki_msgs/AutoDockingAction.h> 00023 00024 #include <message_filters/subscriber.h> 00025 #include <message_filters/time_synchronizer.h> 00026 #include <message_filters/synchronizer.h> 00027 #include <message_filters/sync_policies/approximate_time.h> 00028 00029 #include <std_msgs/String.h> 00030 #include <nav_msgs/Odometry.h> 00031 #include <kobuki_msgs/SensorState.h> 00032 #include <kobuki_msgs/DockInfraRed.h> 00033 00034 #include <sstream> 00035 #include <vector> 00036 #include <ecl/geometry/legacy_pose2d.hpp> 00037 #include <ecl/linear_algebra.hpp> 00038 #include <kdl/frames.hpp> 00039 #include <kdl_conversions/kdl_msg.h> 00040 00041 #include <kobuki_dock_drive/dock_drive.hpp> 00042 00043 namespace kobuki 00044 { 00045 00046 typedef message_filters::sync_policies::ApproximateTime< 00047 nav_msgs::Odometry, 00048 kobuki_msgs::SensorState, 00049 kobuki_msgs::DockInfraRed 00050 > SyncPolicy; 00051 00052 class AutoDockingROS 00053 { 00054 public: 00055 // AutoDockingROS(); 00056 AutoDockingROS(std::string name); 00057 // AutoDockingROS(ros::NodeHandle& nh); 00058 // AutoDockingROS(ros::NodeHandle& nh, std::string name); 00059 ~AutoDockingROS(); 00060 00061 bool init(ros::NodeHandle& nh); 00062 void spin(); 00063 00064 private: 00065 AutoDockingROS* self; 00066 DockDrive dock_; 00067 00068 std::string name_; 00069 bool shutdown_requested_; 00070 00071 ros::NodeHandle nh_; 00072 actionlib::SimpleActionServer<kobuki_msgs::AutoDockingAction> as_; 00073 00074 kobuki_msgs::AutoDockingGoal goal_; 00075 kobuki_msgs::AutoDockingFeedback feedback_; 00076 kobuki_msgs::AutoDockingResult result_; 00077 00078 ros::Subscriber debug_; 00079 ros::Publisher velocity_commander_, motor_power_enabler_, debug_jabber_; 00080 00081 boost::shared_ptr<message_filters::Subscriber<nav_msgs::Odometry> > odom_sub_; 00082 boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::DockInfraRed> > ir_sub_; 00083 boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::SensorState> > core_sub_; 00084 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_; 00085 00086 void goalCb(); 00087 void preemptCb(); 00088 00089 void syncCb(const nav_msgs::OdometryConstPtr& odom, 00090 const kobuki_msgs::SensorStateConstPtr& core, 00091 const kobuki_msgs::DockInfraRedConstPtr& ir); 00092 void debugCb(const std_msgs::StringConstPtr& msg); 00093 }; 00094 00095 } //namespace kobuki 00096 #endif /* AUTO_DOCKING_ROS_HPP_ */ 00097