Custom movegroup capability and its usage

asked 2022-01-14 11:56:59 -0500

zahid990170 gravatar image


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());


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.



edit retag flag offensive close merge delete