Custom movegroup capability and its usage

edit

Hi,

My question concerns the implementation and using of MoveGroupCapability.
I am adapting the code from the following packages vigir_move_group and vigir_plan_execution. In particular, vigir_plan_execution implements a continuous re-planning functionality, that I want to check. And, vigir_move_group implements the move_group capability by implementing the move_group_capability.h.

class ReplanningCapability : public MoveGroupCapability

I have followed certain examples / tutorials on the network, and have been able to successfully compile the code. To be able to load the capability by move_group node, I have added the following

<arg name="capabilities" value="move_group/ReplanningCapability" />

to move_group.launch file of my package.

The class that implements MoveGroupCapability , subscribes to two topics in its implementation of the intialized function as follows.

void move_group::ReplanningCapability::initialize()
{
  plan_execution_.reset(new plan_execution::ContinuousPlanExecution(context_));

  trigger_sub_ = root_node_handle_.subscribe("/trigger_cont", 1, &ReplanningCapability::triggerCb, this);
  abort_sub_ = root_node_handle_.subscribe("/abort_cont", 1, &ReplanningCapability::abortCb, this);

}

Basically, publishing on /trigger_cont should start the continuous re-planning thread.

void move_group::ReplanningCapability::triggerCb(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());

  plan_execution_->startExecution();
}

However, while I do publish on /trigger_cont , the callback in class ReplanningCapability never gets called, and thus the continuous re-planning thread is never executed.

Am I doing the things in the correct manner, or is there another way to use the custom capability.

thanks,

Zahid