move_group interface using a callback function

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

thanasis_t
45 ●9 ●14 ●18

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

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

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

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

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 ( 2019-09-11 01:32:32 -0600 )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 ( 2019-09-11 02:59:29 -0600 )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 ( 2019-09-12 04:39:02 -0600 )edit

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

PeteBlackerThe3rd ( 2019-09-12 05:16:32 -0600 )edit

Sorry for my late response. I have worked out the problem. I should have passed "2" as the argument in the AsyncSpinner. Since more than one threads are created during the execution, the spinner should be able to spin for each thread. Furthermore, i created the callback function through a class.

thanasis_t ( 2019-09-18 02:07:45 -0600 )edit

Hello, could you provide the example code of this situation(using moveit in callback)? I want to do the similar thing of it, but I don't know how to do.

A_YIng ( 2019-10-02 05:03:03 -0600 )edit

An example of using moveit! inside a callback is the following one:

class MoveEnable{
public:
    void coordsCallback(const manos_openpose::Pose waypoints);
};

 void MoveEnable::coordsCallback(const manos_openpose::Pose waypoints){

// insert code here


}

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

ros::init(argc, argv, "manos_openpose");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(2);
spinner.start();

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

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

ros::waitForShutdown();
return 0;
 }

Of course, you need to include the respective libraries. Hope this helps.

thanasis_t ( 2019-10-07 01:43:04 -0600 )edit

add a comment

move_group interface using a callback function

1 Answer

Comments

Question Tools

Stats

Related questions

move_group interface using a callback function edit

1 Answer

Comments

Question Tools

Stats

Related questions

move_group interface using a callback function