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();
Asked by thanasis_t on 2019-09-09 06:19:49 UTC
Answers
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?
Asked by PeteBlackerThe3rd on 2019-09-10 06:45:58 UTC
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.
Asked by thanasis_t on 2019-09-11 01:32:32 UTC
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.
Asked by PeteBlackerThe3rd on 2019-09-11 02:59:29 UTC
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.
Asked by thanasis_t on 2019-09-12 04:39:02 UTC
Can you try this without using the AsyncSpinner to test if it works in the callback in the same thread?
Asked by PeteBlackerThe3rd on 2019-09-12 05:16:32 UTC
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.
Asked by thanasis_t on 2019-09-18 02:07:45 UTC
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.
Asked by A_YIng on 2019-10-02 05:03:03 UTC
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.
Asked by thanasis_t on 2019-10-07 01:43:04 UTC
Comments