auto_docking_ros.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Includes
00011 *****************************************************************************/
00012 
00013 #include "kobuki_auto_docking/auto_docking_ros.hpp"
00014 
00015 namespace kobuki
00016 {
00017 
00018 //AutoDockingROS::AutoDockingROS()
00019 AutoDockingROS::AutoDockingROS(std::string name)
00020 //AutoDockingROS::AutoDockingROS(ros::NodeHandle& nh)
00021 //AutoDockingROS::AutoDockingROS(ros::NodeHandle& nh, std::string name)
00022   : name_(name)
00023   , shutdown_requested_(false)
00024   , as_(nh_, name_+"_action", false)
00025 {
00026   self = this;
00027 
00028   as_.registerGoalCallback(boost::bind(&AutoDockingROS::goalCb, this));
00029   as_.registerPreemptCallback(boost::bind(&AutoDockingROS::preemptCb, this));
00030   as_.start();
00031 }
00032 
00033 AutoDockingROS::~AutoDockingROS()
00034 {
00035   shutdown_requested_ = true;
00036   if (as_.isActive()) {
00037     result_.text = "Aborted: Shutdown requested.";
00038     as_.setAborted( result_, result_.text );
00039   }
00040   dock_.disable();
00041 }
00042 
00043 bool AutoDockingROS::init(ros::NodeHandle& nh)
00044 {
00045   // Configure docking drive
00046   double min_abs_v, min_abs_w;
00047   if (nh.getParam("min_abs_v", min_abs_v) == true)
00048     dock_.setMinAbsV(min_abs_v);
00049 
00050   if (nh.getParam("min_abs_w", min_abs_w) == true)
00051     dock_.setMinAbsW(min_abs_w);
00052 
00053   // Publishers and subscribers
00054   velocity_commander_ = nh.advertise<geometry_msgs::Twist>("velocity", 10);
00055   debug_jabber_ = nh.advertise<std_msgs::String>("debug/feedback", 10);
00056 
00057   debug_ = nh.subscribe("debug/mode_shift", 10, &AutoDockingROS::debugCb, this);
00058 
00059   odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "odom", 10));
00060   core_sub_.reset(new message_filters::Subscriber<kobuki_msgs::SensorState>(nh, "core", 10));
00061   ir_sub_.reset(new message_filters::Subscriber<kobuki_msgs::DockInfraRed>(nh, "dock_ir", 10));
00062   sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), *odom_sub_, *core_sub_, *ir_sub_));
00063   sync_->registerCallback(boost::bind(&AutoDockingROS::syncCb, this, _1, _2, _3));
00064 
00065   return dock_.init();
00066 }
00067 
00068 void AutoDockingROS::spin()
00069 {
00070   return;
00071 
00072   while(!shutdown_requested_){;}
00073 }
00074 
00075 void AutoDockingROS::goalCb()
00076 {
00077   if (dock_.isEnabled()) {
00078     goal_ = *(as_.acceptNewGoal());
00079     result_.text = "Rejected: dock_drive is already enabled.";
00080     as_.setAborted( result_, result_.text );
00081     ROS_INFO_STREAM("[" << name_ << "] New goal received but rejected.");
00082   } else {
00083     goal_ = *(as_.acceptNewGoal());
00084     dock_.enable();
00085     ROS_INFO_STREAM("[" << name_ << "] New goal received and accepted.");
00086   }
00087 }
00088 
00089 void AutoDockingROS::preemptCb()
00090 {
00091   //ROS_DEBUG_STREAM("[" << name_ << "] Preempt requested.");
00092   dock_.disable();
00093   if (as_.isNewGoalAvailable()) {
00094     result_.text = "Preempted: New goal received.";
00095     as_.setPreempted( result_, result_.text );
00096     ROS_INFO_STREAM("[" << name_ << "] " << result_.text );
00097   } else {
00098     result_.text = "Cancelled: Cancel requested.";
00099     as_.setPreempted( result_, result_.text );
00100     ROS_INFO_STREAM("[" << name_ << "] " << result_.text );
00101     dock_.disable();
00102   }
00103 }
00104 
00105 void AutoDockingROS::syncCb(const nav_msgs::OdometryConstPtr& odom,
00106                             const kobuki_msgs::SensorStateConstPtr& core,
00107                             const kobuki_msgs::DockInfraRedConstPtr& ir)
00108 {
00109   //process and run
00110   if (self->dock_.isEnabled()) {
00111     //conversions
00112     KDL::Rotation rot;
00113     tf::quaternionMsgToKDL( odom->pose.pose.orientation, rot );
00114 
00115     double r, p, y;
00116     rot.GetRPY(r, p, y);
00117 
00118     ecl::Pose2D<double> pose;
00119     pose.x(odom->pose.pose.position.x);
00120     pose.y(odom->pose.pose.position.y);
00121     pose.heading(y);
00122 
00123     //update
00124     self->dock_.update(ir->data, core->bumper, core->charger, pose);
00125 
00126     //publish debug stream
00127     std_msgs::StringPtr debug_log(new std_msgs::String);
00128     debug_log->data = self->dock_.getDebugStream();
00129     debug_jabber_.publish(debug_log);
00130 
00131     //publish command velocity
00132     if (self->dock_.canRun()) {
00133       geometry_msgs::TwistPtr cmd_vel(new geometry_msgs::Twist);
00134       cmd_vel->linear.x = self->dock_.getVX();
00135       cmd_vel->angular.z = self->dock_.getWZ();
00136       velocity_commander_.publish(cmd_vel);
00137     }
00138   }
00139 
00140   //action server execution
00141   if( as_.isActive() ) {
00142     if ( dock_.getState() == dock_.DONE ) {
00143       result_.text = "Arrived on docking station successfully.";
00144       as_.setSucceeded(result_);
00145       ROS_INFO_STREAM( "[" << name_ << "]: Arrived on docking station successfully.");
00146       ROS_DEBUG_STREAM( "[" << name_ << "]: Result sent.");
00147       dock_.disable();
00148     } else if ( !dock_.isEnabled() ) { //Action Server is activated, but DockDrive is not enabled, or disabled unexpectedly
00149       ROS_ERROR_STREAM("[" << name_ << "] Unintended Case: ActionService is active, but DockDrive is not enabled..");
00150       result_.text = "Aborted: dock_drive is disabled unexpectedly.";
00151       as_.setAborted( result_, "Aborted: dock_drive is disabled unexpectedly." );
00152       ROS_INFO_STREAM("[" << name_ << "] Goal aborted.");
00153       dock_.disable();
00154     } else {
00155       feedback_.state = dock_.getStateStr();
00156       feedback_.text = dock_.getDebugStr();
00157       as_.publishFeedback(feedback_);
00158       ROS_DEBUG_STREAM( "[" << name_ << "]: Feedback sent.");
00159     }
00160   }
00161   return;
00162 }
00163 
00164 void AutoDockingROS::debugCb(const std_msgs::StringConstPtr& msg)
00165 {
00166   dock_.modeShift(msg->data);
00167 }
00168 
00169 
00170 } //namespace kobuki


kobuki_auto_docking
Author(s): Younghun Ju
autogenerated on Wed Sep 16 2015 04:35:11