Ask Your Question
0

move_group interface using a callback function

asked 2019-09-09 06:19:49 -0500

thanasis_t gravatar image

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();
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-10 06:45:58 -0500

There is an error where you are defining your target pose:

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;

The target pose needs a fully defined position and orientation in 3D space. This includes a 3 dimensional position and a 4 dimensional quaternion. You're correctly setting the position but only setting 1 of the 4 quaternion elements, this will probably result in an invalid quaternion which in turn means no IK solution can be found. You'll need to fully define the orientation of your end effector to be able to plan a movement.

Do you see any errors messages that MoveIt produces on the command line?

edit flag offensive delete link more

Comments

I tried what you are suggesting but it still fails. Besides if that was the case, it wouldn't work in the first scenario either.

thanasis_t gravatar imagethanasis_t ( 2019-09-11 01:32:32 -0500 )edit

Can you add some code to print the actual error code to the terminal. There are about 20 different reasons it can give for failing, but at the moment you're only checking for success. The meaning of the codes can be found here.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-09-11 02:59:29 -0500 )edit

Sorry for my late response. By printing the output of the move_group.plan() command I found out that when i call this function in the callback, it blocks the code and doesn't return anything. However, when I am printing the same value in the first case (where no callback is employed), the returned value is 1 (corresponds to SUCCESS) and everything runs smoothly. Therefore, the problem seems to be with the fact that the functions are called inside a callback. I do not know the reason for this behavior but maybe it is a problem of threading since the callback is executed in a different thread than the rest of the code. Any ideas? Thanks again.

thanasis_t gravatar imagethanasis_t ( 2019-09-12 04:39:02 -0500 )edit

Can you try this without using the AsyncSpinner to test if it works in the callback in the same thread?

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-09-12 05:16:32 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-09-09 06:19:49 -0500

Seen: 13 times

Last updated: Sep 10