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 #include <kobuki_msgs/MotorPower.h> 00034 00035 #include <sstream> 00036 #include <vector> 00037 #include <ecl/geometry/pose2d.hpp> 00038 #include <ecl/linear_algebra.hpp> 00039 #include <kdl/frames.hpp> 00040 #include <kdl_conversions/kdl_msg.h> 00041 00042 #include "dock_drive.hpp" 00043 00044 namespace kobuki 00045 { 00046 00047 typedef message_filters::sync_policies::ApproximateTime< 00048 nav_msgs::Odometry, 00049 kobuki_msgs::SensorState, 00050 kobuki_msgs::DockInfraRed 00051 > SyncPolicy; 00052 00053 class AutoDockingROS 00054 { 00055 public: 00056 // AutoDockingROS(); 00057 AutoDockingROS(std::string name); 00058 // AutoDockingROS(ros::NodeHandle& nh); 00059 // AutoDockingROS(ros::NodeHandle& nh, std::string name); 00060 ~AutoDockingROS(); 00061 00062 bool init(ros::NodeHandle& nh); 00063 void spin(); 00064 00065 private: 00066 AutoDockingROS* self; 00067 DockDrive dock_; 00068 00069 std::string name_; 00070 bool shutdown_requested_; 00071 00072 ros::NodeHandle nh_; 00073 actionlib::SimpleActionServer<kobuki_msgs::AutoDockingAction> as_; 00074 00075 kobuki_msgs::AutoDockingGoal goal_; 00076 kobuki_msgs::AutoDockingFeedback feedback_; 00077 kobuki_msgs::AutoDockingResult result_; 00078 00079 ros::Subscriber debug_; 00080 ros::Publisher velocity_commander_, motor_power_enabler_, debug_jabber_; 00081 00082 boost::shared_ptr<message_filters::Subscriber<nav_msgs::Odometry> > odom_sub_; 00083 boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::DockInfraRed> > ir_sub_; 00084 boost::shared_ptr<message_filters::Subscriber<kobuki_msgs::SensorState> > core_sub_; 00085 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_; 00086 00087 void goalCb(); 00088 void preemptCb(); 00089 00090 void syncCb(const nav_msgs::OdometryConstPtr& odom, 00091 const kobuki_msgs::SensorStateConstPtr& core, 00092 const kobuki_msgs::DockInfraRedConstPtr& ir); 00093 void debugCb(const std_msgs::StringConstPtr& msg); 00094 00095 void toggleMotor(const bool& on_off); 00096 }; 00097 00098 } //namespace kobuki 00099 #endif /* AUTO_DOCKING_ROS_HPP_ */ 00100