Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

move_group interface using a callback function

Hello

by using the move_group interface provided by MoveIt! I am moving the end effector of a robotic manipulator in a specific position in the 3D space. The code I am using to achieve this is shown below and everything works fine.

ros::init(argc, argv, "manos_openpose");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group("arm");

geometry_msgs::Pose target_pose;
target_pose.orientation.w = 0.646470940769;
target_pose.position.x = -0.285039418823;
target_pose.position.y = -0.128284967952;
target_pose.position.z = 1.10245513838;
move_group.setPoseTarget(target_pose);

moveit::planning_interface::MoveGroupInterface::Plan  my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

if (success){
    move_group.move();
}
else{
    ROS_INFO("Failed to plan for this configuration");
}

ros::waitForShutdown();

As a next step I would like to create a node which publishes the target point into a topic. Onto this topic the main node subscribes and whenever a target point is published, the respective callback get activated which should move the end effector. The problem is that in the case of the callback the planner fails to compute the IK and hence the end effector does not get to the desired point. The second block of code is also shown below. Any ideas? Thanks in advance

void coordsCallback(const geometry_msgs::Pose target_pose){

moveit::planning_interface::MoveGroupInterface move_group("arm");

move_group.setPoseTarget(target_pose);
std::cout << target_pose << std::endl;

moveit::planning_interface::MoveGroupInterface::Plan  my_plan;

bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

if (success){
    move_group.move();
}
else{
    ROS_INFO("Failed to plan for this configuration");
}
}

int main(int argc, char** argv){

ros::init(argc, argv, "manos_openpose");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface move_group("arm");


ros::Subscriber sub = nh.subscribe("coords_topic", 1000, coordsCallback);
ros::waitForShutdown();