topological_move_base_action_layer.cpp

Go to the documentation of this file.

#include <topological_tools/topological_move_base_action_layer.h>

#include <geometry_msgs/PoseStamped.h>

using namespace std;
using namespace topological_tools;
using namespace predicate_manager;
using namespace mdm_library;

TopologicalMoveBaseActionLayer::
TopologicalMoveBaseActionLayer ( TopologicalMap& tm ) :
    actions_cb_queue_(),
    predicates_cb_queue_(),
    al_ ( &actions_cb_queue_ ),
    tam_ ( tm, &predicates_cb_queue_ ),
    move_base_client_ ( "move_base", true ) 
{
    ros::Duration d ( 10.0 );
    d.sleep(); 
    while ( ! ( move_base_client_.waitForServer ( d ) ) )
    {
        ROS_WARN_STREAM ( "TopologicalMoveBaseActionLayer:: Waiting for the move_base server to come up." );
    };
}

TopologicalMoveBaseActionLayer::
TopologicalMoveBaseActionLayer ( const std::string& map_file ) :
    actions_cb_queue_(),
    predicates_cb_queue_(),
    al_ ( &actions_cb_queue_ ),
    tam_ ( map_file, &predicates_cb_queue_ ),
    move_base_client_ ( "move_base", true ) 
{
    ros::Duration d ( 10.0 );
    d.sleep(); 
    while ( ! ( move_base_client_.waitForServer ( d ) ) )
    {
        ROS_WARN_STREAM ( "TopologicalMoveBaseActionLayer:: Waiting for the move_base server to come up." );
    };
}

void
TopologicalMoveBaseActionLayer::
addAction ( const std::string& action_name )
{
    al_.addAction ( boost::bind ( &TopologicalMoveBaseActionLayer::moveToLabel, this, action_name ),
                    action_name );
}

void
TopologicalMoveBaseActionLayer::
moveToLabel ( const string& connection_label )
{
    geometry_msgs::PoseStamped goal_pose;
    goal_pose.pose =  tam_.getGoalPoseForLabel ( connection_label );
    goal_pose.header.stamp = ros::Time::now();
    goal_pose.header.frame_id = "/map";

    move_base_msgs::MoveBaseGoal traverse_goal;
    traverse_goal.target_pose = goal_pose;

    move_base_client_.cancelAllGoals();
    move_base_client_.sendGoal ( traverse_goal,
                                 boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseDoneCB, this, _1, _2 ),
                                 boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseActiveCB, this ),
                                 boost::bind ( &TopologicalMoveBaseActionLayer::moveBaseFeedbackCB, this, _1 ) );
}

void
TopologicalMoveBaseActionLayer::
spin ()
{
    using namespace ros;
    Rate r ( 10 ); // TODO: configuravel
    while ( ok() )
    {
        predicates_cb_queue_.callAvailable();
        actions_cb_queue_.callAvailable();
        r.sleep();
    }
}