topological_move_base_action_layer.cpp
Go to the documentation of this file.
00001 
00026 #include <topological_tools/topological_move_base_action_layer.h>
00027 
00028 #include <geometry_msgs/PoseStamped.h>
00029 
00030 using namespace std;
00031 using namespace topological_tools;
00032 using namespace predicate_manager;
00033 using namespace mdm_library;
00034 
00035 TopologicalMoveBaseActionLayer::
00036 TopologicalMoveBaseActionLayer ( TopologicalMap& tm ) :
00037     actions_cb_queue_(),
00038     predicates_cb_queue_(),
00039     al_ ( &actions_cb_queue_ ),
00040     tam_ ( tm, &predicates_cb_queue_ ),
00041     move_base_client_ ( "move_base", true ) 
00042 {
00043     ros::Duration d ( 10.0 );
00044     d.sleep(); 
00045     while ( ! ( move_base_client_.waitForServer ( d ) ) )
00046     {
00047         ROS_WARN_STREAM ( "TopologicalMoveBaseActionLayer:: Waiting for the move_base server to come up." );
00048     };
00049 }
00050 
00051 TopologicalMoveBaseActionLayer::
00052 TopologicalMoveBaseActionLayer ( const std::string& map_file ) :
00053     actions_cb_queue_(),
00054     predicates_cb_queue_(),
00055     al_ ( &actions_cb_queue_ ),
00056     tam_ ( map_file, &predicates_cb_queue_ ),
00057     move_base_client_ ( "move_base", true ) 
00058 {
00059     ros::Duration d ( 10.0 );
00060     d.sleep(); 
00061     while ( ! ( move_base_client_.waitForServer ( d ) ) )
00062     {
00063         ROS_WARN_STREAM ( "TopologicalMoveBaseActionLayer:: Waiting for the move_base server to come up." );
00064     };
00065 }
00066 
00067 void
00068 TopologicalMoveBaseActionLayer::
00069 addAction ( const std::string& action_name )
00070 {
00071     al_.addAction ( boost::bind ( &TopologicalMoveBaseActionLayer::moveToLabel, this, action_name ),
00072                     action_name );
00073 }
00074 
00075 void
00076 TopologicalMoveBaseActionLayer::
00077 moveToLabel ( const string& connection_label )
00078 {
00079     geometry_msgs::PoseStamped goal_pose;
00080     goal_pose.pose =  tam_.getGoalPoseForLabel ( connection_label );
00081     goal_pose.header.stamp = ros::Time::now();
00082     goal_pose.header.frame_id = "/map";
00083 
00084     move_base_msgs::MoveBaseGoal traverse_goal;
00085     traverse_goal.target_pose = goal_pose;
00086 
00087     move_base_client_.cancelAllGoals();
00088     move_base_client_.sendGoal ( traverse_goal,
00089                                  boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseDoneCB, this, _1, _2 ),
00090                                  boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseActiveCB, this ),
00091                                  boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseFeedbackCB, this, _1 ) );
00093 }
00094 
00095 void
00096 TopologicalMoveBaseActionLayer::
00097 spin ()
00098 {
00099     using namespace ros;
00100     Rate r ( 10 ); // TODO: configuravel
00101     while ( ok() )
00102     {
00103         predicates_cb_queue_.callAvailable();
00104         actions_cb_queue_.callAvailable();
00105         r.sleep();
00106     }
00107 }


topological_tools
Author(s): Joao Messias
autogenerated on Wed Aug 26 2015 12:28:47