robowflex::movegroup::MoveGroupHelper Class Reference

A helper class that allows for pulling and pushing of scenes, robots, and trajectories to move group. More...

#include <services.h>

Classes
struct	Action
	A container struct for all relevant information about a motion planning request to move group. More...

Public Types
typedef std::function< void(Action &)>	ResultCallback

Public Member Functions
	MoveGroupHelper (const std::string &move_group=MOVE_GROUP)
	Constructor. Sets up service clients. More...
	~MoveGroupHelper ()
	Destructor. Cleans up services. More...
void	setResultCallback (const ResultCallback &callback)
	Sets a callback function that is called whenever a motion plan request is serviced by move group. More...
bool	executeTrajectory (const robot_trajectory::RobotTrajectory &path)
	Executes a planned trajectory through move group. More...
bool	pullState (RobotPtr robot)
	Pulls the current robot state from move group. More...
bool	pullScene (ScenePtr scene)
	Pulls the current planning scene from move group. More...
bool	pushScene (const SceneConstPtr &scene)
	Pushes the current planning scene to move group. More...
bool	clearOctomap ()
	Clears the octomap in the planning scene of the movegroup. More...

Private Member Functions
void	moveGroupGoalCallback (const moveit_msgs::MoveGroupActionGoal &msg)
	Callback function for a move group goal. More...
void	moveGroupResultCallback (const moveit_msgs::MoveGroupActionResult &msg)
	Callback function for a move group result. More...

Private Attributes
ros::NodeHandle	nh_
	Node handle. More...
ros::Subscriber	goal_sub_
	Move group goal subscriber. More...
ros::Subscriber	result_sub_
	Move group result subscriber. More...
ros::ServiceClient	gpsc_
	Get planning scene service. More...
ros::ServiceClient	apsc_
	Apply planning scene service. More...
ros::ServiceClient	co_
	Clear octomap service. More...
actionlib::SimpleActionClient< moveit_msgs::ExecuteTrajectoryAction >	eac_
ResultCallback	callback_
	Callback function for move group results. More...
std::map< std::string, Action >	requests_
	Move group requests. More...
RobotPtr	robot_
	Robot on the parameter server used by move group. More...

Static Private Attributes
static const std::string	MOVE_GROUP {"/move_group"}
	Name of move_group namespace. More...
static const std::string	GET_SCENE {"get_planning_scene"}
	Name of get scene service. More...
static const std::string	APPLY_SCENE {"apply_planning_scene"}
	Name of apply scene service. More...
static const std::string	CLEAR_OCTOMAP {"clear_octomap"}
	Name of clear octomap service. More...
static const std::string	EXECUTE {"/execute_trajectory"}
	Name of execute trajectory service. More...

Detailed Description

A helper class that allows for pulling and pushing of scenes, robots, and trajectories to move group.

Definition at line 26 of file services.h.

Member Typedef Documentation

ResultCallback

typedef std::function<void(Action &)> robowflex::movegroup::MoveGroupHelper::ResultCallback

Definition at line 54 of file services.h.

Constructor & Destructor Documentation

MoveGroupHelper()

MoveGroupHelper::MoveGroupHelper

(

const std::string &

move_group = MOVE_GROUP

)

Constructor. Sets up service clients.

Parameters

[in]

move_group

Name of the move group namespace.

Definition at line 74 of file services.cpp.

  : nh_("/")
  , goal_sub_(nh_.subscribe(move_group + "/goal", 10, &MoveGroupHelper::moveGroupGoalCallback, this))
  , result_sub_(nh_.subscribe(move_group + "/result", 10, &MoveGroupHelper::moveGroupResultCallback, this))
  , gpsc_(nh_.serviceClient<moveit_msgs::GetPlanningScene>(GET_SCENE, true))
  , apsc_(nh_.serviceClient<moveit_msgs::ApplyPlanningScene>(APPLY_SCENE, true))
  , co_(nh_.serviceClient<std_srvs::Empty>(CLEAR_OCTOMAP, true))
  , eac_(nh_, EXECUTE, true)
  , robot_(std::make_shared<ParamRobot>())
{
    RBX_INFO("Waiting for %s to connect...", EXECUTE);
    eac_.waitForServer();
    RBX_INFO("%s connected!", EXECUTE);
    RBX_INFO("Waiting for %s to connect...", GET_SCENE);
    gpsc_.waitForExistence();
    RBX_INFO("%s connected!", GET_SCENE);
    RBX_INFO("Waiting for %s to connect...", APPLY_SCENE);
    apsc_.waitForExistence();
    RBX_INFO("%s connected!", APPLY_SCENE);
    RBX_INFO("Waiting for %s to connect...", CLEAR_OCTOMAP);
    co_.waitForExistence();
    RBX_INFO("%s connected!", CLEAR_OCTOMAP);
}

~MoveGroupHelper()

MoveGroupHelper::~MoveGroupHelper

(

)

Destructor. Cleans up services.

Definition at line 98 of file services.cpp.

{
    goal_sub_.shutdown();
    result_sub_.shutdown();
    gpsc_.shutdown();
    apsc_.shutdown();
    co_.shutdown();
}

Member Function Documentation

clearOctomap()

bool MoveGroupHelper::clearOctomap

(

)

Clears the octomap in the planning scene of the movegroup.

Returns

True on success, false on failure.

Definition at line 191 of file services.cpp.

{
    std_srvs::EmptyRequest request;
    std_srvs::EmptyResponse response;
 
    return co_.call(request, response);
}

executeTrajectory()

bool MoveGroupHelper::executeTrajectory

(

const robot_trajectory::RobotTrajectory &

path

)

Executes a planned trajectory through move group.

Parameters

[in]

path

Path to execute.

Returns

True on success, false on failure.

Definition at line 112 of file services.cpp.

{
    if (!eac_.isServerConnected())
        return false;
 
    moveit_msgs::ExecuteTrajectoryGoal goal;
 
    // Check if a Trajectory is time parameterized, with some goofiness for indigo.
#if ROBOWFLEX_AT_LEAST_KINETIC
    double value = path.getWayPointDurationFromStart(path.getWayPointCount());
#else
    double value = path.getWaypointDurationFromStart(path.getWayPointCount());
#endif
 
    if (value == 0)
    {
        RBX_ERROR("Trajectory is not parameterized and cannot be executed!  did you use "
                  "Trajectory::computeTimeParameterization?");
        return false;
    }
 
    path.getRobotTrajectoryMsg(goal.trajectory);
 
    eac_.sendGoal(goal);
    if (!eac_.waitForResult())
        RBX_INFO("ExecuteTrajectory action returned early");
 
    return eac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
}

moveGroupGoalCallback()

void MoveGroupHelper::moveGroupGoalCallback ( const moveit_msgs::MoveGroupActionGoal &  msg )

private

Callback function for a move group goal.

Parameters

[in]

msg

Goal message.

Definition at line 199 of file services.cpp.

{
    const std::string &id = msg.goal_id.id;
    RBX_DEBUG("Intercepted request goal ID: `%s`", id);
 
    Action action;
    action.scene.reset(new Scene(robot_));
    pullScene(action.scene);
    action.scene->useMessage(msg.goal.planning_options.planning_scene_diff);
    action.request = msg.goal.request;
 
    requests_.emplace(id, action);
}

moveGroupResultCallback()

void MoveGroupHelper::moveGroupResultCallback ( const moveit_msgs::MoveGroupActionResult &  msg )

private

Callback function for a move group result.

Parameters

[in]

msg

Result message.

Definition at line 213 of file services.cpp.

{
    const std::string &id = msg.status.goal_id.id;
 
    auto request = requests_.find(id);
    if (request == requests_.end())
    {
        RBX_WARN("Intercepted unknown request response ID: `%s`", id);
        return;
    }
 
    RBX_DEBUG("Intercepted request response ID: `%s`", id);
 
    Action &action = request->second;
    action.id = id;
    action.success = msg.result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
    action.time = msg.result.planning_time;
    action.trajectory = msg.result.planned_trajectory;
 
    if (callback_)
        callback_(action);
 
    requests_.erase(request);
}

pullScene()

bool MoveGroupHelper::pullScene

(

ScenePtr

scene

)

Pulls the current planning scene from move group.

Parameters

[out]

scene

Scene to set to the current scene observed by move group.

Returns

True on success, false on failure.

Definition at line 158 of file services.cpp.

{
    moveit_msgs::GetPlanningScene::Request request;
    moveit_msgs::GetPlanningScene::Response response;
    request.components.components = moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS |
                                    moveit_msgs::PlanningSceneComponents::ROBOT_STATE |
                                    moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS |
                                    moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES |
                                    moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY |
                                    moveit_msgs::PlanningSceneComponents::OCTOMAP |
                                    moveit_msgs::PlanningSceneComponents::TRANSFORMS |
                                    moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX |
                                    moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING |
                                    moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
 
    if (gpsc_.call(request, response))
    {
        scene->useMessage(response.scene);
        return true;
    }
 
    return false;
}

pullState()

bool MoveGroupHelper::pullState

(

RobotPtr

robot

)

Pulls the current robot state from move group.

Parameters

[out]

robot

Robot whose state to set.

Returns

True on success, false on failure.

Definition at line 142 of file services.cpp.

{
    moveit_msgs::GetPlanningScene::Request request;
    moveit_msgs::GetPlanningScene::Response response;
    request.components.components = moveit_msgs::PlanningSceneComponents::ROBOT_STATE |
                                    moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS;
 
    if (gpsc_.call(request, response))
    {
        robot->setState(response.scene.robot_state);
        return true;
    }
 
    return false;
}

pushScene()

bool MoveGroupHelper::pushScene

(

const SceneConstPtr &

scene

)

Pushes the current planning scene to move group.

Parameters

[in]

scene

Scene to use to set move group's current scene.

Returns

True on success, false on failure.

Definition at line 182 of file services.cpp.

{
    moveit_msgs::ApplyPlanningScene::Request request;
    moveit_msgs::ApplyPlanningScene::Response response;
    request.scene = scene->getMessage();
 
    return apsc_.call(request, response);
}

setResultCallback()

void MoveGroupHelper::setResultCallback

(

const ResultCallback &

callback

)

Sets a callback function that is called whenever a motion plan request is serviced by move group.

Parameters

[in]

callback

Callback function to call upon move group requests.

Definition at line 107 of file services.cpp.

{
    callback_ = callback;
}

Member Data Documentation

APPLY_SCENE

const std::string MoveGroupHelper::APPLY_SCENE {"apply_planning_scene"}

staticprivate

Name of apply scene service.

Definition at line 129 of file services.h.

apsc_

ros::ServiceClient robowflex::movegroup::MoveGroupHelper::apsc_

private

Apply planning scene service.

Definition at line 115 of file services.h.

callback_

ResultCallback robowflex::movegroup::MoveGroupHelper::callback_

private

Callback function for move group results.

Definition at line 121 of file services.h.

CLEAR_OCTOMAP

const std::string MoveGroupHelper::CLEAR_OCTOMAP {"clear_octomap"}

staticprivate

Name of clear octomap service.

Definition at line 130 of file services.h.

co_

ros::ServiceClient robowflex::movegroup::MoveGroupHelper::co_

private

Clear octomap service.

Definition at line 116 of file services.h.

eac_

actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction> robowflex::movegroup::MoveGroupHelper::eac_

private

Execute trajectory client.

Definition at line 118 of file services.h.

EXECUTE

const std::string MoveGroupHelper::EXECUTE {"/execute_trajectory"}

staticprivate

Name of execute trajectory service.

Definition at line 131 of file services.h.

GET_SCENE

const std::string MoveGroupHelper::GET_SCENE {"get_planning_scene"}

staticprivate

Name of get scene service.

Definition at line 128 of file services.h.

goal_sub_

ros::Subscriber robowflex::movegroup::MoveGroupHelper::goal_sub_

private

Move group goal subscriber.

Definition at line 112 of file services.h.

gpsc_

ros::ServiceClient robowflex::movegroup::MoveGroupHelper::gpsc_

private

Get planning scene service.

Definition at line 114 of file services.h.

MOVE_GROUP

const std::string MoveGroupHelper::MOVE_GROUP {"/move_group"}

staticprivate

Name of move_group namespace.

MoveGroupHelper

Definition at line 127 of file services.h.

nh_

ros::NodeHandle robowflex::movegroup::MoveGroupHelper::nh_

private

Node handle.

Definition at line 111 of file services.h.

requests_

std::map<std::string, Action> robowflex::movegroup::MoveGroupHelper::requests_

private

Move group requests.

Definition at line 123 of file services.h.

result_sub_

ros::Subscriber robowflex::movegroup::MoveGroupHelper::result_sub_

private

Move group result subscriber.

Definition at line 113 of file services.h.

robot_

RobotPtr robowflex::movegroup::MoveGroupHelper::robot_

private

Robot on the parameter server used by move group.

Definition at line 125 of file services.h.

---

The documentation for this class was generated from the following files:

• services.h
• services.cpp