Issues planning to a pose goal as a callback

[Pedroacsilva]

Description

I'm trying to implement a std_msgs::String callback that would read a JSON message containing the target pose coordinates. I am starting from the following tutorial.

My first attempt was the following:

void TaskReqCallback(const std_msgs::String::ConstPtr &msg){
    moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const moveit::core::JointModelGroup* joint_model_group = move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
    geometry_msgs::Pose target_pose1;
// For simplicity's sake, define a hard coded pose
    target_pose1.orientation.w = 1.0;
    target_pose1.position.x = 0.28;
    target_pose1.position.y = -0.2;
    target_pose1.position.z = 0.5;
    move_group_interface.setPoseTarget(target_pose1);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

With the main function being:

int main(int argc, char **argv)
{
    ros::init(argc, argv, "NodeNamet");
    ros::NodeHandle n;
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::Subscriber mqtt_task_req = n.subscribe("MessageTopic", 1, TaskReqCallback);
    ros::Rate loop_rate(0.3);
    while (ros::ok())
    {
        loop_rate.sleep();
    }
    return 0;
}

When I publish a message, triggering this callback, I get the following console output and a segmentation fault:

[ INFO] [1678720867.821292451]: Loading robot model 'panda'...
[ INFO] [1678720868.959599725]: Ready to take commands for planning group panda_arm.
[ INFO] [1678720869.969216552]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!

Your environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04

Expected behaviour

I would expect to see a movement plan on RVIZ after publishing one message in this topic.

Basically, my question is, what would be the best way to implement MoveIt in ROS Callback functions?

EDIT: Ok I think I fixed it. I needed to construct an AsyncSpinner with 2 threads instead of one: one thread for the callback and another one for MoveIt, I guess?

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[rhaschke]

MoveIt's MoveGroupInterface (MGI) and PlanningSceneInterface (PSI) both themselves make heavy use of ROS message communication. As you are calling them from the one and only spinner thread handling ROS messages, these messages cannot be served. Create a class instantiating (and keeping) MGI and PSI for its whole life time and make your callback a method of that class. That should solve your issue.